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