diff --git a/cereal b/cereal index 266fc01950..b19a3ed38d 160000 --- a/cereal +++ b/cereal @@ -1 +1 @@ -Subproject commit 266fc0195003a50faa95d2905839abf8409b6bcc +Subproject commit b19a3ed38de1b712f744e4fdeed0b1f87801bf15 diff --git a/release/files_common b/release/files_common index 26e0fe422c..93213d8ebf 100644 --- a/release/files_common +++ b/release/files_common @@ -225,10 +225,10 @@ selfdrive/controls/lib/latcontrol_pid.py selfdrive/controls/lib/latcontrol_indi.py selfdrive/controls/lib/latcontrol_lqr.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/pid.py -selfdrive/controls/lib/planner.py +selfdrive/controls/lib/longitudinal_planner.py selfdrive/controls/lib/radar_helpers.py selfdrive/controls/lib/vehicle_model.py selfdrive/controls/lib/speed_smoother.py @@ -475,6 +475,7 @@ rednose/** cereal/.gitignore cereal/__init__.py cereal/car.capnp +cereal/legacy.capnp cereal/log.capnp cereal/services.py cereal/service_list.yaml diff --git a/selfdrive/boardd/boardd.cc b/selfdrive/boardd/boardd.cc index a8ddecd186..0b40eac307 100644 --- a/selfdrive/boardd/boardd.cc +++ b/selfdrive/boardd/boardd.cc @@ -273,7 +273,7 @@ void can_health_thread(bool spoofing_started) { MessageBuilder msg; auto healthData = msg.initEvent().initHealth(); - healthData.setHwType(cereal::HealthData::HwType::UNKNOWN); + healthData.setPandaType(cereal::HealthData::PandaType::UNKNOWN); pm.send("health", msg); util::sleep_for(500); } @@ -360,7 +360,7 @@ void can_health_thread(bool spoofing_started) { healthData.setCanSendErrs(health.can_send_errs); healthData.setCanFwdErrs(health.can_fwd_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.setSafetyModel(cereal::CarParams::SafetyModel(health.safety_model)); healthData.setFanSpeedRpm(fan_speed_rpm); @@ -420,10 +420,10 @@ void hardware_control_thread() { #endif // 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")){ // 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){ panda->set_fan_speed(fan_speed); prev_fan_speed = fan_speed; diff --git a/selfdrive/boardd/panda.cc b/selfdrive/boardd/panda.cc index 4d01c6a954..2fceb3c64c 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::HwType::GREY_PANDA) || - (hw_type == cereal::HealthData::HwType::BLACK_PANDA) || - (hw_type == cereal::HealthData::HwType::UNO) || - (hw_type == cereal::HealthData::HwType::DOS); - has_rtc = (hw_type == cereal::HealthData::HwType::UNO) || - (hw_type == cereal::HealthData::HwType::DOS); + (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); return; @@ -186,11 +186,11 @@ void Panda::set_unsafe_mode(uint16_t unsafe_mode) { 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}; 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){ diff --git a/selfdrive/boardd/panda.h b/selfdrive/boardd/panda.h index b1f5e70879..a96af8a6af 100644 --- a/selfdrive/boardd/panda.h +++ b/selfdrive/boardd/panda.h @@ -53,7 +53,7 @@ class Panda { ~Panda(); std::atomic 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 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::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_unsafe_mode(uint16_t unsafe_mode); void set_rtc(struct tm sys_time); diff --git a/selfdrive/car/honda/interface.py b/selfdrive/car/honda/interface.py index 73dd2ca398..97d4b25674 100755 --- a/selfdrive/car/honda/interface.py +++ b/selfdrive/car/honda/interface.py @@ -8,7 +8,7 @@ from selfdrive.config import Conversions as CV from selfdrive.controls.lib.events import ET 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.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 A_ACC_MAX = max(_A_CRUISE_MAX_V_FOLLOWING) diff --git a/selfdrive/car/interfaces.py b/selfdrive/car/interfaces.py index 746dabb9eb..e49817e157 100644 --- a/selfdrive/car/interfaces.py +++ b/selfdrive/car/interfaces.py @@ -53,7 +53,6 @@ class CarInterfaceBase(): def get_std_params(candidate, fingerprint): ret = car.CarParams.new_message() ret.carFingerprint = candidate - ret.isPandaBlack = True # TODO: deprecate this field # standard ALC params ret.steerControlType = car.CarParams.SteerControlType.torque diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 17832003c5..9345425081 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -19,7 +19,7 @@ from selfdrive.controls.lib.latcontrol_lqr import LatControlLQR from selfdrive.controls.lib.events import Events, ET from selfdrive.controls.lib.alertmanager import AlertManager 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.hardware import HARDWARE, TICI @@ -34,11 +34,11 @@ IGNORE_PROCESSES = set(["rtshield", "uploader", "deleter", "loggerd", "logmessag ThermalStatus = log.ThermalData.ThermalStatus State = log.ControlsState.OpenpilotState -HwType = log.HealthData.HwType -LongitudinalPlanSource = log.Plan.LongitudinalPlanSource -Desire = log.PathPlan.Desire -LaneChangeState = log.PathPlan.LaneChangeState -LaneChangeDirection = log.PathPlan.LaneChangeDirection +PandaType = log.HealthData.PandaType +LongitudinalPlanSource = log.LongitudinalPlan.LongitudinalPlanSource +Desire = log.LateralPlan.Desire +LaneChangeState = log.LateralPlan.LaneChangeState +LaneChangeDirection = log.LateralPlan.LaneChangeDirection EventName = car.CarEvent.EventName @@ -56,8 +56,8 @@ class Controls: if self.sm is None: ignore = ['ubloxRaw', 'frontFrame', 'managerState'] if SIMULATION else None self.sm = messaging.SubMaster(['thermal', 'health', 'modelV2', 'liveCalibration', 'ubloxRaw', - 'dMonitoringState', 'plan', 'pathPlan', 'liveLocationKalman', - 'frame', 'frontFrame', 'managerState'], ignore_alive=ignore) + 'driverMonitoringState', 'longitudinalPlan', 'lateralPlan', 'liveLocationKalman', + 'frame', 'frontFrame', 'managerState', 'liveParameters', 'radarState'], ignore_alive=ignore) self.can_sock = can_sock if can_sock is None: @@ -127,10 +127,10 @@ class Controls: self.logged_comm_issue = False self.sm['liveCalibration'].calStatus = Calibration.CALIBRATED - self.sm['thermal'].freeSpace = 1. - self.sm['dMonitoringState'].events = [] - self.sm['dMonitoringState'].awarenessStatus = 1. - self.sm['dMonitoringState'].faceDetected = False + self.sm['thermal'].freeSpacePercent = 1. + self.sm['driverMonitoringState'].events = [] + self.sm['driverMonitoringState'].awarenessStatus = 1. + self.sm['driverMonitoringState'].faceDetected = False self.startup_event = get_startup_event(car_recognized, controller_available) @@ -150,7 +150,7 @@ class Controls: self.events.clear() 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 if self.startup_event is not None: @@ -163,10 +163,10 @@ class Controls: self.events.add(EventName.lowBattery) if self.sm['thermal'].thermalStatus >= ThermalStatus.red: 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 self.events.add(EventName.outOfSpace) - if self.sm['thermal'].memUsedPercent > 90: + if self.sm['thermal'].memoryUsagePercent > 90: self.events.add(EventName.lowMemory) # Check if all manager processes are running @@ -175,8 +175,8 @@ class Controls: self.events.add(EventName.processNotRunning) # Alert if fan isn't spinning for 5 seconds - if self.sm['health'].hwType in [HwType.uno, HwType.dos]: - if self.sm['health'].fanSpeedRpm == 0 and self.sm['thermal'].fanSpeed > 50: + if self.sm['health'].pandaType in [PandaType.uno, PandaType.dos]: + 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: self.events.add(EventName.fanMalfunction) else: @@ -191,8 +191,8 @@ class Controls: self.events.add(EventName.calibrationInvalid) # Handle lane change - if self.sm['pathPlan'].laneChangeState == LaneChangeState.preLaneChange: - direction = self.sm['pathPlan'].laneChangeDirection + if self.sm['lateralPlan'].laneChangeState == LaneChangeState.preLaneChange: + direction = self.sm['lateralPlan'].laneChangeDirection if (CS.leftBlindspot and direction == LaneChangeDirection.left) or \ (CS.rightBlindspot and direction == LaneChangeDirection.right): self.events.add(EventName.laneChangeBlocked) @@ -201,7 +201,7 @@ class Controls: self.events.add(EventName.preLaneChangeLeft) else: self.events.add(EventName.preLaneChangeRight) - elif self.sm['pathPlan'].laneChangeState in [LaneChangeState.laneChangeStarting, + elif self.sm['lateralPlan'].laneChangeState in [LaneChangeState.laneChangeStarting, LaneChangeState.laneChangeFinishing]: self.events.add(EventName.laneChange) @@ -211,9 +211,10 @@ class Controls: self.mismatch_counter >= 200: self.events.add(EventName.controlsMismatch) - if not self.sm.alive['plan'] and self.sm.alive['pathPlan']: - # only plan not being received: radar not communicating - self.events.add(EventName.radarCommIssue) + if len(self.sm['radarState'].radarErrors): + self.events.add(EventName.radarFault) + elif not self.sm.valid['liveParameters']: + self.events.add(EventName.vehicleModelInvalid) elif not self.sm.all_alive_and_valid(): self.events.add(EventName.commIssue) if not self.logged_comm_issue: @@ -222,24 +223,18 @@ class Controls: else: self.logged_comm_issue = False - if not self.sm['pathPlan'].mpcSolutionValid: + if not self.sm['lateralPlan'].mpcSolutionValid: self.events.add(EventName.plannerError) 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 self.events.add(EventName.sensorDataInvalid) - if not self.sm['pathPlan'].paramsValid: - self.events.add(EventName.vehicleModelInvalid) if not self.sm['liveLocationKalman'].posenetOK: self.events.add(EventName.posenetInvalid) if not self.sm['liveLocationKalman'].deviceStable: 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: self.events.add(EventName.relayMalfunction) - if self.sm['plan'].fcw: + if self.sm['longitudinalPlan'].fcw: self.events.add(EventName.fcw) # TODO: fix simulator @@ -256,7 +251,7 @@ class Controls: self.events.add(EventName.modeldLagging) # 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: self.events.add(EventName.noTarget) @@ -370,8 +365,8 @@ class Controls: def state_control(self, CS): """Given the state, this function returns an actuators packet""" - plan = self.sm['plan'] - path_plan = self.sm['pathPlan'] + plan = self.sm['longitudinalPlan'] + path_plan = self.sm['lateralPlan'] actuators = car.CarControl.Actuators.new_message() @@ -384,7 +379,7 @@ class Controls: self.LaC.reset() 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 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)) 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.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.speedVisible = 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 - left_lane_visible = self.sm['pathPlan'].lProb > 0.5 + right_lane_visible = self.sm['lateralPlan'].rProb > 0.5 + left_lane_visible = self.sm['lateralPlan'].lProb > 0.5 CC.hudControl.rightLaneVisible = bool(right_lane_visible) CC.hudControl.leftLaneVisible = bool(left_lane_visible) @@ -472,10 +467,10 @@ class Controls: can_sends = self.CI.apply(CC) 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) - 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 dat = messaging.new_message('controlsState') @@ -488,17 +483,12 @@ class Controls: controlsState.alertBlinkingRate = self.AM.alert_rate controlsState.alertType = self.AM.alert_type controlsState.alertSound = self.AM.audible_alert - controlsState.driverMonitoringOn = self.sm['dMonitoringState'].faceDetected controlsState.canMonoTimes = list(CS.canMonoTimes) - controlsState.planMonoTime = self.sm.logMonoTime['plan'] - controlsState.pathPlanMonoTime = self.sm.logMonoTime['pathPlan'] + controlsState.longitudinalPlanMonoTime = self.sm.logMonoTime['longitudinalPlan'] + controlsState.lateralPlanMonoTime = self.sm.logMonoTime['lateralPlan'] controlsState.enabled = self.enabled 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.steerOverride = CS.steeringPressed controlsState.state = self.state controlsState.engageable = not self.events.any(ET.NO_ENTRY) controlsState.longControlState = self.LoC.long_control_state @@ -510,13 +500,8 @@ class Controls: controlsState.angleSteersDes = float(self.LaC.angle_steers_des) controlsState.vTargetLead = float(v_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.startMonoTime = int(start_time * 1e9) - controlsState.mapValid = self.sm['plan'].mapValid controlsState.forceDecel = bool(force_decel) controlsState.canErrorCounter = self.can_error_counter diff --git a/selfdrive/controls/lib/events.py b/selfdrive/controls/lib/events.py index c16804aa82..7cf010a2d1 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'].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( "Poor GPS reception", "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), }, - 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: { ET.SOFT_DISABLE: SoftDisableAlert("Radar Error: Restart the Car"), ET.NO_ENTRY : NoEntryAlert("Radar Error: Restart the Car"), diff --git a/selfdrive/controls/lib/lane_planner.py b/selfdrive/controls/lib/lane_planner.py index d57801cbf2..06ef69ce50 100644 --- a/selfdrive/controls/lib/lane_planner.py +++ b/selfdrive/controls/lib/lane_planner.py @@ -42,8 +42,8 @@ class LanePlanner: self.rll_std = md.laneLineStds[2] if len(md.meta.desireState): - self.l_lane_change_prob = md.meta.desireState[log.PathPlan.Desire.laneChangeLeft] - self.r_lane_change_prob = md.meta.desireState[log.PathPlan.Desire.laneChangeRight] + self.l_lane_change_prob = md.meta.desireState[log.LateralPlan.Desire.laneChangeLeft] + self.r_lane_change_prob = md.meta.desireState[log.LateralPlan.Desire.laneChangeRight] def get_d_path(self, v_ego, path_t, path_xyz): # Reduce reliance on lanelines that are too far apart or diff --git a/selfdrive/controls/lib/latcontrol_pid.py b/selfdrive/controls/lib/latcontrol_pid.py index aa069e81fc..173dcd58f7 100644 --- a/selfdrive/controls/lib/latcontrol_pid.py +++ b/selfdrive/controls/lib/latcontrol_pid.py @@ -25,7 +25,7 @@ class LatControlPID(): pid_log.active = False self.pid.reset() 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) self.pid.pos_limit = steers_max diff --git a/selfdrive/controls/lib/pathplanner.py b/selfdrive/controls/lib/lateral_planner.py similarity index 82% rename from selfdrive/controls/lib/pathplanner.py rename to selfdrive/controls/lib/lateral_planner.py index 73ccdf594b..d4afe4531c 100644 --- a/selfdrive/controls/lib/pathplanner.py +++ b/selfdrive/controls/lib/lateral_planner.py @@ -12,8 +12,8 @@ from common.params import Params import cereal.messaging as messaging from cereal import log -LaneChangeState = log.PathPlan.LaneChangeState -LaneChangeDirection = log.PathPlan.LaneChangeDirection +LaneChangeState = log.LateralPlan.LaneChangeState +LaneChangeDirection = log.LateralPlan.LaneChangeDirection LOG_MPC = os.environ.get('LOG_MPC', False) @@ -22,27 +22,27 @@ LANE_CHANGE_TIME_MAX = 10. DESIRES = { LaneChangeDirection.none: { - LaneChangeState.off: log.PathPlan.Desire.none, - LaneChangeState.preLaneChange: log.PathPlan.Desire.none, - LaneChangeState.laneChangeStarting: log.PathPlan.Desire.none, - LaneChangeState.laneChangeFinishing: log.PathPlan.Desire.none, + LaneChangeState.off: log.LateralPlan.Desire.none, + LaneChangeState.preLaneChange: log.LateralPlan.Desire.none, + LaneChangeState.laneChangeStarting: log.LateralPlan.Desire.none, + LaneChangeState.laneChangeFinishing: log.LateralPlan.Desire.none, }, LaneChangeDirection.left: { - LaneChangeState.off: log.PathPlan.Desire.none, - LaneChangeState.preLaneChange: log.PathPlan.Desire.none, - LaneChangeState.laneChangeStarting: log.PathPlan.Desire.laneChangeLeft, - LaneChangeState.laneChangeFinishing: log.PathPlan.Desire.laneChangeLeft, + LaneChangeState.off: log.LateralPlan.Desire.none, + LaneChangeState.preLaneChange: log.LateralPlan.Desire.none, + LaneChangeState.laneChangeStarting: log.LateralPlan.Desire.laneChangeLeft, + LaneChangeState.laneChangeFinishing: log.LateralPlan.Desire.laneChangeLeft, }, LaneChangeDirection.right: { - LaneChangeState.off: log.PathPlan.Desire.none, - LaneChangeState.preLaneChange: log.PathPlan.Desire.none, - LaneChangeState.laneChangeStarting: log.PathPlan.Desire.laneChangeRight, - LaneChangeState.laneChangeFinishing: log.PathPlan.Desire.laneChangeRight, + LaneChangeState.off: log.LateralPlan.Desire.none, + LaneChangeState.preLaneChange: log.LateralPlan.Desire.none, + LaneChangeState.laneChangeStarting: log.LateralPlan.Desire.laneChangeRight, + LaneChangeState.laneChangeFinishing: log.LateralPlan.Desire.laneChangeRight, }, } -class PathPlanner(): +class LateralPlanner(): def __init__(self, CP): self.LP = LanePlanner() @@ -57,7 +57,7 @@ class PathPlanner(): self.lane_change_timer = 0.0 self.lane_change_ll_prob = 1.0 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.plan_yaw = np.zeros((TRAJECTORY_SIZE,)) @@ -162,7 +162,7 @@ class PathPlanner(): self.desire = DESIRES[self.lane_change_direction][self.lane_change_state] # 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.rll_prob *= self.lane_change_ll_prob 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): 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.pathPlan.laneWidth = float(self.LP.lane_width) - plan_send.pathPlan.dPathPoints = [float(x) for x in self.y_pts] - plan_send.pathPlan.lProb = float(self.LP.lll_prob) - plan_send.pathPlan.rProb = float(self.LP.rll_prob) - plan_send.pathPlan.dProb = float(self.LP.d_prob) - - plan_send.pathPlan.angleSteers = float(self.desired_steering_wheel_angle_deg) - plan_send.pathPlan.rateSteers = float(self.desired_steering_wheel_angle_rate_deg) - plan_send.pathPlan.angleOffset = float(sm['liveParameters'].angleOffsetAverage) - plan_send.pathPlan.mpcSolutionValid = bool(plan_solution_valid) - plan_send.pathPlan.paramsValid = bool(sm['liveParameters'].valid) - - plan_send.pathPlan.desire = self.desire - plan_send.pathPlan.laneChangeState = self.lane_change_state - plan_send.pathPlan.laneChangeDirection = self.lane_change_direction - - pm.send('pathPlan', plan_send) + plan_send.lateralPlan.laneWidth = float(self.LP.lane_width) + plan_send.lateralPlan.dPathPoints = [float(x) for x in self.y_pts] + plan_send.lateralPlan.lProb = float(self.LP.lll_prob) + plan_send.lateralPlan.rProb = float(self.LP.rll_prob) + plan_send.lateralPlan.dProb = float(self.LP.d_prob) + + 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.mpcSolutionValid = bool(plan_solution_valid) + + plan_send.lateralPlan.desire = self.desire + plan_send.lateralPlan.laneChangeState = self.lane_change_state + plan_send.lateralPlan.laneChangeDirection = self.lane_change_direction + + pm.send('lateralPlan', plan_send) if LOG_MPC: dat = messaging.new_message('liveMpc') diff --git a/selfdrive/controls/lib/planner.py b/selfdrive/controls/lib/longitudinal_planner.py similarity index 84% rename from selfdrive/controls/lib/planner.py rename to selfdrive/controls/lib/longitudinal_planner.py index 92f38a30c3..b812bc48a0 100755 --- a/selfdrive/controls/lib/planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -5,7 +5,6 @@ from common.params import Params from common.numpy_fast import interp import cereal.messaging as messaging -from cereal import car from common.realtime import sec_since_boot from selfdrive.swaglog import cloudlog from selfdrive.config import Conversions as CV @@ -79,9 +78,6 @@ class Planner(): self.fcw_checker = FCWChecker() self.path_x = np.arange(192) - self.radar_dead = False - self.radar_fault = False - self.radar_can_error = False self.fcw = False self.params = Params() @@ -185,12 +181,6 @@ class Planner(): if self.fcw: 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 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 @@ -203,31 +193,25 @@ class Planner(): self.mpc1.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.plan.mdMonoTime = sm.logMonoTime['modelV2'] - plan_send.plan.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) + longitudinalPlan = plan_send.longitudinalPlan + longitudinalPlan.mdMonoTime = sm.logMonoTime['modelV2'] + longitudinalPlan.radarStateMonoTime = sm.logMonoTime['radarState'] - 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 - plan_send.plan.fcw = self.fcw + longitudinalPlan.processingDelay = (plan_send.logMonoTime / 1e9) - sm.rcv_time['radarState'] - pm.send('plan', plan_send) + pm.send('longitudinalPlan', plan_send) diff --git a/selfdrive/controls/plannerd.py b/selfdrive/controls/plannerd.py index d40ac97bf7..2d61a1b53a 100755 --- a/selfdrive/controls/plannerd.py +++ b/selfdrive/controls/plannerd.py @@ -3,9 +3,9 @@ from cereal import car from common.params import Params from common.realtime import Priority, config_realtime_process 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.pathplanner import PathPlanner +from selfdrive.controls.lib.lateral_planner import LateralPlanner import cereal.messaging as messaging @@ -18,7 +18,7 @@ def plannerd_thread(sm=None, pm=None): cloudlog.info("plannerd got CarParams: %s", CP.carName) PL = Planner(CP) - PP = PathPlanner(CP) + PP = LateralPlanner(CP) VM = VehicleModel(CP) @@ -27,7 +27,7 @@ def plannerd_thread(sm=None, pm=None): poll=['radarState', 'modelV2']) if pm is None: - pm = messaging.PubMaster(['plan', 'liveLongitudinalMpc', 'pathPlan', 'liveMpc']) + pm = messaging.PubMaster(['longitudinalPlan', 'liveLongitudinalMpc', 'lateralPlan', 'liveMpc']) sm['liveParameters'].valid = True sm['liveParameters'].sensorValid = True diff --git a/selfdrive/controls/radard.py b/selfdrive/controls/radard.py index ca75146062..8f23d9d731 100755 --- a/selfdrive/controls/radard.py +++ b/selfdrive/controls/radard.py @@ -100,8 +100,8 @@ class RadarD(): def update(self, sm, rr, enable_lead): self.current_time = 1e-9*max(sm.logMonoTime.values()) - if sm.updated['controlsState']: - self.v_ego = sm['controlsState'].vEgo + if sm.updated['carState']: + self.v_ego = sm['carState'].vEgo self.v_ego_hist.append(self.v_ego) if sm.updated['modelV2']: self.ready = True @@ -157,12 +157,12 @@ class RadarD(): # *** publish 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.mdMonoTime = sm.logMonoTime['modelV2'] radarState.canMonoTimes = list(rr.canMonoTimes) radarState.radarErrors = list(rr.errors) - radarState.controlsStateMonoTime = sm.logMonoTime['controlsState'] + radarState.carStateMonoTime = sm.logMonoTime['carState'] if enable_lead: 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: can_sock = messaging.sub_sock('can') if sm is None: - sm = messaging.SubMaster(['modelV2', 'controlsState']) + sm = messaging.SubMaster(['modelV2', 'carState']) if pm is None: pm = messaging.PubMaster(['radarState', 'liveTracks']) diff --git a/selfdrive/controls/tests/test_following_distance.py b/selfdrive/controls/tests/test_following_distance.py index 5968605dbe..b67d1b51bb 100644 --- a/selfdrive/controls/tests/test_following_distance.py +++ b/selfdrive/controls/tests/test_following_distance.py @@ -4,7 +4,7 @@ import numpy as np from cereal import log import cereal.messaging as messaging 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.long_mpc import LongitudinalMpc diff --git a/selfdrive/controls/tests/test_startup.py b/selfdrive/controls/tests/test_startup.py index 0ddbd354ec..541b11cb2f 100755 --- a/selfdrive/controls/tests/test_startup.py +++ b/selfdrive/controls/tests/test_startup.py @@ -52,7 +52,7 @@ class TestStartup(unittest.TestCase): time.sleep(2) # wait for controlsd to be ready health = messaging.new_message('health') - health.health.hwType = log.HealthData.HwType.uno + health.health.pandaType = log.HealthData.PandaType.uno pm.send('health', health) # fingerprint diff --git a/selfdrive/debug/cycle_alerts.py b/selfdrive/debug/cycle_alerts.py index 45bc8984c2..4e3cd7663c 100755 --- a/selfdrive/debug/cycle_alerts.py +++ b/selfdrive/debug/cycle_alerts.py @@ -17,7 +17,7 @@ def cycle_alerts(duration=200, is_metric=False): CP = CarInterface.get_params("HONDA CIVIC 2016 TOURING") sm = messaging.SubMaster(['thermal', 'health', 'frame', 'model', 'liveCalibration', - 'dMonitoringState', 'plan', 'pathPlan', 'liveLocationKalman']) + 'driverMonitoringState', 'plan', 'lateralPlan', 'liveLocationKalman']) controls_state = messaging.pub_sock('controlsState') thermal = messaging.pub_sock('thermal') diff --git a/selfdrive/debug/internal/check_alive_valid.py b/selfdrive/debug/internal/check_alive_valid.py index cd8a2463eb..51ad862745 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', 'dMonitoringState', 'plan', 'pathPlan']) + sm = messaging.SubMaster(['thermal', 'health', 'model', 'liveCalibration', 'driverMonitoringState', 'plan', 'lateralPlan']) i = 0 while True: diff --git a/selfdrive/debug/mpc/live_lateral_mpc.py b/selfdrive/debug/mpc/live_lateral_mpc.py index b1cd7102e5..c85be58aa9 100755 --- a/selfdrive/debug/mpc/live_lateral_mpc.py +++ b/selfdrive/debug/mpc/live_lateral_mpc.py @@ -58,7 +58,7 @@ def mpc_vwr_thread(addr="127.0.0.1"): # *** log *** livempc = messaging.sub_sock('liveMpc', 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: 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) 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_ydata(path_x) diff --git a/selfdrive/debug/test_fw_query_on_routes.py b/selfdrive/debug/test_fw_query_on_routes.py index 90346d5241..cf1db6348a 100755 --- a/selfdrive/debug/test_fw_query_on_routes.py +++ b/selfdrive/debug/test_fw_query_on_routes.py @@ -47,7 +47,7 @@ if __name__ == "__main__": for msg in lr: if msg.which() == "health": - if msg.health.hwType not in ['uno', 'blackPanda']: + if msg.health.pandaType not in ['uno', 'blackPanda']: dongles.append(dongle_id) break diff --git a/selfdrive/manager.py b/selfdrive/manager.py index 42e05b358d..6d4b042afe 100755 --- a/selfdrive/manager.py +++ b/selfdrive/manager.py @@ -463,7 +463,7 @@ def manager_thread(): while 1: msg = messaging.recv_sock(thermal_sock, wait=True) - if msg.thermal.freeSpace < 0.05: + if msg.thermal.freeSpacePercent < 0.05: logger_dead = True if msg.thermal.started: diff --git a/selfdrive/modeld/modeld.cc b/selfdrive/modeld/modeld.cc index e2308e582c..356812156d 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({"pathPlan", "frame"}); + SubMaster sm({"lateralPlan", "frame"}); // cl init 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){ // TODO: path planner timeout? - desire = ((int)sm["pathPlan"].getPathPlan().getDesire()); + desire = ((int)sm["lateralPlan"].getLateralPlan().getDesire()); frame_id = sm["frame"].getFrame().getFrameId(); } diff --git a/selfdrive/modeld/models/dmonitoring.cc b/selfdrive/modeld/models/dmonitoring.cc index 7b17dd7d6f..33900579fb 100644 --- a/selfdrive/modeld/models/dmonitoring.cc +++ b/selfdrive/modeld/models/dmonitoring.cc @@ -186,13 +186,13 @@ void dmonitoring_publish(PubMaster &pm, uint32_t frame_id, const DMonitoringResu framed.setRightEyeProb(res.right_eye_prob); framed.setLeftBlinkProb(res.left_blink_prob); framed.setRightBlinkProb(res.right_blink_prob); - framed.setSgProb(res.sg_prob); + framed.setSunglassesProb(res.sg_prob); framed.setPoorVision(res.poor_vision); framed.setPartialFace(res.partial_face); framed.setDistractedPose(res.distracted_pose); framed.setDistractedEyes(res.distracted_eyes); if (send_raw_pred) { - framed.setRawPred(raw_pred.asBytes()); + framed.setRawPredictions(raw_pred.asBytes()); } pm.send("driverState", msg); diff --git a/selfdrive/modeld/models/driving.cc b/selfdrive/modeld/models/driving.cc index 08135e0285..767bc2fe1f 100644 --- a/selfdrive/modeld/models/driving.cc +++ b/selfdrive/modeld/models/driving.cc @@ -273,7 +273,7 @@ void model_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t frame_id, flo framed.setTimestampEof(timestamp_eof); framed.setModelExecutionTime(model_execution_time); if (send_raw_pred) { - framed.setRawPred(raw_pred.asBytes()); + framed.setRawPredictions(raw_pred.asBytes()); } fill_model(framed, net_outputs); pm.send("modelV2", msg); diff --git a/selfdrive/monitoring/dmonitoringd.py b/selfdrive/monitoring/dmonitoringd.py index 1eebbe4e61..83313b8200 100755 --- a/selfdrive/monitoring/dmonitoringd.py +++ b/selfdrive/monitoring/dmonitoringd.py @@ -9,15 +9,13 @@ from selfdrive.locationd.calibrationd import Calibration def dmonitoringd_thread(sm=None, pm=None): if pm is None: - pm = messaging.PubMaster(['dMonitoringState']) + pm = messaging.PubMaster(['driverMonitoringState']) if sm is None: sm = messaging.SubMaster(['driverState', 'liveCalibration', 'carState', 'controlsState', 'modelV2'], poll=['driverState']) driver_status = DriverStatus(rhd=Params().get("IsRHD") == b"1") - offroad = Params().get("IsOffroad") == b"1" - sm['liveCalibration'].calStatus = Calibration.INVALID sm['liveCalibration'].rpyCalib = [0, 0, 0] sm['carState'].buttonEvents = [] @@ -58,14 +56,13 @@ def dmonitoringd_thread(sm=None, pm=None): # Update events from driver state driver_status.update(events, driver_engaged, sm['controlsState'].enabled, sm['carState'].standstill) - # build dMonitoringState packet - dat = messaging.new_message('dMonitoringState') - dat.dMonitoringState = { + # build driverMonitoringState packet + dat = messaging.new_message('driverMonitoringState') + dat.driverMonitoringState = { "events": events.to_msg(), "faceDetected": driver_status.face_detected, "isDistracted": driver_status.driver_distracted, "awarenessStatus": driver_status.awareness, - "isRHD": driver_status.is_rhd_region, "posePitchOffset": driver_status.pose.pitch_offseter.filtered_stat.mean(), "posePitchValidCount": driver_status.pose.pitch_offseter.filtered_stat.n, "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, "isLowStd": driver_status.pose.low_std, "hiStdCount": driver_status.hi_stds, - "isPreview": offroad, "isActiveMode": driver_status.active_monitoring_mode, } - pm.send('dMonitoringState', dat) + pm.send('driverMonitoringState', dat) def main(sm=None, pm=None): dmonitoringd_thread(sm, pm) diff --git a/selfdrive/monitoring/driver_monitor.py b/selfdrive/monitoring/driver_monitor.py index e70bebd687..1e7c012035 100644 --- a/selfdrive/monitoring/driver_monitor.py +++ b/selfdrive/monitoring/driver_monitor.py @@ -191,8 +191,8 @@ class DriverStatus(): # self.pose.roll_std = driver_state.faceOrientationStd[2] 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.blink.left_blink = driver_state.leftBlinkProb * (driver_state.leftEyeProb > _EYE_THRESHOLD) * (driver_state.sgProb < _SG_THRESHOLD) - self.blink.right_blink = driver_state.rightBlinkProb * (driver_state.rightEyeProb > _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.sunglassesProb < _SG_THRESHOLD) self.driver_distracted = self._is_driver_distracted(self.pose, self.blink) > 0 and \ driver_state.faceProb > _FACE_THRESHOLD and self.pose.low_std diff --git a/selfdrive/monitoring/test_monitoring.py b/selfdrive/monitoring/test_monitoring.py index 7648d3fba1..5872655fee 100755 --- a/selfdrive/monitoring/test_monitoring.py +++ b/selfdrive/monitoring/test_monitoring.py @@ -31,7 +31,6 @@ def make_msg(face_detected, distracted=False, model_uncertain=False): ds.rightBlinkProb = 1. * distracted ds.faceOrientationStd = [1.*model_uncertain, 1.*model_uncertain, 1.*model_uncertain] ds.facePositionStd = [1.*model_uncertain, 1.*model_uncertain] - ds.sgProb = 0. return ds # driver state from neural net, 10Hz diff --git a/selfdrive/test/longitudinal_maneuvers/maneuver.py b/selfdrive/test/longitudinal_maneuvers/maneuver.py index d60dcc4e31..b6efd79796 100644 --- a/selfdrive/test/longitudinal_maneuvers/maneuver.py +++ b/selfdrive/test/longitudinal_maneuvers/maneuver.py @@ -68,7 +68,6 @@ class Maneuver(): 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, cruise_speed=last_controls_state.vCruise, - jerk_factor=last_controls_state.jerkFactor, a_target=last_controls_state.aTarget, fcw=log['fcw']) diff --git a/selfdrive/test/longitudinal_maneuvers/maneuverplots.py b/selfdrive/test/longitudinal_maneuvers/maneuverplots.py index 503c184456..c1c1f28e8f 100644 --- a/selfdrive/test/longitudinal_maneuvers/maneuverplots.py +++ b/selfdrive/test/longitudinal_maneuvers/maneuverplots.py @@ -28,7 +28,6 @@ class ManeuverPlot(): self.v_target_lead_array = [] self.pid_speed_array = [] self.cruise_speed_array = [] - self.jerk_factor_array = [] self.a_target_array = [] @@ -40,7 +39,7 @@ class ManeuverPlot(): 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, - 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.gas_array.append(gas) self.brake_array.append(brake) @@ -57,7 +56,6 @@ class ManeuverPlot(): self.v_target_lead_array.append(v_target_lead) self.pid_speed_array.append(pid_speed) self.cruise_speed_array.append(cruise_speed) - self.jerk_factor_array.append(jerk_factor) self.a_target_array.append(a_target) self.fcw_array.append(fcw) diff --git a/selfdrive/test/longitudinal_maneuvers/plant.py b/selfdrive/test/longitudinal_maneuvers/plant.py index 17a91c1db7..67f4cc46dd 100755 --- a/selfdrive/test/longitudinal_maneuvers/plant.py +++ b/selfdrive/test/longitudinal_maneuvers/plant.py @@ -110,7 +110,6 @@ class Plant(): self.rate = rate if not Plant.messaging_initialized: - Plant.pm = messaging.PubMaster(['frame', 'frontFrame', 'ubloxRaw']) Plant.logcan = messaging.pub_sock('can') Plant.sendcan = messaging.sub_sock('sendcan') @@ -119,10 +118,10 @@ class Plant(): Plant.live_location_kalman = messaging.pub_sock('liveLocationKalman') Plant.health = messaging.pub_sock('health') 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.controls_state = messaging.sub_sock('controlsState') - Plant.plan = messaging.sub_sock('plan') + Plant.plan = messaging.sub_sock('longitudinalPlan') Plant.messaging_initialized = True self.frame = 0 @@ -200,7 +199,7 @@ class Plant(): fcw = None for a in messaging.drain_sock(Plant.plan): - if a.plan.fcw: + if a.longitudinalPlan.fcw: fcw = True if self.cp.vl[0x1fa]['COMPUTER_BRAKE_REQUEST']: @@ -370,9 +369,9 @@ class Plant(): live_parameters.liveParameters.stiffnessFactor = 1.0 Plant.live_params.send(live_parameters.to_bytes()) - dmon_state = messaging.new_message('dMonitoringState') - dmon_state.dMonitoringState.isDistracted = False - Plant.dMonitoringState.send(dmon_state.to_bytes()) + dmon_state = messaging.new_message('driverMonitoringState') + dmon_state.driverMonitoringState.isDistracted = False + Plant.driverMonitoringState.send(dmon_state.to_bytes()) health = messaging.new_message('health') health.health.safetyModel = car.CarParams.SafetyModel.hondaNidec @@ -380,7 +379,7 @@ class Plant(): Plant.health.send(health.to_bytes()) thermal = messaging.new_message('thermal') - thermal.thermal.freeSpace = 1. + thermal.thermal.freeSpacePercent = 1. thermal.thermal.batteryPercent = 100 Plant.thermal.send(thermal.to_bytes()) diff --git a/selfdrive/test/longitudinal_maneuvers/test_longitudinal.py b/selfdrive/test/longitudinal_maneuvers/test_longitudinal.py index a41d0db3af..c687ff7a30 100755 --- a/selfdrive/test/longitudinal_maneuvers/test_longitudinal.py +++ b/selfdrive/test/longitudinal_maneuvers/test_longitudinal.py @@ -330,14 +330,6 @@ class LongitudinalControl(unittest.TestCase): params.put("OpenpilotEnabledToggle", "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 def test_longitudinal_setup(self): pass diff --git a/selfdrive/test/model_replay.py b/selfdrive/test/model_replay.py index 6e3ab67924..1ed3e58145 100755 --- a/selfdrive/test/model_replay.py +++ b/selfdrive/test/model_replay.py @@ -21,10 +21,10 @@ if __name__ == "__main__": if os.path.isfile(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) 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) print("Finished replay") diff --git a/selfdrive/test/process_replay/camera_replay.py b/selfdrive/test/process_replay/camera_replay.py index fe13a5cf66..c906bdf3e7 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', 'pathPlan']) + pm = messaging.PubMaster(['frame', 'liveCalibration', 'lateralPlan']) sm = messaging.SubMaster(['modelV2']) # TODO: add dmonitoringmodeld @@ -49,7 +49,7 @@ def camera_replay(lr, fr, desire=None, calib=None): sm.update(1000) 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"] for msg in cal[:5]: @@ -63,9 +63,9 @@ def camera_replay(lr, fr, desire=None, calib=None): elif msg.which() == "frame": if desire is not None: for i in desire[frame_idx].nonzero()[0]: - dat = messaging.new_message('pathPlan') - dat.pathPlan.desire = desires_by_index[i] - pm.send('pathPlan', dat) + dat = messaging.new_message('lateralPlan') + dat.lateralPlan.desire = desires_by_index[i] + pm.send('lateralPlan', dat) f = msg.as_builder() img = fr.get(frame_idx, pix_fmt="rgb24")[0][:,:,::-1] diff --git a/selfdrive/test/process_replay/process_replay.py b/selfdrive/test/process_replay/process_replay.py index b85fdc538c..17a0a94f63 100755 --- a/selfdrive/test/process_replay/process_replay.py +++ b/selfdrive/test/process_replay/process_replay.py @@ -160,7 +160,7 @@ def fingerprint(msgs, fsm, can_sock): can_sock.recv_ready.set() 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) fsm.update_called.clear() @@ -221,7 +221,7 @@ CONFIGS = [ proc_name="controlsd", pub_sub={ "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": [], }, ignore=["logMonoTime", "valid", "controlsState.startMonoTime", "controlsState.cumLagMs"], @@ -233,7 +233,7 @@ CONFIGS = [ proc_name="radard", pub_sub={ "can": ["radarState", "liveTracks"], - "liveParameters": [], "controlsState": [], "modelV2": [], + "liveParameters": [], "carState": [], "modelV2": [], }, ignore=["logMonoTime", "valid", "radarState.cumLagMs"], init_callback=get_car_params, @@ -243,10 +243,10 @@ CONFIGS = [ ProcessConfig( proc_name="plannerd", pub_sub={ - "modelV2": ["pathPlan"], "radarState": ["plan"], + "modelV2": ["lateralPlan"], "radarState": ["longitudinalPlan"], "carState": [], "controlsState": [], "liveParameters": [], }, - ignore=["logMonoTime", "valid", "plan.processingDelay"], + ignore=["logMonoTime", "valid", "longitudinalPlan.processingDelay"], init_callback=get_car_params, should_recv_callback=None, tolerance=None, @@ -265,7 +265,7 @@ CONFIGS = [ ProcessConfig( proc_name="dmonitoringd", pub_sub={ - "driverState": ["dMonitoringState"], + "driverState": ["driverMonitoringState"], "liveCalibration": [], "carState": [], "modelV2": [], "controlsState": [], }, ignore=["logMonoTime", "valid"], diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 5ee99cb7ee..ea4d516aec 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -2ecf7c5d8816aaf70dc42a5ec37a0106fcd42799 \ No newline at end of file +3d94188da5fdb35c18664d32ac3c720b221a78c7 \ No newline at end of file diff --git a/selfdrive/test/test_car_models.py b/selfdrive/test/test_car_models.py index 46645c799a..dcf1d96b46 100755 --- a/selfdrive/test/test_car_models.py +++ b/selfdrive/test/test_car_models.py @@ -580,7 +580,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,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 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: extra_socks.append("sendcan") 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) failures = [s for s in tested_socks + extra_socks if s not in recvd_socks] diff --git a/selfdrive/test/test_models.py b/selfdrive/test/test_models.py index 74339f75fe..ad101e2078 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 -HwType = log.HealthData.HwType +PandaType = log.HealthData.PandaType ROUTES = {v['carFingerprint']: k for k, v in routes.items() if v['enableCamera']} diff --git a/selfdrive/thermald/power_monitoring.py b/selfdrive/thermald/power_monitoring.py index 5d59cf3879..a291b15145 100644 --- a/selfdrive/thermald/power_monitoring.py +++ b/selfdrive/thermald/power_monitoring.py @@ -43,7 +43,7 @@ class PowerMonitoring: 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.hwType == log.HealthData.HwType.unknown: + if health is None or health.health.pandaType == log.HealthData.PandaType.unknown: with self.integration_lock: self.last_measurement_time = None self.next_pulsed_measurement_time = None @@ -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.hwType == log.HealthData.HwType.uno + is_uno = health.health.pandaType == log.HealthData.PandaType.uno # Get current power draw somehow current_power = HARDWARE.get_current_power_draw() if current_power is not None: diff --git a/selfdrive/thermald/tests/test_power_monitoring.py b/selfdrive/thermald/tests/test_power_monitoring.py index a733930891..be2e4a09e4 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.HwType.whitePanda, - log.HealthData.HwType.greyPanda, - log.HealthData.HwType.blackPanda, - log.HealthData.HwType.uno]] +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]] def pm_patch(name, value, constant=False): if constant: @@ -39,7 +39,7 @@ class TestPowerMonitoring(unittest.TestCase): def mock_health(self, ignition, hw_type, car_voltage=12): health = messaging.new_message('health') - health.health.hwType = hw_type + health.health.pandaType = hw_type health.health.voltage = car_voltage * 1e3 health.health.ignitionLine = ignition 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 = PowerMonitoring() 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): pm.calculate(health) 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 = PowerMonitoring() 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): pm.calculate(health) if i % 10 == 0: diff --git a/selfdrive/thermald/thermald.py b/selfdrive/thermald/thermald.py index c9b9c09752..cc5f814837 100755 --- a/selfdrive/thermald/thermald.py +++ b/selfdrive/thermald/thermald.py @@ -208,7 +208,7 @@ def thermald_thread(): usb_power = health.health.usbPowerMode != log.HealthData.UsbPowerMode.client # 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 if no_panda_cnt > DISCONNECT_TIMEOUT / DT_TRML: if startup_conditions["ignition"]: @@ -219,8 +219,8 @@ def thermald_thread(): startup_conditions["ignition"] = health.health.ignitionLine or health.health.ignitionCan # Setup fan handler on first connect to panda - if handle_fan is None and health.health.hwType != log.HealthData.HwType.unknown: - is_uno = health.health.hwType == log.HealthData.HwType.uno + if handle_fan is None and health.health.pandaType != log.HealthData.PandaType.unknown: + is_uno = health.health.pandaType == log.HealthData.PandaType.uno if (not EON) or is_uno: cloudlog.info("Setting up UNO fan handler") @@ -232,8 +232,8 @@ def thermald_thread(): # Handle disconnect if health_prev is not None: - if health.health.hwType == log.HealthData.HwType.unknown and \ - health_prev.health.hwType != log.HealthData.HwType.unknown: + if health.health.pandaType == log.HealthData.PandaType.unknown and \ + health_prev.health.pandaType != log.HealthData.PandaType.unknown: params.panda_disconnect() health_prev = health @@ -245,9 +245,9 @@ def thermald_thread(): except Exception: cloudlog.exception("Error getting network status") - msg.thermal.freeSpace = get_available_percent(default=100.0) / 100.0 - msg.thermal.memUsedPercent = int(round(psutil.virtual_memory().percent)) - msg.thermal.cpuPerc = int(round(psutil.cpu_percent())) + 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() @@ -271,7 +271,7 @@ def thermald_thread(): if handle_fan is not None: 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 # 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.freeSpace > 0.02 + startup_conditions["free_space"] = msg.thermal.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,8 +357,8 @@ 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.hwType not in [log.HealthData.HwType.whitePanda, - log.HealthData.HwType.greyPanda] + 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"]) # 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.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 pm.send("thermal", msg) diff --git a/selfdrive/ui/android/ui.cc b/selfdrive/ui/android/ui.cc index 2960d5d504..1c965dedfe 100644 --- a/selfdrive/ui/android/ui.cc +++ b/selfdrive/ui/android/ui.cc @@ -163,7 +163,7 @@ int main(int argc, char* argv[]) { } // 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 float clipped_brightness = fmin(512, (s->light_sensor*brightness_m) + brightness_b); diff --git a/selfdrive/ui/paint.cc b/selfdrive/ui/paint.cc index 2ebe934c1c..21fe6d3833 100644 --- a/selfdrive/ui/paint.cc +++ b/selfdrive/ui/paint.cc @@ -233,7 +233,7 @@ static void ui_draw_vision_maxspeed(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)); 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"); @@ -244,12 +244,7 @@ static void ui_draw_vision_event(UIState *s) { 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_y = s->viz_rect.y + (bdr_s*1.5); - if (s->scene.controls_state.getDecelForModel() && s->scene.controls_state.getEnabled()) { - // 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()) { + if (s->scene.controls_state.getEngageable()) { // draw steering wheel const int bg_wheel_size = 96; const int bg_wheel_x = viz_event_x + (viz_event_w-bg_wheel_size); diff --git a/selfdrive/ui/sidebar.cc b/selfdrive/ui/sidebar.cc index 3e363eb593..79ba0d5f33 100644 --- a/selfdrive/ui/sidebar.cc +++ b/selfdrive/ui/sidebar.cc @@ -118,7 +118,7 @@ static void draw_panda_metric(UIState *s) { int panda_severity = 0; 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_message = "NO\nPANDA"; } diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc index 72ceb5e785..ba729b0b83 100644 --- a/selfdrive/ui/ui.cc +++ b/selfdrive/ui/ui.cc @@ -41,7 +41,7 @@ static void ui_init_vision(UIState *s) { void ui_init(UIState *s) { 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->status = STATUS_OFFROAD; @@ -127,6 +127,9 @@ static void update_sockets(UIState *s) { if (s->started && sm.updated("controlsState")) { scene.controls_state = sm["controlsState"].getControlsState(); } + if (sm.updated("carState")) { + scene.car_state = sm["carState"].getCarState(); + } if (sm.updated("radarState")) { auto radar_state = sm["radarState"].getRadarState(); const auto line = sm["modelV2"].getModelV2().getPosition(); @@ -153,10 +156,10 @@ static void update_sockets(UIState *s) { } if (sm.updated("health")) { auto health = sm["health"].getHealth(); - scene.hwType = health.getHwType(); + scene.pandaType = health.getPandaType(); s->ignition = health.getIgnitionLine() || health.getIgnitionCan(); } 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")) { s->longitudinal_control = sm["carParams"].getCarParams().getOpenpilotLongitudinalControl(); @@ -164,11 +167,10 @@ static void update_sockets(UIState *s) { if (sm.updated("driverState")) { scene.driver_state = sm["driverState"].getDriverState(); } - if (sm.updated("dMonitoringState")) { - scene.dmonitoring_state = sm["dMonitoringState"].getDMonitoringState(); - scene.is_rhd = scene.dmonitoring_state.getIsRHD(); - scene.frontview = scene.dmonitoring_state.getIsPreview(); - } else if (scene.frontview && (sm.frame - sm.rcv_frame("dMonitoringState")) > UI_FREQ/2) { + if (sm.updated("driverMonitoringState")) { + scene.dmonitoring_state = sm["driverMonitoringState"].getDriverMonitoringState(); + scene.frontview = !s->ignition; + } else if (scene.frontview && (sm.frame - sm.rcv_frame("driverMonitoringState")) > UI_FREQ/2) { scene.frontview = false; } if (sm.updated("sensorEvents")) { @@ -283,6 +285,7 @@ static void update_status(UIState *s) { s->status = STATUS_DISENGAGED; s->started_frame = s->sm->frame; + read_param(&s->scene.is_rhd, "IsRHD"); s->active_app = cereal::UiLayoutState::App::NONE; s->sidebar_collapsed = true; s->scene.alert_size = cereal::ControlsState::AlertSize::NONE; diff --git a/selfdrive/ui/ui.hpp b/selfdrive/ui/ui.hpp index 8ae730cb75..189fec52a5 100644 --- a/selfdrive/ui/ui.hpp +++ b/selfdrive/ui/ui.hpp @@ -117,14 +117,15 @@ typedef struct UIScene { float alert_blinking_rate; cereal::ControlsState::AlertSize alert_size; - cereal::HealthData::HwType hwType; + cereal::HealthData::PandaType pandaType; NetStatus athenaStatus; cereal::ThermalData::Reader thermal; cereal::RadarState::LeadData::Reader lead_data[2]; + cereal::CarState::Reader car_state; cereal::ControlsState::Reader controls_state; cereal::DriverState::Reader driver_state; - cereal::DMonitoringState::Reader dmonitoring_state; + cereal::DriverMonitoringState::Reader dmonitoring_state; // modelV2 float lane_line_probs[4]; diff --git a/tools/carcontrols/debug_controls.py b/tools/carcontrols/debug_controls.py index 2334e2576b..595147048f 100755 --- a/tools/carcontrols/debug_controls.py +++ b/tools/carcontrols/debug_controls.py @@ -7,7 +7,7 @@ import cereal.messaging as messaging from selfdrive.car.car_helpers import get_car, get_one_can from selfdrive.boardd.boardd import can_list_to_can_capnp -HwType = log.HealthData.HwType +PandaType = log.HealthData.PandaType def steer_thread(): diff --git a/tools/nui/Unlogger.cpp b/tools/nui/Unlogger.cpp index a2381a42bd..ee8327eda2 100644 --- a/tools/nui/Unlogger.cpp +++ b/tools/nui/Unlogger.cpp @@ -4,8 +4,9 @@ #include // include the dynamic struct -#include "cereal/gen/cpp/car.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 "Unlogger.hpp" diff --git a/tools/nui/main.cpp b/tools/nui/main.cpp index 76a9579b55..be661afe59 100644 --- a/tools/nui/main.cpp +++ b/tools/nui/main.cpp @@ -165,6 +165,7 @@ void Window::paintEvent(QPaintEvent *event) { //p.drawRect(0, 0, 600, 100); // 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(); if (last_event_size != this_event_size) { if (px != NULL) delete px; @@ -179,10 +180,11 @@ void Window::paintEvent(QPaintEvent *event) { for (auto e : events) { auto type = e.which(); //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(); uint64_t t = (e.getLogMonoTime()-t0); - float vEgo = controlsState.getVEgo(); int enabled = controlsState.getState() == cereal::ControlsState::OpenpilotState::ENABLED; int rt = timeToPixel(t); // 250 ms per pixel if (rt != lt) { diff --git a/tools/replay/ui.py b/tools/replay/ui.py index 1f26072dd1..6676167932 100755 --- a/tools/replay/ui.py +++ b/tools/replay/ui.py @@ -70,7 +70,7 @@ def ui_thread(addr, frame_address): frame = messaging.sub_sock('frame', addr=addr, conflate=True) 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 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: model_data = extract_model_data(sm['model']) 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 if sm.updated['liveMpc']: diff --git a/tools/sim/bridge.py b/tools/sim/bridge.py index 3b3d8e9e40..242e87f1a1 100755 --- a/tools/sim/bridge.py +++ b/tools/sim/bridge.py @@ -88,7 +88,7 @@ def health_function(): dat.valid = True dat.health = { 'ignitionLine': True, - 'hwType': "blackPanda", + 'pandaType': "blackPanda", 'controlsAllowed': True, 'safetyModel': 'hondaNidec' } @@ -104,7 +104,7 @@ def fake_gps(): time.sleep(0.01) def fake_driver_monitoring(): - pm = messaging.PubMaster(['driverState','dMonitoringState']) + pm = messaging.PubMaster(['driverState','driverMonitoringState']) while 1: # dmonitoringmodeld output @@ -113,14 +113,14 @@ def fake_driver_monitoring(): pm.send('driverState', dat) # dmonitoringd output - dat = messaging.new_message('dMonitoringState') - dat.dMonitoringState = { + dat = messaging.new_message('driverMonitoringState') + dat.driverMonitoringState = { "faceDetected": True, "isDistracted": False, "awarenessStatus": 1., "isRHD": False, } - pm.send('dMonitoringState', dat) + pm.send('driverMonitoringState', dat) time.sleep(DT_DMON)