Cereal cleanup (#20003)

* start cleanup

* fan speed

* cleanup dm

* fix cereal

* hwType -> pandaType

* update refs

* update refs

* bump cereal

* freeSpacePercent

* cereal master
old-commit-hash: 000bd226aa
commatwo_master
Adeeb Shihadeh 4 years ago committed by GitHub
parent f4e94524d4
commit 966945880b
  1. 2
      cereal
  2. 5
      release/files_common
  3. 8
      selfdrive/boardd/boardd.cc
  4. 16
      selfdrive/boardd/panda.cc
  5. 4
      selfdrive/boardd/panda.h
  6. 2
      selfdrive/car/honda/interface.py
  7. 1
      selfdrive/car/interfaces.py
  8. 91
      selfdrive/controls/controlsd.py
  9. 13
      selfdrive/controls/lib/events.py
  10. 4
      selfdrive/controls/lib/lane_planner.py
  11. 2
      selfdrive/controls/lib/latcontrol_pid.py
  12. 69
      selfdrive/controls/lib/lateral_planner.py
  13. 48
      selfdrive/controls/lib/longitudinal_planner.py
  14. 8
      selfdrive/controls/plannerd.py
  15. 10
      selfdrive/controls/radard.py
  16. 2
      selfdrive/controls/tests/test_following_distance.py
  17. 2
      selfdrive/controls/tests/test_startup.py
  18. 2
      selfdrive/debug/cycle_alerts.py
  19. 2
      selfdrive/debug/internal/check_alive_valid.py
  20. 4
      selfdrive/debug/mpc/live_lateral_mpc.py
  21. 2
      selfdrive/debug/test_fw_query_on_routes.py
  22. 2
      selfdrive/manager.py
  23. 4
      selfdrive/modeld/modeld.cc
  24. 4
      selfdrive/modeld/models/dmonitoring.cc
  25. 2
      selfdrive/modeld/models/driving.cc
  26. 14
      selfdrive/monitoring/dmonitoringd.py
  27. 4
      selfdrive/monitoring/driver_monitor.py
  28. 1
      selfdrive/monitoring/test_monitoring.py
  29. 1
      selfdrive/test/longitudinal_maneuvers/maneuver.py
  30. 4
      selfdrive/test/longitudinal_maneuvers/maneuverplots.py
  31. 15
      selfdrive/test/longitudinal_maneuvers/plant.py
  32. 8
      selfdrive/test/longitudinal_maneuvers/test_longitudinal.py
  33. 4
      selfdrive/test/model_replay.py
  34. 10
      selfdrive/test/process_replay/camera_replay.py
  35. 12
      selfdrive/test/process_replay/process_replay.py
  36. 2
      selfdrive/test/process_replay/ref_commit
  37. 4
      selfdrive/test/test_car_models.py
  38. 2
      selfdrive/test/test_models.py
  39. 4
      selfdrive/thermald/power_monitoring.py
  40. 14
      selfdrive/thermald/tests/test_power_monitoring.py
  41. 26
      selfdrive/thermald/thermald.py
  42. 2
      selfdrive/ui/android/ui.cc
  43. 9
      selfdrive/ui/paint.cc
  44. 2
      selfdrive/ui/sidebar.cc
  45. 19
      selfdrive/ui/ui.cc
  46. 5
      selfdrive/ui/ui.hpp
  47. 2
      tools/carcontrols/debug_controls.py
  48. 3
      tools/nui/Unlogger.cpp
  49. 6
      tools/nui/main.cpp
  50. 4
      tools/replay/ui.py
  51. 10
      tools/sim/bridge.py

@ -1 +1 @@
Subproject commit 266fc0195003a50faa95d2905839abf8409b6bcc Subproject commit b19a3ed38de1b712f744e4fdeed0b1f87801bf15

@ -225,10 +225,10 @@ selfdrive/controls/lib/latcontrol_pid.py
selfdrive/controls/lib/latcontrol_indi.py selfdrive/controls/lib/latcontrol_indi.py
selfdrive/controls/lib/latcontrol_lqr.py selfdrive/controls/lib/latcontrol_lqr.py
selfdrive/controls/lib/longcontrol.py selfdrive/controls/lib/longcontrol.py
selfdrive/controls/lib/pathplanner.py selfdrive/controls/lib/lateral_planner.py
selfdrive/controls/lib/lane_planner.py selfdrive/controls/lib/lane_planner.py
selfdrive/controls/lib/pid.py selfdrive/controls/lib/pid.py
selfdrive/controls/lib/planner.py selfdrive/controls/lib/longitudinal_planner.py
selfdrive/controls/lib/radar_helpers.py selfdrive/controls/lib/radar_helpers.py
selfdrive/controls/lib/vehicle_model.py selfdrive/controls/lib/vehicle_model.py
selfdrive/controls/lib/speed_smoother.py selfdrive/controls/lib/speed_smoother.py
@ -475,6 +475,7 @@ rednose/**
cereal/.gitignore cereal/.gitignore
cereal/__init__.py cereal/__init__.py
cereal/car.capnp cereal/car.capnp
cereal/legacy.capnp
cereal/log.capnp cereal/log.capnp
cereal/services.py cereal/services.py
cereal/service_list.yaml cereal/service_list.yaml

@ -273,7 +273,7 @@ void can_health_thread(bool spoofing_started) {
MessageBuilder msg; MessageBuilder msg;
auto healthData = msg.initEvent().initHealth(); auto healthData = msg.initEvent().initHealth();
healthData.setHwType(cereal::HealthData::HwType::UNKNOWN); healthData.setPandaType(cereal::HealthData::PandaType::UNKNOWN);
pm.send("health", msg); pm.send("health", msg);
util::sleep_for(500); util::sleep_for(500);
} }
@ -360,7 +360,7 @@ void can_health_thread(bool spoofing_started) {
healthData.setCanSendErrs(health.can_send_errs); healthData.setCanSendErrs(health.can_send_errs);
healthData.setCanFwdErrs(health.can_fwd_errs); healthData.setCanFwdErrs(health.can_fwd_errs);
healthData.setGmlanSendErrs(health.gmlan_send_errs); healthData.setGmlanSendErrs(health.gmlan_send_errs);
healthData.setHwType(panda->hw_type); healthData.setPandaType(panda->hw_type);
healthData.setUsbPowerMode(cereal::HealthData::UsbPowerMode(health.usb_power_mode)); healthData.setUsbPowerMode(cereal::HealthData::UsbPowerMode(health.usb_power_mode));
healthData.setSafetyModel(cereal::CarParams::SafetyModel(health.safety_model)); healthData.setSafetyModel(cereal::CarParams::SafetyModel(health.safety_model));
healthData.setFanSpeedRpm(fan_speed_rpm); healthData.setFanSpeedRpm(fan_speed_rpm);
@ -420,10 +420,10 @@ void hardware_control_thread() {
#endif #endif
// Other pandas don't have fan/IR to control // Other pandas don't have fan/IR to control
if (panda->hw_type != cereal::HealthData::HwType::UNO && panda->hw_type != cereal::HealthData::HwType::DOS) continue; if (panda->hw_type != cereal::HealthData::PandaType::UNO && panda->hw_type != cereal::HealthData::PandaType::DOS) continue;
if (sm.updated("thermal")){ if (sm.updated("thermal")){
// Fan speed // Fan speed
uint16_t fan_speed = sm["thermal"].getThermal().getFanSpeed(); uint16_t fan_speed = sm["thermal"].getThermal().getFanSpeedRpmDesired();
if (fan_speed != prev_fan_speed || cnt % 100 == 0){ if (fan_speed != prev_fan_speed || cnt % 100 == 0){
panda->set_fan_speed(fan_speed); panda->set_fan_speed(fan_speed);
prev_fan_speed = fan_speed; prev_fan_speed = fan_speed;

@ -53,12 +53,12 @@ Panda::Panda(){
hw_type = get_hw_type(); hw_type = get_hw_type();
is_pigeon = is_pigeon =
(hw_type == cereal::HealthData::HwType::GREY_PANDA) || (hw_type == cereal::HealthData::PandaType::GREY_PANDA) ||
(hw_type == cereal::HealthData::HwType::BLACK_PANDA) || (hw_type == cereal::HealthData::PandaType::BLACK_PANDA) ||
(hw_type == cereal::HealthData::HwType::UNO) || (hw_type == cereal::HealthData::PandaType::UNO) ||
(hw_type == cereal::HealthData::HwType::DOS); (hw_type == cereal::HealthData::PandaType::DOS);
has_rtc = (hw_type == cereal::HealthData::HwType::UNO) || has_rtc = (hw_type == cereal::HealthData::PandaType::UNO) ||
(hw_type == cereal::HealthData::HwType::DOS); (hw_type == cereal::HealthData::PandaType::DOS);
return; return;
@ -186,11 +186,11 @@ void Panda::set_unsafe_mode(uint16_t unsafe_mode) {
usb_write(0xdf, unsafe_mode, 0); usb_write(0xdf, unsafe_mode, 0);
} }
cereal::HealthData::HwType Panda::get_hw_type() { cereal::HealthData::PandaType Panda::get_hw_type() {
unsigned char hw_query[1] = {0}; unsigned char hw_query[1] = {0};
usb_read(0xc1, 0, 0, hw_query, 1); usb_read(0xc1, 0, 0, hw_query, 1);
return (cereal::HealthData::HwType)(hw_query[0]); return (cereal::HealthData::PandaType)(hw_query[0]);
} }
void Panda::set_rtc(struct tm sys_time){ void Panda::set_rtc(struct tm sys_time){

@ -53,7 +53,7 @@ class Panda {
~Panda(); ~Panda();
std::atomic<bool> connected = true; std::atomic<bool> connected = true;
cereal::HealthData::HwType hw_type = cereal::HealthData::HwType::UNKNOWN; cereal::HealthData::PandaType hw_type = cereal::HealthData::PandaType::UNKNOWN;
bool is_pigeon = false; bool is_pigeon = false;
bool has_rtc = 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); int usb_bulk_read(unsigned char endpoint, unsigned char* data, int length, unsigned int timeout=TIMEOUT);
// Panda functionality // Panda functionality
cereal::HealthData::HwType get_hw_type(); cereal::HealthData::PandaType get_hw_type();
void set_safety_model(cereal::CarParams::SafetyModel safety_model, int safety_param=0); void set_safety_model(cereal::CarParams::SafetyModel safety_model, int safety_param=0);
void set_unsafe_mode(uint16_t unsafe_mode); void set_unsafe_mode(uint16_t unsafe_mode);
void set_rtc(struct tm sys_time); void set_rtc(struct tm sys_time);

@ -8,7 +8,7 @@ from selfdrive.config import Conversions as CV
from selfdrive.controls.lib.events import ET from selfdrive.controls.lib.events import ET
from selfdrive.car.honda.values import CruiseButtons, CAR, HONDA_BOSCH from selfdrive.car.honda.values import CruiseButtons, CAR, HONDA_BOSCH
from selfdrive.car import STD_CARGO_KG, CivicParams, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint from selfdrive.car import STD_CARGO_KG, CivicParams, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint
from selfdrive.controls.lib.planner import _A_CRUISE_MAX_V_FOLLOWING from selfdrive.controls.lib.longitudinal_planner import _A_CRUISE_MAX_V_FOLLOWING
from selfdrive.car.interfaces import CarInterfaceBase from selfdrive.car.interfaces import CarInterfaceBase
A_ACC_MAX = max(_A_CRUISE_MAX_V_FOLLOWING) A_ACC_MAX = max(_A_CRUISE_MAX_V_FOLLOWING)

@ -53,7 +53,6 @@ class CarInterfaceBase():
def get_std_params(candidate, fingerprint): def get_std_params(candidate, fingerprint):
ret = car.CarParams.new_message() ret = car.CarParams.new_message()
ret.carFingerprint = candidate ret.carFingerprint = candidate
ret.isPandaBlack = True # TODO: deprecate this field
# standard ALC params # standard ALC params
ret.steerControlType = car.CarParams.SteerControlType.torque ret.steerControlType = car.CarParams.SteerControlType.torque

@ -19,7 +19,7 @@ from selfdrive.controls.lib.latcontrol_lqr import LatControlLQR
from selfdrive.controls.lib.events import Events, ET from selfdrive.controls.lib.events import Events, ET
from selfdrive.controls.lib.alertmanager import AlertManager from selfdrive.controls.lib.alertmanager import AlertManager
from selfdrive.controls.lib.vehicle_model import VehicleModel from selfdrive.controls.lib.vehicle_model import VehicleModel
from selfdrive.controls.lib.planner import LON_MPC_STEP from selfdrive.controls.lib.longitudinal_planner import LON_MPC_STEP
from selfdrive.locationd.calibrationd import Calibration from selfdrive.locationd.calibrationd import Calibration
from selfdrive.hardware import HARDWARE, TICI from selfdrive.hardware import HARDWARE, TICI
@ -34,11 +34,11 @@ IGNORE_PROCESSES = set(["rtshield", "uploader", "deleter", "loggerd", "logmessag
ThermalStatus = log.ThermalData.ThermalStatus ThermalStatus = log.ThermalData.ThermalStatus
State = log.ControlsState.OpenpilotState State = log.ControlsState.OpenpilotState
HwType = log.HealthData.HwType PandaType = log.HealthData.PandaType
LongitudinalPlanSource = log.Plan.LongitudinalPlanSource LongitudinalPlanSource = log.LongitudinalPlan.LongitudinalPlanSource
Desire = log.PathPlan.Desire Desire = log.LateralPlan.Desire
LaneChangeState = log.PathPlan.LaneChangeState LaneChangeState = log.LateralPlan.LaneChangeState
LaneChangeDirection = log.PathPlan.LaneChangeDirection LaneChangeDirection = log.LateralPlan.LaneChangeDirection
EventName = car.CarEvent.EventName EventName = car.CarEvent.EventName
@ -56,8 +56,8 @@ class Controls:
if self.sm is None: if self.sm is None:
ignore = ['ubloxRaw', 'frontFrame', 'managerState'] if SIMULATION else None ignore = ['ubloxRaw', 'frontFrame', 'managerState'] if SIMULATION else None
self.sm = messaging.SubMaster(['thermal', 'health', 'modelV2', 'liveCalibration', 'ubloxRaw', self.sm = messaging.SubMaster(['thermal', 'health', 'modelV2', 'liveCalibration', 'ubloxRaw',
'dMonitoringState', 'plan', 'pathPlan', 'liveLocationKalman', 'driverMonitoringState', 'longitudinalPlan', 'lateralPlan', 'liveLocationKalman',
'frame', 'frontFrame', 'managerState'], ignore_alive=ignore) 'frame', 'frontFrame', 'managerState', 'liveParameters', 'radarState'], ignore_alive=ignore)
self.can_sock = can_sock self.can_sock = can_sock
if can_sock is None: if can_sock is None:
@ -127,10 +127,10 @@ class Controls:
self.logged_comm_issue = False self.logged_comm_issue = False
self.sm['liveCalibration'].calStatus = Calibration.CALIBRATED self.sm['liveCalibration'].calStatus = Calibration.CALIBRATED
self.sm['thermal'].freeSpace = 1. self.sm['thermal'].freeSpacePercent = 1.
self.sm['dMonitoringState'].events = [] self.sm['driverMonitoringState'].events = []
self.sm['dMonitoringState'].awarenessStatus = 1. self.sm['driverMonitoringState'].awarenessStatus = 1.
self.sm['dMonitoringState'].faceDetected = False self.sm['driverMonitoringState'].faceDetected = False
self.startup_event = get_startup_event(car_recognized, controller_available) self.startup_event = get_startup_event(car_recognized, controller_available)
@ -150,7 +150,7 @@ class Controls:
self.events.clear() self.events.clear()
self.events.add_from_msg(CS.events) self.events.add_from_msg(CS.events)
self.events.add_from_msg(self.sm['dMonitoringState'].events) self.events.add_from_msg(self.sm['driverMonitoringState'].events)
# Handle startup event # Handle startup event
if self.startup_event is not None: if self.startup_event is not None:
@ -163,10 +163,10 @@ class Controls:
self.events.add(EventName.lowBattery) self.events.add(EventName.lowBattery)
if self.sm['thermal'].thermalStatus >= ThermalStatus.red: if self.sm['thermal'].thermalStatus >= ThermalStatus.red:
self.events.add(EventName.overheat) self.events.add(EventName.overheat)
if self.sm['thermal'].freeSpace < 0.07: if self.sm['thermal'].freeSpacePercent < 0.07:
# under 7% of space free no enable allowed # under 7% of space free no enable allowed
self.events.add(EventName.outOfSpace) self.events.add(EventName.outOfSpace)
if self.sm['thermal'].memUsedPercent > 90: if self.sm['thermal'].memoryUsagePercent > 90:
self.events.add(EventName.lowMemory) self.events.add(EventName.lowMemory)
# Check if all manager processes are running # Check if all manager processes are running
@ -175,8 +175,8 @@ class Controls:
self.events.add(EventName.processNotRunning) self.events.add(EventName.processNotRunning)
# Alert if fan isn't spinning for 5 seconds # Alert if fan isn't spinning for 5 seconds
if self.sm['health'].hwType in [HwType.uno, HwType.dos]: if self.sm['health'].pandaType in [PandaType.uno, PandaType.dos]:
if self.sm['health'].fanSpeedRpm == 0 and self.sm['thermal'].fanSpeed > 50: if self.sm['health'].fanSpeedRpm == 0 and self.sm['thermal'].fanSpeedRpmDesired > 50:
if (self.sm.frame - self.last_functional_fan_frame) * DT_CTRL > 5.0: if (self.sm.frame - self.last_functional_fan_frame) * DT_CTRL > 5.0:
self.events.add(EventName.fanMalfunction) self.events.add(EventName.fanMalfunction)
else: else:
@ -191,8 +191,8 @@ class Controls:
self.events.add(EventName.calibrationInvalid) self.events.add(EventName.calibrationInvalid)
# Handle lane change # Handle lane change
if self.sm['pathPlan'].laneChangeState == LaneChangeState.preLaneChange: if self.sm['lateralPlan'].laneChangeState == LaneChangeState.preLaneChange:
direction = self.sm['pathPlan'].laneChangeDirection direction = self.sm['lateralPlan'].laneChangeDirection
if (CS.leftBlindspot and direction == LaneChangeDirection.left) or \ if (CS.leftBlindspot and direction == LaneChangeDirection.left) or \
(CS.rightBlindspot and direction == LaneChangeDirection.right): (CS.rightBlindspot and direction == LaneChangeDirection.right):
self.events.add(EventName.laneChangeBlocked) self.events.add(EventName.laneChangeBlocked)
@ -201,7 +201,7 @@ class Controls:
self.events.add(EventName.preLaneChangeLeft) self.events.add(EventName.preLaneChangeLeft)
else: else:
self.events.add(EventName.preLaneChangeRight) self.events.add(EventName.preLaneChangeRight)
elif self.sm['pathPlan'].laneChangeState in [LaneChangeState.laneChangeStarting, elif self.sm['lateralPlan'].laneChangeState in [LaneChangeState.laneChangeStarting,
LaneChangeState.laneChangeFinishing]: LaneChangeState.laneChangeFinishing]:
self.events.add(EventName.laneChange) self.events.add(EventName.laneChange)
@ -211,9 +211,10 @@ class Controls:
self.mismatch_counter >= 200: self.mismatch_counter >= 200:
self.events.add(EventName.controlsMismatch) self.events.add(EventName.controlsMismatch)
if not self.sm.alive['plan'] and self.sm.alive['pathPlan']: if len(self.sm['radarState'].radarErrors):
# only plan not being received: radar not communicating self.events.add(EventName.radarFault)
self.events.add(EventName.radarCommIssue) elif not self.sm.valid['liveParameters']:
self.events.add(EventName.vehicleModelInvalid)
elif not self.sm.all_alive_and_valid(): elif not self.sm.all_alive_and_valid():
self.events.add(EventName.commIssue) self.events.add(EventName.commIssue)
if not self.logged_comm_issue: if not self.logged_comm_issue:
@ -222,24 +223,18 @@ class Controls:
else: else:
self.logged_comm_issue = False self.logged_comm_issue = False
if not self.sm['pathPlan'].mpcSolutionValid: if not self.sm['lateralPlan'].mpcSolutionValid:
self.events.add(EventName.plannerError) self.events.add(EventName.plannerError)
if not self.sm['liveLocationKalman'].sensorsOK and not NOSENSOR: if not self.sm['liveLocationKalman'].sensorsOK and not NOSENSOR:
if self.sm.frame > 5 / DT_CTRL: # Give locationd some time to receive all the inputs if self.sm.frame > 5 / DT_CTRL: # Give locationd some time to receive all the inputs
self.events.add(EventName.sensorDataInvalid) self.events.add(EventName.sensorDataInvalid)
if not self.sm['pathPlan'].paramsValid:
self.events.add(EventName.vehicleModelInvalid)
if not self.sm['liveLocationKalman'].posenetOK: if not self.sm['liveLocationKalman'].posenetOK:
self.events.add(EventName.posenetInvalid) self.events.add(EventName.posenetInvalid)
if not self.sm['liveLocationKalman'].deviceStable: if not self.sm['liveLocationKalman'].deviceStable:
self.events.add(EventName.deviceFalling) self.events.add(EventName.deviceFalling)
if not self.sm['plan'].radarValid:
self.events.add(EventName.radarFault)
if self.sm['plan'].radarCanError:
self.events.add(EventName.radarCanError)
if log.HealthData.FaultType.relayMalfunction in self.sm['health'].faults: if log.HealthData.FaultType.relayMalfunction in self.sm['health'].faults:
self.events.add(EventName.relayMalfunction) self.events.add(EventName.relayMalfunction)
if self.sm['plan'].fcw: if self.sm['longitudinalPlan'].fcw:
self.events.add(EventName.fcw) self.events.add(EventName.fcw)
# TODO: fix simulator # TODO: fix simulator
@ -256,7 +251,7 @@ class Controls:
self.events.add(EventName.modeldLagging) self.events.add(EventName.modeldLagging)
# Only allow engagement with brake pressed when stopped behind another stopped car # Only allow engagement with brake pressed when stopped behind another stopped car
if CS.brakePressed and self.sm['plan'].vTargetFuture >= STARTING_TARGET_SPEED \ if CS.brakePressed and self.sm['longitudinalPlan'].vTargetFuture >= STARTING_TARGET_SPEED \
and self.CP.openpilotLongitudinalControl and CS.vEgo < 0.3: and self.CP.openpilotLongitudinalControl and CS.vEgo < 0.3:
self.events.add(EventName.noTarget) self.events.add(EventName.noTarget)
@ -370,8 +365,8 @@ class Controls:
def state_control(self, CS): def state_control(self, CS):
"""Given the state, this function returns an actuators packet""" """Given the state, this function returns an actuators packet"""
plan = self.sm['plan'] plan = self.sm['longitudinalPlan']
path_plan = self.sm['pathPlan'] path_plan = self.sm['lateralPlan']
actuators = car.CarControl.Actuators.new_message() actuators = car.CarControl.Actuators.new_message()
@ -384,7 +379,7 @@ class Controls:
self.LaC.reset() self.LaC.reset()
self.LoC.reset(v_pid=CS.vEgo) self.LoC.reset(v_pid=CS.vEgo)
plan_age = DT_CTRL * (self.sm.frame - self.sm.rcv_frame['plan']) plan_age = DT_CTRL * (self.sm.frame - self.sm.rcv_frame['longitudinalPlan'])
# no greater than dt mpc + dt, to prevent too high extraps # no greater than dt mpc + dt, to prevent too high extraps
dt = min(plan_age, LON_MPC_STEP + DT_CTRL) + DT_CTRL dt = min(plan_age, LON_MPC_STEP + DT_CTRL) + DT_CTRL
@ -432,15 +427,15 @@ class Controls:
brake_discount = (1.0 - clip(actuators.brake * 3., 0.0, 1.0)) brake_discount = (1.0 - clip(actuators.brake * 3., 0.0, 1.0))
speed_override = max(0.0, (self.LoC.v_pid + CS.cruiseState.speedOffset) * brake_discount) speed_override = max(0.0, (self.LoC.v_pid + CS.cruiseState.speedOffset) * brake_discount)
CC.cruiseControl.speedOverride = float(speed_override if self.CP.enableCruise else 0.0) CC.cruiseControl.speedOverride = float(speed_override if self.CP.enableCruise else 0.0)
CC.cruiseControl.accelOverride = self.CI.calc_accel_override(CS.aEgo, self.sm['plan'].aTarget, CS.vEgo, self.sm['plan'].vTarget) CC.cruiseControl.accelOverride = self.CI.calc_accel_override(CS.aEgo, self.sm['longitudinalPlan'].aTarget, CS.vEgo, self.sm['longitudinalPlan'].vTarget)
CC.hudControl.setSpeed = float(self.v_cruise_kph * CV.KPH_TO_MS) CC.hudControl.setSpeed = float(self.v_cruise_kph * CV.KPH_TO_MS)
CC.hudControl.speedVisible = self.enabled CC.hudControl.speedVisible = self.enabled
CC.hudControl.lanesVisible = self.enabled CC.hudControl.lanesVisible = self.enabled
CC.hudControl.leadVisible = self.sm['plan'].hasLead CC.hudControl.leadVisible = self.sm['longitudinalPlan'].hasLead
right_lane_visible = self.sm['pathPlan'].rProb > 0.5 right_lane_visible = self.sm['lateralPlan'].rProb > 0.5
left_lane_visible = self.sm['pathPlan'].lProb > 0.5 left_lane_visible = self.sm['lateralPlan'].lProb > 0.5
CC.hudControl.rightLaneVisible = bool(right_lane_visible) CC.hudControl.rightLaneVisible = bool(right_lane_visible)
CC.hudControl.leftLaneVisible = bool(left_lane_visible) CC.hudControl.leftLaneVisible = bool(left_lane_visible)
@ -472,10 +467,10 @@ class Controls:
can_sends = self.CI.apply(CC) can_sends = self.CI.apply(CC)
self.pm.send('sendcan', can_list_to_can_capnp(can_sends, msgtype='sendcan', valid=CS.canValid)) self.pm.send('sendcan', can_list_to_can_capnp(can_sends, msgtype='sendcan', valid=CS.canValid))
force_decel = (self.sm['dMonitoringState'].awarenessStatus < 0.) or \ force_decel = (self.sm['driverMonitoringState'].awarenessStatus < 0.) or \
(self.state == State.softDisabling) (self.state == State.softDisabling)
steer_angle_rad = (CS.steeringAngle - self.sm['pathPlan'].angleOffset) * CV.DEG_TO_RAD steer_angle_rad = (CS.steeringAngle - self.sm['lateralPlan'].angleOffset) * CV.DEG_TO_RAD
# controlsState # controlsState
dat = messaging.new_message('controlsState') dat = messaging.new_message('controlsState')
@ -488,17 +483,12 @@ class Controls:
controlsState.alertBlinkingRate = self.AM.alert_rate controlsState.alertBlinkingRate = self.AM.alert_rate
controlsState.alertType = self.AM.alert_type controlsState.alertType = self.AM.alert_type
controlsState.alertSound = self.AM.audible_alert controlsState.alertSound = self.AM.audible_alert
controlsState.driverMonitoringOn = self.sm['dMonitoringState'].faceDetected
controlsState.canMonoTimes = list(CS.canMonoTimes) controlsState.canMonoTimes = list(CS.canMonoTimes)
controlsState.planMonoTime = self.sm.logMonoTime['plan'] controlsState.longitudinalPlanMonoTime = self.sm.logMonoTime['longitudinalPlan']
controlsState.pathPlanMonoTime = self.sm.logMonoTime['pathPlan'] controlsState.lateralPlanMonoTime = self.sm.logMonoTime['lateralPlan']
controlsState.enabled = self.enabled controlsState.enabled = self.enabled
controlsState.active = self.active controlsState.active = self.active
controlsState.vEgo = CS.vEgo
controlsState.vEgoRaw = CS.vEgoRaw
controlsState.angleSteers = CS.steeringAngle
controlsState.curvature = self.VM.calc_curvature(steer_angle_rad, CS.vEgo) controlsState.curvature = self.VM.calc_curvature(steer_angle_rad, CS.vEgo)
controlsState.steerOverride = CS.steeringPressed
controlsState.state = self.state controlsState.state = self.state
controlsState.engageable = not self.events.any(ET.NO_ENTRY) controlsState.engageable = not self.events.any(ET.NO_ENTRY)
controlsState.longControlState = self.LoC.long_control_state controlsState.longControlState = self.LoC.long_control_state
@ -510,13 +500,8 @@ class Controls:
controlsState.angleSteersDes = float(self.LaC.angle_steers_des) controlsState.angleSteersDes = float(self.LaC.angle_steers_des)
controlsState.vTargetLead = float(v_acc) controlsState.vTargetLead = float(v_acc)
controlsState.aTarget = float(a_acc) controlsState.aTarget = float(a_acc)
controlsState.jerkFactor = float(self.sm['plan'].jerkFactor)
controlsState.gpsPlannerActive = self.sm['plan'].gpsPlannerActive
controlsState.vCurvature = self.sm['plan'].vCurvature
controlsState.decelForModel = self.sm['plan'].longitudinalPlanSource == LongitudinalPlanSource.model
controlsState.cumLagMs = -self.rk.remaining * 1000. controlsState.cumLagMs = -self.rk.remaining * 1000.
controlsState.startMonoTime = int(start_time * 1e9) controlsState.startMonoTime = int(start_time * 1e9)
controlsState.mapValid = self.sm['plan'].mapValid
controlsState.forceDecel = bool(force_decel) controlsState.forceDecel = bool(force_decel)
controlsState.canErrorCounter = self.can_error_counter controlsState.canErrorCounter = self.can_error_counter

@ -195,7 +195,7 @@ def calibration_incomplete_alert(CP: car.CarParams, sm: messaging.SubMaster, met
Priority.LOWEST, VisualAlert.none, AudibleAlert.none, 0., 0., .2) Priority.LOWEST, VisualAlert.none, AudibleAlert.none, 0., 0., .2)
def no_gps_alert(CP: car.CarParams, sm: messaging.SubMaster, metric: bool) -> Alert: def no_gps_alert(CP: car.CarParams, sm: messaging.SubMaster, metric: bool) -> Alert:
gps_integrated = sm['health'].hwType in [log.HealthData.HwType.uno, log.HealthData.HwType.dos] gps_integrated = sm['health'].pandaType in [log.HealthData.PandaType.uno, log.HealthData.PandaType.dos]
return Alert( return Alert(
"Poor GPS reception", "Poor GPS reception",
"If sky is visible, contact support" if gps_integrated else "Check GPS antenna placement", "If sky is visible, contact support" if gps_integrated else "Check GPS antenna placement",
@ -620,17 +620,6 @@ EVENTS: Dict[int, Dict[str, Union[Alert, Callable[[Any, messaging.SubMaster, boo
audible_alert=AudibleAlert.chimeDisengage), audible_alert=AudibleAlert.chimeDisengage),
}, },
EventName.radarCommIssue: {
ET.SOFT_DISABLE: SoftDisableAlert("Radar Communication Issue"),
ET.NO_ENTRY: NoEntryAlert("Radar Communication Issue",
audible_alert=AudibleAlert.chimeDisengage),
},
EventName.radarCanError: {
ET.SOFT_DISABLE: SoftDisableAlert("Radar Error: Restart the Car"),
ET.NO_ENTRY: NoEntryAlert("Radar Error: Restart the Car"),
},
EventName.radarFault: { EventName.radarFault: {
ET.SOFT_DISABLE: SoftDisableAlert("Radar Error: Restart the Car"), ET.SOFT_DISABLE: SoftDisableAlert("Radar Error: Restart the Car"),
ET.NO_ENTRY : NoEntryAlert("Radar Error: Restart the Car"), ET.NO_ENTRY : NoEntryAlert("Radar Error: Restart the Car"),

@ -42,8 +42,8 @@ class LanePlanner:
self.rll_std = md.laneLineStds[2] self.rll_std = md.laneLineStds[2]
if len(md.meta.desireState): if len(md.meta.desireState):
self.l_lane_change_prob = md.meta.desireState[log.PathPlan.Desire.laneChangeLeft] self.l_lane_change_prob = md.meta.desireState[log.LateralPlan.Desire.laneChangeLeft]
self.r_lane_change_prob = md.meta.desireState[log.PathPlan.Desire.laneChangeRight] self.r_lane_change_prob = md.meta.desireState[log.LateralPlan.Desire.laneChangeRight]
def get_d_path(self, v_ego, path_t, path_xyz): def get_d_path(self, v_ego, path_t, path_xyz):
# Reduce reliance on lanelines that are too far apart or # Reduce reliance on lanelines that are too far apart or

@ -25,7 +25,7 @@ class LatControlPID():
pid_log.active = False pid_log.active = False
self.pid.reset() self.pid.reset()
else: else:
self.angle_steers_des = path_plan.angleSteers # get from MPC/PathPlanner self.angle_steers_des = path_plan.angleSteers # get from MPC/LateralPlanner
steers_max = get_steer_max(CP, CS.vEgo) steers_max = get_steer_max(CP, CS.vEgo)
self.pid.pos_limit = steers_max self.pid.pos_limit = steers_max

@ -12,8 +12,8 @@ from common.params import Params
import cereal.messaging as messaging import cereal.messaging as messaging
from cereal import log from cereal import log
LaneChangeState = log.PathPlan.LaneChangeState LaneChangeState = log.LateralPlan.LaneChangeState
LaneChangeDirection = log.PathPlan.LaneChangeDirection LaneChangeDirection = log.LateralPlan.LaneChangeDirection
LOG_MPC = os.environ.get('LOG_MPC', False) LOG_MPC = os.environ.get('LOG_MPC', False)
@ -22,27 +22,27 @@ LANE_CHANGE_TIME_MAX = 10.
DESIRES = { DESIRES = {
LaneChangeDirection.none: { LaneChangeDirection.none: {
LaneChangeState.off: log.PathPlan.Desire.none, LaneChangeState.off: log.LateralPlan.Desire.none,
LaneChangeState.preLaneChange: log.PathPlan.Desire.none, LaneChangeState.preLaneChange: log.LateralPlan.Desire.none,
LaneChangeState.laneChangeStarting: log.PathPlan.Desire.none, LaneChangeState.laneChangeStarting: log.LateralPlan.Desire.none,
LaneChangeState.laneChangeFinishing: log.PathPlan.Desire.none, LaneChangeState.laneChangeFinishing: log.LateralPlan.Desire.none,
}, },
LaneChangeDirection.left: { LaneChangeDirection.left: {
LaneChangeState.off: log.PathPlan.Desire.none, LaneChangeState.off: log.LateralPlan.Desire.none,
LaneChangeState.preLaneChange: log.PathPlan.Desire.none, LaneChangeState.preLaneChange: log.LateralPlan.Desire.none,
LaneChangeState.laneChangeStarting: log.PathPlan.Desire.laneChangeLeft, LaneChangeState.laneChangeStarting: log.LateralPlan.Desire.laneChangeLeft,
LaneChangeState.laneChangeFinishing: log.PathPlan.Desire.laneChangeLeft, LaneChangeState.laneChangeFinishing: log.LateralPlan.Desire.laneChangeLeft,
}, },
LaneChangeDirection.right: { LaneChangeDirection.right: {
LaneChangeState.off: log.PathPlan.Desire.none, LaneChangeState.off: log.LateralPlan.Desire.none,
LaneChangeState.preLaneChange: log.PathPlan.Desire.none, LaneChangeState.preLaneChange: log.LateralPlan.Desire.none,
LaneChangeState.laneChangeStarting: log.PathPlan.Desire.laneChangeRight, LaneChangeState.laneChangeStarting: log.LateralPlan.Desire.laneChangeRight,
LaneChangeState.laneChangeFinishing: log.PathPlan.Desire.laneChangeRight, LaneChangeState.laneChangeFinishing: log.LateralPlan.Desire.laneChangeRight,
}, },
} }
class PathPlanner(): class LateralPlanner():
def __init__(self, CP): def __init__(self, CP):
self.LP = LanePlanner() self.LP = LanePlanner()
@ -57,7 +57,7 @@ class PathPlanner():
self.lane_change_timer = 0.0 self.lane_change_timer = 0.0
self.lane_change_ll_prob = 1.0 self.lane_change_ll_prob = 1.0
self.prev_one_blinker = False self.prev_one_blinker = False
self.desire = log.PathPlan.Desire.none self.desire = log.LateralPlan.Desire.none
self.path_xyz = np.zeros((TRAJECTORY_SIZE,3)) self.path_xyz = np.zeros((TRAJECTORY_SIZE,3))
self.plan_yaw = np.zeros((TRAJECTORY_SIZE,)) self.plan_yaw = np.zeros((TRAJECTORY_SIZE,))
@ -162,7 +162,7 @@ class PathPlanner():
self.desire = DESIRES[self.lane_change_direction][self.lane_change_state] self.desire = DESIRES[self.lane_change_direction][self.lane_change_state]
# Turn off lanes during lane change # Turn off lanes during lane change
if self.desire == log.PathPlan.Desire.laneChangeRight or self.desire == log.PathPlan.Desire.laneChangeLeft: if self.desire == log.LateralPlan.Desire.laneChangeRight or self.desire == log.LateralPlan.Desire.laneChangeLeft:
self.LP.lll_prob *= self.lane_change_ll_prob self.LP.lll_prob *= self.lane_change_ll_prob
self.LP.rll_prob *= self.lane_change_ll_prob self.LP.rll_prob *= self.lane_change_ll_prob
d_path_xyz = self.LP.get_d_path(v_ego, self.t_idxs, self.path_xyz) d_path_xyz = self.LP.get_d_path(v_ego, self.t_idxs, self.path_xyz)
@ -227,25 +227,24 @@ class PathPlanner():
def publish(self, sm, pm): def publish(self, sm, pm):
plan_solution_valid = self.solution_invalid_cnt < 2 plan_solution_valid = self.solution_invalid_cnt < 2
plan_send = messaging.new_message('pathPlan') plan_send = messaging.new_message('lateralPlan')
plan_send.valid = sm.all_alive_and_valid(service_list=['carState', 'controlsState', 'liveParameters', 'modelV2']) plan_send.valid = sm.all_alive_and_valid(service_list=['carState', 'controlsState', 'liveParameters', 'modelV2'])
plan_send.pathPlan.laneWidth = float(self.LP.lane_width) plan_send.lateralPlan.laneWidth = float(self.LP.lane_width)
plan_send.pathPlan.dPathPoints = [float(x) for x in self.y_pts] plan_send.lateralPlan.dPathPoints = [float(x) for x in self.y_pts]
plan_send.pathPlan.lProb = float(self.LP.lll_prob) plan_send.lateralPlan.lProb = float(self.LP.lll_prob)
plan_send.pathPlan.rProb = float(self.LP.rll_prob) plan_send.lateralPlan.rProb = float(self.LP.rll_prob)
plan_send.pathPlan.dProb = float(self.LP.d_prob) plan_send.lateralPlan.dProb = float(self.LP.d_prob)
plan_send.pathPlan.angleSteers = float(self.desired_steering_wheel_angle_deg) plan_send.lateralPlan.angleSteers = float(self.desired_steering_wheel_angle_deg)
plan_send.pathPlan.rateSteers = float(self.desired_steering_wheel_angle_rate_deg) plan_send.lateralPlan.rateSteers = float(self.desired_steering_wheel_angle_rate_deg)
plan_send.pathPlan.angleOffset = float(sm['liveParameters'].angleOffsetAverage) plan_send.lateralPlan.angleOffset = float(sm['liveParameters'].angleOffsetAverage)
plan_send.pathPlan.mpcSolutionValid = bool(plan_solution_valid) plan_send.lateralPlan.mpcSolutionValid = bool(plan_solution_valid)
plan_send.pathPlan.paramsValid = bool(sm['liveParameters'].valid)
plan_send.lateralPlan.desire = self.desire
plan_send.pathPlan.desire = self.desire plan_send.lateralPlan.laneChangeState = self.lane_change_state
plan_send.pathPlan.laneChangeState = self.lane_change_state plan_send.lateralPlan.laneChangeDirection = self.lane_change_direction
plan_send.pathPlan.laneChangeDirection = self.lane_change_direction
pm.send('lateralPlan', plan_send)
pm.send('pathPlan', plan_send)
if LOG_MPC: if LOG_MPC:
dat = messaging.new_message('liveMpc') dat = messaging.new_message('liveMpc')

@ -5,7 +5,6 @@ from common.params import Params
from common.numpy_fast import interp from common.numpy_fast import interp
import cereal.messaging as messaging import cereal.messaging as messaging
from cereal import car
from common.realtime import sec_since_boot from common.realtime import sec_since_boot
from selfdrive.swaglog import cloudlog from selfdrive.swaglog import cloudlog
from selfdrive.config import Conversions as CV from selfdrive.config import Conversions as CV
@ -79,9 +78,6 @@ class Planner():
self.fcw_checker = FCWChecker() self.fcw_checker = FCWChecker()
self.path_x = np.arange(192) self.path_x = np.arange(192)
self.radar_dead = False
self.radar_fault = False
self.radar_can_error = False
self.fcw = False self.fcw = False
self.params = Params() self.params = Params()
@ -185,12 +181,6 @@ class Planner():
if self.fcw: if self.fcw:
cloudlog.info("FCW triggered %s", self.fcw_checker.counters) cloudlog.info("FCW triggered %s", self.fcw_checker.counters)
self.radar_dead = not sm.alive['radarState']
radar_errors = list(sm['radarState'].radarErrors)
self.radar_fault = car.RadarData.Error.fault in radar_errors
self.radar_can_error = car.RadarData.Error.canError in radar_errors
# Interpolate 0.05 seconds and save as starting point for next iteration # Interpolate 0.05 seconds and save as starting point for next iteration
a_acc_sol = self.a_acc_start + (CP.radarTimeStep / LON_MPC_STEP) * (self.a_acc - self.a_acc_start) a_acc_sol = self.a_acc_start + (CP.radarTimeStep / LON_MPC_STEP) * (self.a_acc - self.a_acc_start)
v_acc_sol = self.v_acc_start + CP.radarTimeStep * (a_acc_sol + self.a_acc_start) / 2.0 v_acc_sol = self.v_acc_start + CP.radarTimeStep * (a_acc_sol + self.a_acc_start) / 2.0
@ -203,31 +193,25 @@ class Planner():
self.mpc1.publish(pm) self.mpc1.publish(pm)
self.mpc2.publish(pm) self.mpc2.publish(pm)
plan_send = messaging.new_message('plan') plan_send = messaging.new_message('longitudinalPlan')
plan_send.valid = sm.all_alive_and_valid(service_list=['carState', 'controlsState', 'radarState']) plan_send.valid = sm.all_alive_and_valid(service_list=['carState', 'controlsState', 'radarState'])
plan_send.plan.mdMonoTime = sm.logMonoTime['modelV2'] longitudinalPlan = plan_send.longitudinalPlan
plan_send.plan.radarStateMonoTime = sm.logMonoTime['radarState'] longitudinalPlan.mdMonoTime = sm.logMonoTime['modelV2']
longitudinalPlan.radarStateMonoTime = sm.logMonoTime['radarState']
# longitudal plan
plan_send.plan.vCruise = float(self.v_cruise)
plan_send.plan.aCruise = float(self.a_cruise)
plan_send.plan.vStart = float(self.v_acc_start)
plan_send.plan.aStart = float(self.a_acc_start)
plan_send.plan.vTarget = float(self.v_acc)
plan_send.plan.aTarget = float(self.a_acc)
plan_send.plan.vTargetFuture = float(self.v_acc_future)
plan_send.plan.hasLead = self.mpc1.prev_lead_status
plan_send.plan.longitudinalPlanSource = self.longitudinalPlanSource
radar_valid = not (self.radar_dead or self.radar_fault)
plan_send.plan.radarValid = bool(radar_valid)
plan_send.plan.radarCanError = bool(self.radar_can_error)
plan_send.plan.processingDelay = (plan_send.logMonoTime / 1e9) - sm.rcv_time['radarState'] longitudinalPlan.vCruise = float(self.v_cruise)
longitudinalPlan.aCruise = float(self.a_cruise)
longitudinalPlan.vStart = float(self.v_acc_start)
longitudinalPlan.aStart = float(self.a_acc_start)
longitudinalPlan.vTarget = float(self.v_acc)
longitudinalPlan.aTarget = float(self.a_acc)
longitudinalPlan.vTargetFuture = float(self.v_acc_future)
longitudinalPlan.hasLead = self.mpc1.prev_lead_status
longitudinalPlan.longitudinalPlanSource = self.longitudinalPlanSource
longitudinalPlan.fcw = self.fcw
# Send out fcw longitudinalPlan.processingDelay = (plan_send.logMonoTime / 1e9) - sm.rcv_time['radarState']
plan_send.plan.fcw = self.fcw
pm.send('plan', plan_send) pm.send('longitudinalPlan', plan_send)

@ -3,9 +3,9 @@ from cereal import car
from common.params import Params from common.params import Params
from common.realtime import Priority, config_realtime_process from common.realtime import Priority, config_realtime_process
from selfdrive.swaglog import cloudlog from selfdrive.swaglog import cloudlog
from selfdrive.controls.lib.planner import Planner from selfdrive.controls.lib.longitudinal_planner import Planner
from selfdrive.controls.lib.vehicle_model import VehicleModel from selfdrive.controls.lib.vehicle_model import VehicleModel
from selfdrive.controls.lib.pathplanner import PathPlanner from selfdrive.controls.lib.lateral_planner import LateralPlanner
import cereal.messaging as messaging import cereal.messaging as messaging
@ -18,7 +18,7 @@ def plannerd_thread(sm=None, pm=None):
cloudlog.info("plannerd got CarParams: %s", CP.carName) cloudlog.info("plannerd got CarParams: %s", CP.carName)
PL = Planner(CP) PL = Planner(CP)
PP = PathPlanner(CP) PP = LateralPlanner(CP)
VM = VehicleModel(CP) VM = VehicleModel(CP)
@ -27,7 +27,7 @@ def plannerd_thread(sm=None, pm=None):
poll=['radarState', 'modelV2']) poll=['radarState', 'modelV2'])
if pm is None: if pm is None:
pm = messaging.PubMaster(['plan', 'liveLongitudinalMpc', 'pathPlan', 'liveMpc']) pm = messaging.PubMaster(['longitudinalPlan', 'liveLongitudinalMpc', 'lateralPlan', 'liveMpc'])
sm['liveParameters'].valid = True sm['liveParameters'].valid = True
sm['liveParameters'].sensorValid = True sm['liveParameters'].sensorValid = True

@ -100,8 +100,8 @@ class RadarD():
def update(self, sm, rr, enable_lead): def update(self, sm, rr, enable_lead):
self.current_time = 1e-9*max(sm.logMonoTime.values()) self.current_time = 1e-9*max(sm.logMonoTime.values())
if sm.updated['controlsState']: if sm.updated['carState']:
self.v_ego = sm['controlsState'].vEgo self.v_ego = sm['carState'].vEgo
self.v_ego_hist.append(self.v_ego) self.v_ego_hist.append(self.v_ego)
if sm.updated['modelV2']: if sm.updated['modelV2']:
self.ready = True self.ready = True
@ -157,12 +157,12 @@ class RadarD():
# *** publish radarState *** # *** publish radarState ***
dat = messaging.new_message('radarState') dat = messaging.new_message('radarState')
dat.valid = sm.all_alive_and_valid() dat.valid = sm.all_alive_and_valid() and len(rr.errors) == 0
radarState = dat.radarState radarState = dat.radarState
radarState.mdMonoTime = sm.logMonoTime['modelV2'] radarState.mdMonoTime = sm.logMonoTime['modelV2']
radarState.canMonoTimes = list(rr.canMonoTimes) radarState.canMonoTimes = list(rr.canMonoTimes)
radarState.radarErrors = list(rr.errors) radarState.radarErrors = list(rr.errors)
radarState.controlsStateMonoTime = sm.logMonoTime['controlsState'] radarState.carStateMonoTime = sm.logMonoTime['carState']
if enable_lead: if enable_lead:
if len(sm['modelV2'].leads) > 1: if len(sm['modelV2'].leads) > 1:
@ -188,7 +188,7 @@ def radard_thread(sm=None, pm=None, can_sock=None):
if can_sock is None: if can_sock is None:
can_sock = messaging.sub_sock('can') can_sock = messaging.sub_sock('can')
if sm is None: if sm is None:
sm = messaging.SubMaster(['modelV2', 'controlsState']) sm = messaging.SubMaster(['modelV2', 'carState'])
if pm is None: if pm is None:
pm = messaging.PubMaster(['radarState', 'liveTracks']) pm = messaging.PubMaster(['radarState', 'liveTracks'])

@ -4,7 +4,7 @@ import numpy as np
from cereal import log from cereal import log
import cereal.messaging as messaging import cereal.messaging as messaging
from selfdrive.config import Conversions as CV from selfdrive.config import Conversions as CV
from selfdrive.controls.lib.planner import calc_cruise_accel_limits from selfdrive.controls.lib.longitudinal_planner import calc_cruise_accel_limits
from selfdrive.controls.lib.speed_smoother import speed_smoother from selfdrive.controls.lib.speed_smoother import speed_smoother
from selfdrive.controls.lib.long_mpc import LongitudinalMpc from selfdrive.controls.lib.long_mpc import LongitudinalMpc

@ -52,7 +52,7 @@ class TestStartup(unittest.TestCase):
time.sleep(2) # wait for controlsd to be ready time.sleep(2) # wait for controlsd to be ready
health = messaging.new_message('health') health = messaging.new_message('health')
health.health.hwType = log.HealthData.HwType.uno health.health.pandaType = log.HealthData.PandaType.uno
pm.send('health', health) pm.send('health', health)
# fingerprint # fingerprint

@ -17,7 +17,7 @@ def cycle_alerts(duration=200, is_metric=False):
CP = CarInterface.get_params("HONDA CIVIC 2016 TOURING") CP = CarInterface.get_params("HONDA CIVIC 2016 TOURING")
sm = messaging.SubMaster(['thermal', 'health', 'frame', 'model', 'liveCalibration', sm = messaging.SubMaster(['thermal', 'health', 'frame', 'model', 'liveCalibration',
'dMonitoringState', 'plan', 'pathPlan', 'liveLocationKalman']) 'driverMonitoringState', 'plan', 'lateralPlan', 'liveLocationKalman'])
controls_state = messaging.pub_sock('controlsState') controls_state = messaging.pub_sock('controlsState')
thermal = messaging.pub_sock('thermal') thermal = messaging.pub_sock('thermal')

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

@ -58,7 +58,7 @@ def mpc_vwr_thread(addr="127.0.0.1"):
# *** log *** # *** log ***
livempc = messaging.sub_sock('liveMpc', addr=addr) livempc = messaging.sub_sock('liveMpc', addr=addr)
model = messaging.sub_sock('model', addr=addr) model = messaging.sub_sock('model', addr=addr)
path_plan_sock = messaging.sub_sock('pathPlan', addr=addr) path_plan_sock = messaging.sub_sock('lateralPlan', addr=addr)
while 1: while 1:
lMpc = messaging.recv_sock(livempc, wait=True) lMpc = messaging.recv_sock(livempc, wait=True)
@ -75,7 +75,7 @@ def mpc_vwr_thread(addr="127.0.0.1"):
r_path_y = np.polyval(l_poly, path_x) r_path_y = np.polyval(l_poly, path_x)
if pp is not None: if pp is not None:
p_path_y = np.polyval(pp.pathPlan.dPolyDEPRECATED, path_x) p_path_y = np.polyval(pp.lateralPlan.dPolyDEPRECATED, path_x)
lineP.set_xdata(p_path_y) lineP.set_xdata(p_path_y)
lineP.set_ydata(path_x) lineP.set_ydata(path_x)

@ -47,7 +47,7 @@ if __name__ == "__main__":
for msg in lr: for msg in lr:
if msg.which() == "health": if msg.which() == "health":
if msg.health.hwType not in ['uno', 'blackPanda']: if msg.health.pandaType not in ['uno', 'blackPanda']:
dongles.append(dongle_id) dongles.append(dongle_id)
break break

@ -463,7 +463,7 @@ def manager_thread():
while 1: while 1:
msg = messaging.recv_sock(thermal_sock, wait=True) msg = messaging.recv_sock(thermal_sock, wait=True)
if msg.thermal.freeSpace < 0.05: if msg.thermal.freeSpacePercent < 0.05:
logger_dead = True logger_dead = True
if msg.thermal.started: if msg.thermal.started:

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

@ -186,13 +186,13 @@ void dmonitoring_publish(PubMaster &pm, uint32_t frame_id, const DMonitoringResu
framed.setRightEyeProb(res.right_eye_prob); framed.setRightEyeProb(res.right_eye_prob);
framed.setLeftBlinkProb(res.left_blink_prob); framed.setLeftBlinkProb(res.left_blink_prob);
framed.setRightBlinkProb(res.right_blink_prob); framed.setRightBlinkProb(res.right_blink_prob);
framed.setSgProb(res.sg_prob); framed.setSunglassesProb(res.sg_prob);
framed.setPoorVision(res.poor_vision); framed.setPoorVision(res.poor_vision);
framed.setPartialFace(res.partial_face); framed.setPartialFace(res.partial_face);
framed.setDistractedPose(res.distracted_pose); framed.setDistractedPose(res.distracted_pose);
framed.setDistractedEyes(res.distracted_eyes); framed.setDistractedEyes(res.distracted_eyes);
if (send_raw_pred) { if (send_raw_pred) {
framed.setRawPred(raw_pred.asBytes()); framed.setRawPredictions(raw_pred.asBytes());
} }
pm.send("driverState", msg); pm.send("driverState", msg);

@ -273,7 +273,7 @@ void model_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t frame_id, flo
framed.setTimestampEof(timestamp_eof); framed.setTimestampEof(timestamp_eof);
framed.setModelExecutionTime(model_execution_time); framed.setModelExecutionTime(model_execution_time);
if (send_raw_pred) { if (send_raw_pred) {
framed.setRawPred(raw_pred.asBytes()); framed.setRawPredictions(raw_pred.asBytes());
} }
fill_model(framed, net_outputs); fill_model(framed, net_outputs);
pm.send("modelV2", msg); pm.send("modelV2", msg);

@ -9,15 +9,13 @@ from selfdrive.locationd.calibrationd import Calibration
def dmonitoringd_thread(sm=None, pm=None): def dmonitoringd_thread(sm=None, pm=None):
if pm is None: if pm is None:
pm = messaging.PubMaster(['dMonitoringState']) pm = messaging.PubMaster(['driverMonitoringState'])
if sm is None: if sm is None:
sm = messaging.SubMaster(['driverState', 'liveCalibration', 'carState', 'controlsState', 'modelV2'], poll=['driverState']) sm = messaging.SubMaster(['driverState', 'liveCalibration', 'carState', 'controlsState', 'modelV2'], poll=['driverState'])
driver_status = DriverStatus(rhd=Params().get("IsRHD") == b"1") driver_status = DriverStatus(rhd=Params().get("IsRHD") == b"1")
offroad = Params().get("IsOffroad") == b"1"
sm['liveCalibration'].calStatus = Calibration.INVALID sm['liveCalibration'].calStatus = Calibration.INVALID
sm['liveCalibration'].rpyCalib = [0, 0, 0] sm['liveCalibration'].rpyCalib = [0, 0, 0]
sm['carState'].buttonEvents = [] sm['carState'].buttonEvents = []
@ -58,14 +56,13 @@ def dmonitoringd_thread(sm=None, pm=None):
# Update events from driver state # Update events from driver state
driver_status.update(events, driver_engaged, sm['controlsState'].enabled, sm['carState'].standstill) driver_status.update(events, driver_engaged, sm['controlsState'].enabled, sm['carState'].standstill)
# build dMonitoringState packet # build driverMonitoringState packet
dat = messaging.new_message('dMonitoringState') dat = messaging.new_message('driverMonitoringState')
dat.dMonitoringState = { dat.driverMonitoringState = {
"events": events.to_msg(), "events": events.to_msg(),
"faceDetected": driver_status.face_detected, "faceDetected": driver_status.face_detected,
"isDistracted": driver_status.driver_distracted, "isDistracted": driver_status.driver_distracted,
"awarenessStatus": driver_status.awareness, "awarenessStatus": driver_status.awareness,
"isRHD": driver_status.is_rhd_region,
"posePitchOffset": driver_status.pose.pitch_offseter.filtered_stat.mean(), "posePitchOffset": driver_status.pose.pitch_offseter.filtered_stat.mean(),
"posePitchValidCount": driver_status.pose.pitch_offseter.filtered_stat.n, "posePitchValidCount": driver_status.pose.pitch_offseter.filtered_stat.n,
"poseYawOffset": driver_status.pose.yaw_offseter.filtered_stat.mean(), "poseYawOffset": driver_status.pose.yaw_offseter.filtered_stat.mean(),
@ -75,10 +72,9 @@ def dmonitoringd_thread(sm=None, pm=None):
"awarenessPassive": driver_status.awareness_passive, "awarenessPassive": driver_status.awareness_passive,
"isLowStd": driver_status.pose.low_std, "isLowStd": driver_status.pose.low_std,
"hiStdCount": driver_status.hi_stds, "hiStdCount": driver_status.hi_stds,
"isPreview": offroad,
"isActiveMode": driver_status.active_monitoring_mode, "isActiveMode": driver_status.active_monitoring_mode,
} }
pm.send('dMonitoringState', dat) pm.send('driverMonitoringState', dat)
def main(sm=None, pm=None): def main(sm=None, pm=None):
dmonitoringd_thread(sm, pm) dmonitoringd_thread(sm, pm)

@ -191,8 +191,8 @@ class DriverStatus():
# self.pose.roll_std = driver_state.faceOrientationStd[2] # self.pose.roll_std = driver_state.faceOrientationStd[2]
model_std_max = max(self.pose.pitch_std, self.pose.yaw_std) model_std_max = max(self.pose.pitch_std, self.pose.yaw_std)
self.pose.low_std = model_std_max < _POSESTD_THRESHOLD and not self.face_partial self.pose.low_std = model_std_max < _POSESTD_THRESHOLD and not self.face_partial
self.blink.left_blink = driver_state.leftBlinkProb * (driver_state.leftEyeProb > _EYE_THRESHOLD) * (driver_state.sgProb < _SG_THRESHOLD) self.blink.left_blink = driver_state.leftBlinkProb * (driver_state.leftEyeProb > _EYE_THRESHOLD) * (driver_state.sunglassesProb < _SG_THRESHOLD)
self.blink.right_blink = driver_state.rightBlinkProb * (driver_state.rightEyeProb > _EYE_THRESHOLD) * (driver_state.sgProb < _SG_THRESHOLD) self.blink.right_blink = driver_state.rightBlinkProb * (driver_state.rightEyeProb > _EYE_THRESHOLD) * (driver_state.sunglassesProb < _SG_THRESHOLD)
self.driver_distracted = self._is_driver_distracted(self.pose, self.blink) > 0 and \ self.driver_distracted = self._is_driver_distracted(self.pose, self.blink) > 0 and \
driver_state.faceProb > _FACE_THRESHOLD and self.pose.low_std driver_state.faceProb > _FACE_THRESHOLD and self.pose.low_std

@ -31,7 +31,6 @@ def make_msg(face_detected, distracted=False, model_uncertain=False):
ds.rightBlinkProb = 1. * distracted ds.rightBlinkProb = 1. * distracted
ds.faceOrientationStd = [1.*model_uncertain, 1.*model_uncertain, 1.*model_uncertain] ds.faceOrientationStd = [1.*model_uncertain, 1.*model_uncertain, 1.*model_uncertain]
ds.facePositionStd = [1.*model_uncertain, 1.*model_uncertain] ds.facePositionStd = [1.*model_uncertain, 1.*model_uncertain]
ds.sgProb = 0.
return ds return ds
# driver state from neural net, 10Hz # driver state from neural net, 10Hz

@ -68,7 +68,6 @@ class Maneuver():
d_rel=d_rel, v_rel=v_rel, v_lead=speed_lead, d_rel=d_rel, v_rel=v_rel, v_lead=speed_lead,
v_target_lead=last_controls_state.vTargetLead, pid_speed=last_controls_state.vPid, v_target_lead=last_controls_state.vTargetLead, pid_speed=last_controls_state.vPid,
cruise_speed=last_controls_state.vCruise, cruise_speed=last_controls_state.vCruise,
jerk_factor=last_controls_state.jerkFactor,
a_target=last_controls_state.aTarget, a_target=last_controls_state.aTarget,
fcw=log['fcw']) fcw=log['fcw'])

@ -28,7 +28,6 @@ class ManeuverPlot():
self.v_target_lead_array = [] self.v_target_lead_array = []
self.pid_speed_array = [] self.pid_speed_array = []
self.cruise_speed_array = [] self.cruise_speed_array = []
self.jerk_factor_array = []
self.a_target_array = [] self.a_target_array = []
@ -40,7 +39,7 @@ class ManeuverPlot():
def add_data(self, time, gas, brake, steer_torque, distance, speed, def add_data(self, time, gas, brake, steer_torque, distance, speed,
acceleration, up_accel_cmd, ui_accel_cmd, uf_accel_cmd, d_rel, v_rel, acceleration, up_accel_cmd, ui_accel_cmd, uf_accel_cmd, d_rel, v_rel,
v_lead, v_target_lead, pid_speed, cruise_speed, jerk_factor, a_target, fcw): v_lead, v_target_lead, pid_speed, cruise_speed, a_target, fcw):
self.time_array.append(time) self.time_array.append(time)
self.gas_array.append(gas) self.gas_array.append(gas)
self.brake_array.append(brake) self.brake_array.append(brake)
@ -57,7 +56,6 @@ class ManeuverPlot():
self.v_target_lead_array.append(v_target_lead) self.v_target_lead_array.append(v_target_lead)
self.pid_speed_array.append(pid_speed) self.pid_speed_array.append(pid_speed)
self.cruise_speed_array.append(cruise_speed) self.cruise_speed_array.append(cruise_speed)
self.jerk_factor_array.append(jerk_factor)
self.a_target_array.append(a_target) self.a_target_array.append(a_target)
self.fcw_array.append(fcw) self.fcw_array.append(fcw)

@ -110,7 +110,6 @@ class Plant():
self.rate = rate self.rate = rate
if not Plant.messaging_initialized: if not Plant.messaging_initialized:
Plant.pm = messaging.PubMaster(['frame', 'frontFrame', 'ubloxRaw']) Plant.pm = messaging.PubMaster(['frame', 'frontFrame', 'ubloxRaw'])
Plant.logcan = messaging.pub_sock('can') Plant.logcan = messaging.pub_sock('can')
Plant.sendcan = messaging.sub_sock('sendcan') Plant.sendcan = messaging.sub_sock('sendcan')
@ -119,10 +118,10 @@ class Plant():
Plant.live_location_kalman = messaging.pub_sock('liveLocationKalman') Plant.live_location_kalman = messaging.pub_sock('liveLocationKalman')
Plant.health = messaging.pub_sock('health') Plant.health = messaging.pub_sock('health')
Plant.thermal = messaging.pub_sock('thermal') Plant.thermal = messaging.pub_sock('thermal')
Plant.dMonitoringState = messaging.pub_sock('dMonitoringState') Plant.driverMonitoringState = messaging.pub_sock('driverMonitoringState')
Plant.cal = messaging.pub_sock('liveCalibration') Plant.cal = messaging.pub_sock('liveCalibration')
Plant.controls_state = messaging.sub_sock('controlsState') Plant.controls_state = messaging.sub_sock('controlsState')
Plant.plan = messaging.sub_sock('plan') Plant.plan = messaging.sub_sock('longitudinalPlan')
Plant.messaging_initialized = True Plant.messaging_initialized = True
self.frame = 0 self.frame = 0
@ -200,7 +199,7 @@ class Plant():
fcw = None fcw = None
for a in messaging.drain_sock(Plant.plan): for a in messaging.drain_sock(Plant.plan):
if a.plan.fcw: if a.longitudinalPlan.fcw:
fcw = True fcw = True
if self.cp.vl[0x1fa]['COMPUTER_BRAKE_REQUEST']: if self.cp.vl[0x1fa]['COMPUTER_BRAKE_REQUEST']:
@ -370,9 +369,9 @@ class Plant():
live_parameters.liveParameters.stiffnessFactor = 1.0 live_parameters.liveParameters.stiffnessFactor = 1.0
Plant.live_params.send(live_parameters.to_bytes()) Plant.live_params.send(live_parameters.to_bytes())
dmon_state = messaging.new_message('dMonitoringState') dmon_state = messaging.new_message('driverMonitoringState')
dmon_state.dMonitoringState.isDistracted = False dmon_state.driverMonitoringState.isDistracted = False
Plant.dMonitoringState.send(dmon_state.to_bytes()) Plant.driverMonitoringState.send(dmon_state.to_bytes())
health = messaging.new_message('health') health = messaging.new_message('health')
health.health.safetyModel = car.CarParams.SafetyModel.hondaNidec health.health.safetyModel = car.CarParams.SafetyModel.hondaNidec
@ -380,7 +379,7 @@ class Plant():
Plant.health.send(health.to_bytes()) Plant.health.send(health.to_bytes())
thermal = messaging.new_message('thermal') thermal = messaging.new_message('thermal')
thermal.thermal.freeSpace = 1. thermal.thermal.freeSpacePercent = 1.
thermal.thermal.batteryPercent = 100 thermal.thermal.batteryPercent = 100
Plant.thermal.send(thermal.to_bytes()) Plant.thermal.send(thermal.to_bytes())

@ -330,14 +330,6 @@ class LongitudinalControl(unittest.TestCase):
params.put("OpenpilotEnabledToggle", "1") params.put("OpenpilotEnabledToggle", "1")
params.put("CommunityFeaturesToggle", "1") params.put("CommunityFeaturesToggle", "1")
manager.prepare_managed_process('radard')
manager.prepare_managed_process('controlsd')
manager.prepare_managed_process('plannerd')
@classmethod
def tearDownClass(cls):
pass
# hack # hack
def test_longitudinal_setup(self): def test_longitudinal_setup(self):
pass pass

@ -21,10 +21,10 @@ if __name__ == "__main__":
if os.path.isfile(cache_path): if os.path.isfile(cache_path):
os.remove(cache_path) os.remove(cache_path)
output_size = len(np.frombuffer(msgs[0].model.rawPred, dtype=np.float32)) output_size = len(np.frombuffer(msgs[0].model.rawPredictions, dtype=np.float32))
output_data = np.zeros((len(msgs), output_size), dtype=np.float32) output_data = np.zeros((len(msgs), output_size), dtype=np.float32)
for i, msg in enumerate(msgs): for i, msg in enumerate(msgs):
output_data[i] = np.frombuffer(msg.model.rawPred, dtype=np.float32) output_data[i] = np.frombuffer(msg.model.rawPredictions, dtype=np.float32)
np.save(os.path.expanduser('~/modeldata.npy'), output_data) np.save(os.path.expanduser('~/modeldata.npy'), output_data)
print("Finished replay") print("Finished replay")

@ -34,7 +34,7 @@ def camera_replay(lr, fr, desire=None, calib=None):
spinner = Spinner() spinner = Spinner()
spinner.update("starting model replay") spinner.update("starting model replay")
pm = messaging.PubMaster(['frame', 'liveCalibration', 'pathPlan']) pm = messaging.PubMaster(['frame', 'liveCalibration', 'lateralPlan'])
sm = messaging.SubMaster(['modelV2']) sm = messaging.SubMaster(['modelV2'])
# TODO: add dmonitoringmodeld # TODO: add dmonitoringmodeld
@ -49,7 +49,7 @@ def camera_replay(lr, fr, desire=None, calib=None):
sm.update(1000) sm.update(1000)
print("procs started") print("procs started")
desires_by_index = {v:k for k,v in log.PathPlan.Desire.schema.enumerants.items()} desires_by_index = {v:k for k,v in log.LateralPlan.Desire.schema.enumerants.items()}
cal = [msg for msg in lr if msg.which() == "liveCalibration"] cal = [msg for msg in lr if msg.which() == "liveCalibration"]
for msg in cal[:5]: for msg in cal[:5]:
@ -63,9 +63,9 @@ def camera_replay(lr, fr, desire=None, calib=None):
elif msg.which() == "frame": elif msg.which() == "frame":
if desire is not None: if desire is not None:
for i in desire[frame_idx].nonzero()[0]: for i in desire[frame_idx].nonzero()[0]:
dat = messaging.new_message('pathPlan') dat = messaging.new_message('lateralPlan')
dat.pathPlan.desire = desires_by_index[i] dat.lateralPlan.desire = desires_by_index[i]
pm.send('pathPlan', dat) pm.send('lateralPlan', dat)
f = msg.as_builder() f = msg.as_builder()
img = fr.get(frame_idx, pix_fmt="rgb24")[0][:,:,::-1] img = fr.get(frame_idx, pix_fmt="rgb24")[0][:,:,::-1]

@ -160,7 +160,7 @@ def fingerprint(msgs, fsm, can_sock):
can_sock.recv_ready.set() can_sock.recv_ready.set()
can_sock.wait = False can_sock.wait = False
# we know fingerprinting is done when controlsd sets sm['pathPlan'].sensorValid # we know fingerprinting is done when controlsd sets sm['lateralPlan'].sensorValid
wait_for_event(fsm.update_called) wait_for_event(fsm.update_called)
fsm.update_called.clear() fsm.update_called.clear()
@ -221,7 +221,7 @@ CONFIGS = [
proc_name="controlsd", proc_name="controlsd",
pub_sub={ pub_sub={
"can": ["controlsState", "carState", "carControl", "sendcan", "carEvents", "carParams"], "can": ["controlsState", "carState", "carControl", "sendcan", "carEvents", "carParams"],
"thermal": [], "health": [], "liveCalibration": [], "dMonitoringState": [], "plan": [], "pathPlan": [], "gpsLocation": [], "liveLocationKalman": [], "thermal": [], "health": [], "liveCalibration": [], "driverMonitoringState": [], "longitudinalPlan": [], "lateralPlan": [], "gpsLocation": [], "liveLocationKalman": [], "liveParameters": [], "radarState": [],
"modelV2": [], "frontFrame": [], "frame": [], "ubloxRaw": [], "managerState": [], "modelV2": [], "frontFrame": [], "frame": [], "ubloxRaw": [], "managerState": [],
}, },
ignore=["logMonoTime", "valid", "controlsState.startMonoTime", "controlsState.cumLagMs"], ignore=["logMonoTime", "valid", "controlsState.startMonoTime", "controlsState.cumLagMs"],
@ -233,7 +233,7 @@ CONFIGS = [
proc_name="radard", proc_name="radard",
pub_sub={ pub_sub={
"can": ["radarState", "liveTracks"], "can": ["radarState", "liveTracks"],
"liveParameters": [], "controlsState": [], "modelV2": [], "liveParameters": [], "carState": [], "modelV2": [],
}, },
ignore=["logMonoTime", "valid", "radarState.cumLagMs"], ignore=["logMonoTime", "valid", "radarState.cumLagMs"],
init_callback=get_car_params, init_callback=get_car_params,
@ -243,10 +243,10 @@ CONFIGS = [
ProcessConfig( ProcessConfig(
proc_name="plannerd", proc_name="plannerd",
pub_sub={ pub_sub={
"modelV2": ["pathPlan"], "radarState": ["plan"], "modelV2": ["lateralPlan"], "radarState": ["longitudinalPlan"],
"carState": [], "controlsState": [], "liveParameters": [], "carState": [], "controlsState": [], "liveParameters": [],
}, },
ignore=["logMonoTime", "valid", "plan.processingDelay"], ignore=["logMonoTime", "valid", "longitudinalPlan.processingDelay"],
init_callback=get_car_params, init_callback=get_car_params,
should_recv_callback=None, should_recv_callback=None,
tolerance=None, tolerance=None,
@ -265,7 +265,7 @@ CONFIGS = [
ProcessConfig( ProcessConfig(
proc_name="dmonitoringd", proc_name="dmonitoringd",
pub_sub={ pub_sub={
"driverState": ["dMonitoringState"], "driverState": ["driverMonitoringState"],
"liveCalibration": [], "carState": [], "modelV2": [], "controlsState": [], "liveCalibration": [], "carState": [], "modelV2": [], "controlsState": [],
}, },
ignore=["logMonoTime", "valid"], ignore=["logMonoTime", "valid"],

@ -1 +1 @@
2ecf7c5d8816aaf70dc42a5ec37a0106fcd42799 3d94188da5fdb35c18664d32ac3c720b221a78c7

@ -580,7 +580,7 @@ if __name__ == "__main__":
# Start unlogger # Start unlogger
print("Start unlogger") print("Start unlogger")
unlogger_cmd = [os.path.join(BASEDIR, 'tools/replay/unlogger.py'), route, '/tmp'] unlogger_cmd = [os.path.join(BASEDIR, 'tools/replay/unlogger.py'), route, '/tmp']
disable_socks = 'frame,encodeIdx,plan,pathPlan,liveLongitudinalMpc,radarState,controlsState,liveTracks,liveMpc,sendcan,carState,carControl,carEvents,carParams' disable_socks = 'frame,encodeIdx,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 unlogger = subprocess.Popen(unlogger_cmd + ['--disable', disable_socks, '--no-interactive'], preexec_fn=os.setsid) # pylint: disable=subprocess-popen-preexec-fn
print("Check sockets") print("Check sockets")
@ -589,7 +589,7 @@ if __name__ == "__main__":
if (route not in passive_routes) and (route not in forced_dashcam_routes) and has_camera: if (route not in passive_routes) and (route not in forced_dashcam_routes) and has_camera:
extra_socks.append("sendcan") extra_socks.append("sendcan")
if route not in passive_routes: if route not in passive_routes:
extra_socks.append("pathPlan") extra_socks.append("lateralPlan")
recvd_socks = wait_for_sockets(tested_socks + extra_socks, timeout=30) recvd_socks = wait_for_sockets(tested_socks + extra_socks, timeout=30)
failures = [s for s in tested_socks + extra_socks if s not in recvd_socks] failures = [s for s in tested_socks + extra_socks if s not in recvd_socks]

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

@ -43,7 +43,7 @@ class PowerMonitoring:
now = sec_since_boot() now = sec_since_boot()
# If health is None, we're probably not in a car, so we don't care # If health is None, we're probably not in a car, so we don't care
if health is None or health.health.hwType == log.HealthData.HwType.unknown: if health is None or health.health.pandaType == log.HealthData.PandaType.unknown:
with self.integration_lock: with self.integration_lock:
self.last_measurement_time = None self.last_measurement_time = None
self.next_pulsed_measurement_time = None self.next_pulsed_measurement_time = None
@ -77,7 +77,7 @@ class PowerMonitoring:
self.last_measurement_time = now self.last_measurement_time = now
else: else:
# No ignition, we integrate the offroad power used by the device # No ignition, we integrate the offroad power used by the device
is_uno = health.health.hwType == log.HealthData.HwType.uno is_uno = health.health.pandaType == log.HealthData.PandaType.uno
# Get current power draw somehow # Get current power draw somehow
current_power = HARDWARE.get_current_power_draw() current_power = HARDWARE.get_current_power_draw()
if current_power is not None: if current_power is not None:

@ -21,10 +21,10 @@ with patch("common.realtime.sec_since_boot", new=mock_sec_since_boot):
CAR_CHARGING_RATE_W, VBATT_PAUSE_CHARGING CAR_CHARGING_RATE_W, VBATT_PAUSE_CHARGING
TEST_DURATION_S = 50 TEST_DURATION_S = 50
ALL_PANDA_TYPES = [(hw_type,) for hw_type in [log.HealthData.HwType.whitePanda, ALL_PANDA_TYPES = [(hw_type,) for hw_type in [log.HealthData.PandaType.whitePanda,
log.HealthData.HwType.greyPanda, log.HealthData.PandaType.greyPanda,
log.HealthData.HwType.blackPanda, log.HealthData.PandaType.blackPanda,
log.HealthData.HwType.uno]] log.HealthData.PandaType.uno]]
def pm_patch(name, value, constant=False): def pm_patch(name, value, constant=False):
if constant: if constant:
@ -39,7 +39,7 @@ class TestPowerMonitoring(unittest.TestCase):
def mock_health(self, ignition, hw_type, car_voltage=12): def mock_health(self, ignition, hw_type, car_voltage=12):
health = messaging.new_message('health') health = messaging.new_message('health')
health.health.hwType = hw_type health.health.pandaType = hw_type
health.health.voltage = car_voltage * 1e3 health.health.voltage = car_voltage * 1e3
health.health.ignitionLine = ignition health.health.ignitionLine = ignition
health.health.ignitionCan = False health.health.ignitionCan = False
@ -179,7 +179,7 @@ class TestPowerMonitoring(unittest.TestCase):
pm_patch("HARDWARE.get_battery_status", "Discharging"), pm_patch("HARDWARE.get_current_power_draw", None): pm_patch("HARDWARE.get_battery_status", "Discharging"), pm_patch("HARDWARE.get_current_power_draw", None):
pm = PowerMonitoring() pm = PowerMonitoring()
pm.car_battery_capacity_uWh = CAR_BATTERY_CAPACITY_uWh pm.car_battery_capacity_uWh = CAR_BATTERY_CAPACITY_uWh
health = self.mock_health(False, log.HealthData.HwType.uno, car_voltage=(VBATT_PAUSE_CHARGING - 1)) health = self.mock_health(False, log.HealthData.PandaType.uno, car_voltage=(VBATT_PAUSE_CHARGING - 1))
for i in range(TEST_TIME): for i in range(TEST_TIME):
pm.calculate(health) pm.calculate(health)
if i % 10 == 0: if i % 10 == 0:
@ -196,7 +196,7 @@ class TestPowerMonitoring(unittest.TestCase):
pm_patch("HARDWARE.get_battery_status", "Discharging"), pm_patch("HARDWARE.get_current_power_draw", None): pm_patch("HARDWARE.get_battery_status", "Discharging"), pm_patch("HARDWARE.get_current_power_draw", None):
pm = PowerMonitoring() pm = PowerMonitoring()
pm.car_battery_capacity_uWh = CAR_BATTERY_CAPACITY_uWh pm.car_battery_capacity_uWh = CAR_BATTERY_CAPACITY_uWh
health = self.mock_health(True, log.HealthData.HwType.uno, car_voltage=(VBATT_PAUSE_CHARGING - 1)) health = self.mock_health(True, log.HealthData.PandaType.uno, car_voltage=(VBATT_PAUSE_CHARGING - 1))
for i in range(TEST_TIME): for i in range(TEST_TIME):
pm.calculate(health) pm.calculate(health)
if i % 10 == 0: if i % 10 == 0:

@ -208,7 +208,7 @@ def thermald_thread():
usb_power = health.health.usbPowerMode != log.HealthData.UsbPowerMode.client usb_power = health.health.usbPowerMode != log.HealthData.UsbPowerMode.client
# If we lose connection to the panda, wait 5 seconds before going offroad # If we lose connection to the panda, wait 5 seconds before going offroad
if health.health.hwType == log.HealthData.HwType.unknown: if health.health.pandaType == log.HealthData.PandaType.unknown:
no_panda_cnt += 1 no_panda_cnt += 1
if no_panda_cnt > DISCONNECT_TIMEOUT / DT_TRML: if no_panda_cnt > DISCONNECT_TIMEOUT / DT_TRML:
if startup_conditions["ignition"]: if startup_conditions["ignition"]:
@ -219,8 +219,8 @@ def thermald_thread():
startup_conditions["ignition"] = health.health.ignitionLine or health.health.ignitionCan startup_conditions["ignition"] = health.health.ignitionLine or health.health.ignitionCan
# Setup fan handler on first connect to panda # Setup fan handler on first connect to panda
if handle_fan is None and health.health.hwType != log.HealthData.HwType.unknown: if handle_fan is None and health.health.pandaType != log.HealthData.PandaType.unknown:
is_uno = health.health.hwType == log.HealthData.HwType.uno is_uno = health.health.pandaType == log.HealthData.PandaType.uno
if (not EON) or is_uno: if (not EON) or is_uno:
cloudlog.info("Setting up UNO fan handler") cloudlog.info("Setting up UNO fan handler")
@ -232,8 +232,8 @@ def thermald_thread():
# Handle disconnect # Handle disconnect
if health_prev is not None: if health_prev is not None:
if health.health.hwType == log.HealthData.HwType.unknown and \ if health.health.pandaType == log.HealthData.PandaType.unknown and \
health_prev.health.hwType != log.HealthData.HwType.unknown: health_prev.health.pandaType != log.HealthData.PandaType.unknown:
params.panda_disconnect() params.panda_disconnect()
health_prev = health health_prev = health
@ -245,9 +245,9 @@ def thermald_thread():
except Exception: except Exception:
cloudlog.exception("Error getting network status") cloudlog.exception("Error getting network status")
msg.thermal.freeSpace = get_available_percent(default=100.0) / 100.0 msg.thermal.freeSpacePercent = get_available_percent(default=100.0) / 100.0
msg.thermal.memUsedPercent = int(round(psutil.virtual_memory().percent)) msg.thermal.memoryUsagePercent = int(round(psutil.virtual_memory().percent))
msg.thermal.cpuPerc = int(round(psutil.cpu_percent())) msg.thermal.cpuUsagePercent = int(round(psutil.cpu_percent()))
msg.thermal.networkType = network_type msg.thermal.networkType = network_type
msg.thermal.networkStrength = network_strength msg.thermal.networkStrength = network_strength
msg.thermal.batteryPercent = HARDWARE.get_battery_capacity() msg.thermal.batteryPercent = HARDWARE.get_battery_capacity()
@ -271,7 +271,7 @@ def thermald_thread():
if handle_fan is not None: if handle_fan is not None:
fan_speed = handle_fan(max_cpu_temp, bat_temp, fan_speed, startup_conditions["ignition"]) fan_speed = handle_fan(max_cpu_temp, bat_temp, fan_speed, startup_conditions["ignition"])
msg.thermal.fanSpeed = fan_speed msg.thermal.fanSpeedRpmDesired = fan_speed
# If device is offroad we want to cool down before going onroad # If device is offroad we want to cool down before going onroad
# since going onroad increases load and can make temps go over 107 # 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"])) 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 # with 2% left, we killall, otherwise the phone will take a long time to boot
startup_conditions["free_space"] = msg.thermal.freeSpace > 0.02 startup_conditions["free_space"] = msg.thermal.freeSpacePercent > 0.02
startup_conditions["completed_training"] = params.get("CompletedTrainingVersion") == training_version or \ startup_conditions["completed_training"] = params.get("CompletedTrainingVersion") == training_version or \
(current_branch in ['dashcam', 'dashcam-staging']) (current_branch in ['dashcam', 'dashcam-staging'])
startup_conditions["not_driver_view"] = not params.get("IsDriverViewEnabled") == b"1" startup_conditions["not_driver_view"] = not params.get("IsDriverViewEnabled") == b"1"
@ -357,8 +357,8 @@ def thermald_thread():
startup_conditions["device_temp_good"] = thermal_status < ThermalStatus.danger startup_conditions["device_temp_good"] = thermal_status < ThermalStatus.danger
set_offroad_alert_if_changed("Offroad_TemperatureTooHigh", (not startup_conditions["device_temp_good"])) set_offroad_alert_if_changed("Offroad_TemperatureTooHigh", (not startup_conditions["device_temp_good"]))
startup_conditions["hardware_supported"] = health is not None and health.health.hwType not in [log.HealthData.HwType.whitePanda, startup_conditions["hardware_supported"] = health is not None and health.health.pandaType not in [log.HealthData.PandaType.whitePanda,
log.HealthData.HwType.greyPanda] log.HealthData.PandaType.greyPanda]
set_offroad_alert_if_changed("Offroad_HardwareUnsupported", health is not None and not startup_conditions["hardware_supported"]) set_offroad_alert_if_changed("Offroad_HardwareUnsupported", health is not None and not startup_conditions["hardware_supported"])
# Handle offroad/onroad transition # Handle offroad/onroad transition
@ -399,7 +399,7 @@ def thermald_thread():
msg.thermal.chargingError = current_filter.x > 0. and msg.thermal.batteryPercent < 90 # if current is positive, then battery is being discharged 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.started = started_ts is not None
msg.thermal.startedTs = int(1e9*(started_ts or 0)) msg.thermal.startedMonoTime = int(1e9*(started_ts or 0))
msg.thermal.thermalStatus = thermal_status msg.thermal.thermalStatus = thermal_status
pm.send("thermal", msg) pm.send("thermal", msg)

@ -163,7 +163,7 @@ int main(int argc, char* argv[]) {
} }
// up one notch every 5 m/s // up one notch every 5 m/s
s->sound->setVolume(fmin(MAX_VOLUME, MIN_VOLUME + s->scene.controls_state.getVEgo() / 5)); s->sound->setVolume(fmin(MAX_VOLUME, MIN_VOLUME + s->scene.car_state.getVEgo() / 5));
// set brightness // set brightness
float clipped_brightness = fmin(512, (s->light_sensor*brightness_m) + brightness_b); float clipped_brightness = fmin(512, (s->light_sensor*brightness_m) + brightness_b);

@ -233,7 +233,7 @@ static void ui_draw_vision_maxspeed(UIState *s) {
} }
static void ui_draw_vision_speed(UIState *s) { static void ui_draw_vision_speed(UIState *s) {
const float speed = std::max(0.0, s->scene.controls_state.getVEgo() * (s->is_metric ? 3.6 : 2.2369363)); const float speed = std::max(0.0, s->scene.car_state.getVEgo() * (s->is_metric ? 3.6 : 2.2369363));
const std::string speed_str = std::to_string((int)std::nearbyint(speed)); const std::string speed_str = std::to_string((int)std::nearbyint(speed));
nvgTextAlign(s->vg, NVG_ALIGN_CENTER | NVG_ALIGN_BASELINE); nvgTextAlign(s->vg, NVG_ALIGN_CENTER | NVG_ALIGN_BASELINE);
ui_draw_text(s, s->viz_rect.centerX(), 240, speed_str.c_str(), 96 * 2.5, COLOR_WHITE, "sans-bold"); ui_draw_text(s, s->viz_rect.centerX(), 240, speed_str.c_str(), 96 * 2.5, COLOR_WHITE, "sans-bold");
@ -244,12 +244,7 @@ static void ui_draw_vision_event(UIState *s) {
const int viz_event_w = 220; const int viz_event_w = 220;
const int viz_event_x = s->viz_rect.right() - (viz_event_w + bdr_s*2); const int viz_event_x = s->viz_rect.right() - (viz_event_w + bdr_s*2);
const int viz_event_y = s->viz_rect.y + (bdr_s*1.5); const int viz_event_y = s->viz_rect.y + (bdr_s*1.5);
if (s->scene.controls_state.getDecelForModel() && s->scene.controls_state.getEnabled()) { if (s->scene.controls_state.getEngageable()) {
// draw winding road sign
const int img_turn_size = 160*1.5;
const Rect rect = {viz_event_x - (img_turn_size / 4), viz_event_y + bdr_s - 25, img_turn_size, img_turn_size};
ui_draw_image(s, rect, "trafficSign_turn", 1.0f);
} else if (s->scene.controls_state.getEngageable()) {
// draw steering wheel // draw steering wheel
const int bg_wheel_size = 96; const int bg_wheel_size = 96;
const int bg_wheel_x = viz_event_x + (viz_event_w-bg_wheel_size); const int bg_wheel_x = viz_event_x + (viz_event_w-bg_wheel_size);

@ -118,7 +118,7 @@ static void draw_panda_metric(UIState *s) {
int panda_severity = 0; int panda_severity = 0;
std::string panda_message = "VEHICLE\nONLINE"; std::string panda_message = "VEHICLE\nONLINE";
if (s->scene.hwType == cereal::HealthData::HwType::UNKNOWN) { if (s->scene.pandaType == cereal::HealthData::PandaType::UNKNOWN) {
panda_severity = 2; panda_severity = 2;
panda_message = "NO\nPANDA"; panda_message = "NO\nPANDA";
} }

@ -41,7 +41,7 @@ static void ui_init_vision(UIState *s) {
void ui_init(UIState *s) { void ui_init(UIState *s) {
s->sm = new SubMaster({"modelV2", "controlsState", "uiLayoutState", "liveCalibration", "radarState", "thermal", "frame", s->sm = new SubMaster({"modelV2", "controlsState", "uiLayoutState", "liveCalibration", "radarState", "thermal", "frame",
"health", "carParams", "driverState", "dMonitoringState", "sensorEvents"}); "health", "carParams", "driverState", "driverMonitoringState", "sensorEvents", "carState"});
s->started = false; s->started = false;
s->status = STATUS_OFFROAD; s->status = STATUS_OFFROAD;
@ -127,6 +127,9 @@ static void update_sockets(UIState *s) {
if (s->started && sm.updated("controlsState")) { if (s->started && sm.updated("controlsState")) {
scene.controls_state = sm["controlsState"].getControlsState(); scene.controls_state = sm["controlsState"].getControlsState();
} }
if (sm.updated("carState")) {
scene.car_state = sm["carState"].getCarState();
}
if (sm.updated("radarState")) { if (sm.updated("radarState")) {
auto radar_state = sm["radarState"].getRadarState(); auto radar_state = sm["radarState"].getRadarState();
const auto line = sm["modelV2"].getModelV2().getPosition(); const auto line = sm["modelV2"].getModelV2().getPosition();
@ -153,10 +156,10 @@ static void update_sockets(UIState *s) {
} }
if (sm.updated("health")) { if (sm.updated("health")) {
auto health = sm["health"].getHealth(); auto health = sm["health"].getHealth();
scene.hwType = health.getHwType(); scene.pandaType = health.getPandaType();
s->ignition = health.getIgnitionLine() || health.getIgnitionCan(); s->ignition = health.getIgnitionLine() || health.getIgnitionCan();
} else if ((s->sm->frame - s->sm->rcv_frame("health")) > 5*UI_FREQ) { } else if ((s->sm->frame - s->sm->rcv_frame("health")) > 5*UI_FREQ) {
scene.hwType = cereal::HealthData::HwType::UNKNOWN; scene.pandaType = cereal::HealthData::PandaType::UNKNOWN;
} }
if (sm.updated("carParams")) { if (sm.updated("carParams")) {
s->longitudinal_control = sm["carParams"].getCarParams().getOpenpilotLongitudinalControl(); s->longitudinal_control = sm["carParams"].getCarParams().getOpenpilotLongitudinalControl();
@ -164,11 +167,10 @@ static void update_sockets(UIState *s) {
if (sm.updated("driverState")) { if (sm.updated("driverState")) {
scene.driver_state = sm["driverState"].getDriverState(); scene.driver_state = sm["driverState"].getDriverState();
} }
if (sm.updated("dMonitoringState")) { if (sm.updated("driverMonitoringState")) {
scene.dmonitoring_state = sm["dMonitoringState"].getDMonitoringState(); scene.dmonitoring_state = sm["driverMonitoringState"].getDriverMonitoringState();
scene.is_rhd = scene.dmonitoring_state.getIsRHD(); scene.frontview = !s->ignition;
scene.frontview = scene.dmonitoring_state.getIsPreview(); } else if (scene.frontview && (sm.frame - sm.rcv_frame("driverMonitoringState")) > UI_FREQ/2) {
} else if (scene.frontview && (sm.frame - sm.rcv_frame("dMonitoringState")) > UI_FREQ/2) {
scene.frontview = false; scene.frontview = false;
} }
if (sm.updated("sensorEvents")) { if (sm.updated("sensorEvents")) {
@ -283,6 +285,7 @@ static void update_status(UIState *s) {
s->status = STATUS_DISENGAGED; s->status = STATUS_DISENGAGED;
s->started_frame = s->sm->frame; s->started_frame = s->sm->frame;
read_param(&s->scene.is_rhd, "IsRHD");
s->active_app = cereal::UiLayoutState::App::NONE; s->active_app = cereal::UiLayoutState::App::NONE;
s->sidebar_collapsed = true; s->sidebar_collapsed = true;
s->scene.alert_size = cereal::ControlsState::AlertSize::NONE; s->scene.alert_size = cereal::ControlsState::AlertSize::NONE;

@ -117,14 +117,15 @@ typedef struct UIScene {
float alert_blinking_rate; float alert_blinking_rate;
cereal::ControlsState::AlertSize alert_size; cereal::ControlsState::AlertSize alert_size;
cereal::HealthData::HwType hwType; cereal::HealthData::PandaType pandaType;
NetStatus athenaStatus; NetStatus athenaStatus;
cereal::ThermalData::Reader thermal; cereal::ThermalData::Reader thermal;
cereal::RadarState::LeadData::Reader lead_data[2]; cereal::RadarState::LeadData::Reader lead_data[2];
cereal::CarState::Reader car_state;
cereal::ControlsState::Reader controls_state; cereal::ControlsState::Reader controls_state;
cereal::DriverState::Reader driver_state; cereal::DriverState::Reader driver_state;
cereal::DMonitoringState::Reader dmonitoring_state; cereal::DriverMonitoringState::Reader dmonitoring_state;
// modelV2 // modelV2
float lane_line_probs[4]; float lane_line_probs[4];

@ -7,7 +7,7 @@ import cereal.messaging as messaging
from selfdrive.car.car_helpers import get_car, get_one_can from selfdrive.car.car_helpers import get_car, get_one_can
from selfdrive.boardd.boardd import can_list_to_can_capnp from selfdrive.boardd.boardd import can_list_to_can_capnp
HwType = log.HealthData.HwType PandaType = log.HealthData.PandaType
def steer_thread(): def steer_thread():

@ -4,8 +4,9 @@
#include <capnp/schema.h> #include <capnp/schema.h>
// include the dynamic struct // include the dynamic struct
#include "cereal/gen/cpp/car.capnp.c++"
#include "cereal/gen/cpp/log.capnp.c++" #include "cereal/gen/cpp/log.capnp.c++"
#include "cereal/gen/cpp/car.capnp.c++"
#include "cereal/gen/cpp/legacy.capnp.c++"
#include "cereal/services.h" #include "cereal/services.h"
#include "Unlogger.hpp" #include "Unlogger.hpp"

@ -165,6 +165,7 @@ void Window::paintEvent(QPaintEvent *event) {
//p.drawRect(0, 0, 600, 100); //p.drawRect(0, 0, 600, 100);
// TODO: we really don't have to redraw this every time, only on updates to events // TODO: we really don't have to redraw this every time, only on updates to events
float vEgo = 0.;
int this_event_size = events.size(); int this_event_size = events.size();
if (last_event_size != this_event_size) { if (last_event_size != this_event_size) {
if (px != NULL) delete px; if (px != NULL) delete px;
@ -179,10 +180,11 @@ void Window::paintEvent(QPaintEvent *event) {
for (auto e : events) { for (auto e : events) {
auto type = e.which(); auto type = e.which();
//printf("%lld %d\n", e.getLogMonoTime()-t0, type); //printf("%lld %d\n", e.getLogMonoTime()-t0, type);
if (type == cereal::Event::CONTROLS_STATE) { if (type == cereal::Event::CAR_STATE) {
vEgo = e.getCarState().getVEgo();
} else if (type == cereal::Event::CONTROLS_STATE) {
auto controlsState = e.getControlsState(); auto controlsState = e.getControlsState();
uint64_t t = (e.getLogMonoTime()-t0); uint64_t t = (e.getLogMonoTime()-t0);
float vEgo = controlsState.getVEgo();
int enabled = controlsState.getState() == cereal::ControlsState::OpenpilotState::ENABLED; int enabled = controlsState.getState() == cereal::ControlsState::OpenpilotState::ENABLED;
int rt = timeToPixel(t); // 250 ms per pixel int rt = timeToPixel(t); // 250 ms per pixel
if (rt != lt) { if (rt != lt) {

@ -70,7 +70,7 @@ def ui_thread(addr, frame_address):
frame = messaging.sub_sock('frame', addr=addr, conflate=True) frame = messaging.sub_sock('frame', addr=addr, conflate=True)
sm = messaging.SubMaster(['carState', 'plan', 'carControl', 'radarState', 'liveCalibration', 'controlsState', sm = messaging.SubMaster(['carState', 'plan', 'carControl', 'radarState', 'liveCalibration', 'controlsState',
'liveTracks', 'model', 'liveMpc', 'liveParameters', 'pathPlan', 'frame'], addr=addr) 'liveTracks', 'model', 'liveMpc', 'liveParameters', 'lateralPlan', 'frame'], addr=addr)
calibration = None calibration = None
img = np.zeros((480, 640, 3), dtype='uint8') img = np.zeros((480, 640, 3), dtype='uint8')
@ -184,7 +184,7 @@ def ui_thread(addr, frame_address):
if len(sm['model'].path.poly) > 0: if len(sm['model'].path.poly) > 0:
model_data = extract_model_data(sm['model']) model_data = extract_model_data(sm['model'])
plot_model(model_data, VM, sm['controlsState'].vEgo, sm['controlsState'].curvature, imgw, calibration, plot_model(model_data, VM, sm['controlsState'].vEgo, sm['controlsState'].curvature, imgw, calibration,
top_down, np.array(sm['pathPlan'].dPolyDEPRECATED)) top_down, np.array(sm['lateralPlan'].dPolyDEPRECATED))
# MPC # MPC
if sm.updated['liveMpc']: if sm.updated['liveMpc']:

@ -88,7 +88,7 @@ def health_function():
dat.valid = True dat.valid = True
dat.health = { dat.health = {
'ignitionLine': True, 'ignitionLine': True,
'hwType': "blackPanda", 'pandaType': "blackPanda",
'controlsAllowed': True, 'controlsAllowed': True,
'safetyModel': 'hondaNidec' 'safetyModel': 'hondaNidec'
} }
@ -104,7 +104,7 @@ def fake_gps():
time.sleep(0.01) time.sleep(0.01)
def fake_driver_monitoring(): def fake_driver_monitoring():
pm = messaging.PubMaster(['driverState','dMonitoringState']) pm = messaging.PubMaster(['driverState','driverMonitoringState'])
while 1: while 1:
# dmonitoringmodeld output # dmonitoringmodeld output
@ -113,14 +113,14 @@ def fake_driver_monitoring():
pm.send('driverState', dat) pm.send('driverState', dat)
# dmonitoringd output # dmonitoringd output
dat = messaging.new_message('dMonitoringState') dat = messaging.new_message('driverMonitoringState')
dat.dMonitoringState = { dat.driverMonitoringState = {
"faceDetected": True, "faceDetected": True,
"isDistracted": False, "isDistracted": False,
"awarenessStatus": 1., "awarenessStatus": 1.,
"isRHD": False, "isRHD": False,
} }
pm.send('dMonitoringState', dat) pm.send('driverMonitoringState', dat)
time.sleep(DT_DMON) time.sleep(DT_DMON)

Loading…
Cancel
Save