|
|
|
@ -9,7 +9,7 @@ from common.realtime import sec_since_boot, config_realtime_process, Priority, R |
|
|
|
from common.profiler import Profiler |
|
|
|
from common.profiler import Profiler |
|
|
|
from common.params import Params, put_nonblocking |
|
|
|
from common.params import Params, put_nonblocking |
|
|
|
import cereal.messaging as messaging |
|
|
|
import cereal.messaging as messaging |
|
|
|
from selfdrive.config import Conversions as CV |
|
|
|
from common.conversions import Conversions as CV |
|
|
|
from selfdrive.swaglog import cloudlog |
|
|
|
from selfdrive.swaglog import cloudlog |
|
|
|
from selfdrive.boardd.boardd import can_list_to_can_capnp |
|
|
|
from selfdrive.boardd.boardd import can_list_to_can_capnp |
|
|
|
from selfdrive.car.car_helpers import get_car, get_startup_event, get_one_can |
|
|
|
from selfdrive.car.car_helpers import get_car, get_startup_event, get_one_can |
|
|
|
@ -466,7 +466,7 @@ class Controls: |
|
|
|
self.v_cruise_kph = initialize_v_cruise(CS.vEgo, CS.buttonEvents, self.v_cruise_kph_last) |
|
|
|
self.v_cruise_kph = initialize_v_cruise(CS.vEgo, CS.buttonEvents, self.v_cruise_kph_last) |
|
|
|
|
|
|
|
|
|
|
|
# Check if actuators are enabled |
|
|
|
# Check if actuators are enabled |
|
|
|
self.active = self.state == State.enabled or self.state == State.softDisabling |
|
|
|
self.active = self.state in (State.enabled, State.softDisabling) |
|
|
|
if self.active: |
|
|
|
if self.active: |
|
|
|
self.current_alert_types.append(ET.WARNING) |
|
|
|
self.current_alert_types.append(ET.WARNING) |
|
|
|
|
|
|
|
|
|
|
|
@ -474,7 +474,7 @@ class Controls: |
|
|
|
self.enabled = self.active or self.state == State.preEnabled |
|
|
|
self.enabled = self.active or self.state == State.preEnabled |
|
|
|
|
|
|
|
|
|
|
|
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 a CarControl packet""" |
|
|
|
|
|
|
|
|
|
|
|
# Update VehicleModel |
|
|
|
# Update VehicleModel |
|
|
|
params = self.sm['liveParameters'] |
|
|
|
params = self.sm['liveParameters'] |
|
|
|
@ -485,7 +485,14 @@ class Controls: |
|
|
|
lat_plan = self.sm['lateralPlan'] |
|
|
|
lat_plan = self.sm['lateralPlan'] |
|
|
|
long_plan = self.sm['longitudinalPlan'] |
|
|
|
long_plan = self.sm['longitudinalPlan'] |
|
|
|
|
|
|
|
|
|
|
|
actuators = car.CarControl.Actuators.new_message() |
|
|
|
CC = car.CarControl.new_message() |
|
|
|
|
|
|
|
CC.enabled = self.enabled |
|
|
|
|
|
|
|
# Check which actuators can be enabled |
|
|
|
|
|
|
|
CC.latActive = self.active and not CS.steerFaultTemporary and not CS.steerFaultPermanent and \ |
|
|
|
|
|
|
|
CS.vEgo > self.CP.minSteerSpeed and not CS.standstill |
|
|
|
|
|
|
|
CC.longActive = self.active |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
actuators = CC.actuators |
|
|
|
actuators.longControlState = self.LoC.long_control_state |
|
|
|
actuators.longControlState = self.LoC.long_control_state |
|
|
|
|
|
|
|
|
|
|
|
if CS.leftBlinker or CS.rightBlinker: |
|
|
|
if CS.leftBlinker or CS.rightBlinker: |
|
|
|
@ -493,37 +500,40 @@ class Controls: |
|
|
|
|
|
|
|
|
|
|
|
# State specific actions |
|
|
|
# State specific actions |
|
|
|
|
|
|
|
|
|
|
|
if not self.active: |
|
|
|
if not CC.latActive: |
|
|
|
self.LaC.reset() |
|
|
|
self.LaC.reset() |
|
|
|
|
|
|
|
if not CC.longActive: |
|
|
|
self.LoC.reset(v_pid=CS.vEgo) |
|
|
|
self.LoC.reset(v_pid=CS.vEgo) |
|
|
|
|
|
|
|
|
|
|
|
if not self.joystick_mode: |
|
|
|
if not self.joystick_mode: |
|
|
|
# accel PID loop |
|
|
|
# accel PID loop |
|
|
|
pid_accel_limits = self.CI.get_pid_accel_limits(self.CP, CS.vEgo, self.v_cruise_kph * CV.KPH_TO_MS) |
|
|
|
pid_accel_limits = self.CI.get_pid_accel_limits(self.CP, CS.vEgo, self.v_cruise_kph * CV.KPH_TO_MS) |
|
|
|
t_since_plan = (self.sm.frame - self.sm.rcv_frame['longitudinalPlan']) * DT_CTRL |
|
|
|
t_since_plan = (self.sm.frame - self.sm.rcv_frame['longitudinalPlan']) * DT_CTRL |
|
|
|
actuators.accel = self.LoC.update(self.active, CS, self.CP, long_plan, pid_accel_limits, t_since_plan) |
|
|
|
actuators.accel = self.LoC.update(CC.longActive, CS, self.CP, long_plan, pid_accel_limits, t_since_plan) |
|
|
|
|
|
|
|
|
|
|
|
# Steering PID loop and lateral MPC |
|
|
|
# Steering PID loop and lateral MPC |
|
|
|
lat_active = self.active and not CS.steerFaultTemporary and not CS.steerFaultPermanent and CS.vEgo > self.CP.minSteerSpeed |
|
|
|
|
|
|
|
desired_curvature, desired_curvature_rate = get_lag_adjusted_curvature(self.CP, CS.vEgo, |
|
|
|
desired_curvature, desired_curvature_rate = get_lag_adjusted_curvature(self.CP, CS.vEgo, |
|
|
|
lat_plan.psis, |
|
|
|
lat_plan.psis, |
|
|
|
lat_plan.curvatures, |
|
|
|
lat_plan.curvatures, |
|
|
|
lat_plan.curvatureRates) |
|
|
|
lat_plan.curvatureRates) |
|
|
|
actuators.steer, actuators.steeringAngleDeg, lac_log = self.LaC.update(lat_active, CS, self.CP, self.VM, params, self.last_actuators, |
|
|
|
actuators.steer, actuators.steeringAngleDeg, lac_log = self.LaC.update(CC.latActive, CS, self.CP, self.VM, |
|
|
|
desired_curvature, desired_curvature_rate) |
|
|
|
params, self.last_actuators, desired_curvature, |
|
|
|
|
|
|
|
desired_curvature_rate) |
|
|
|
else: |
|
|
|
else: |
|
|
|
lac_log = log.ControlsState.LateralDebugState.new_message() |
|
|
|
lac_log = log.ControlsState.LateralDebugState.new_message() |
|
|
|
if self.sm.rcv_frame['testJoystick'] > 0 and self.active: |
|
|
|
if self.sm.rcv_frame['testJoystick'] > 0: |
|
|
|
actuators.accel = 4.0*clip(self.sm['testJoystick'].axes[0], -1, 1) |
|
|
|
if CC.longActive: |
|
|
|
|
|
|
|
actuators.accel = 4.0*clip(self.sm['testJoystick'].axes[0], -1, 1) |
|
|
|
|
|
|
|
|
|
|
|
steer = clip(self.sm['testJoystick'].axes[1], -1, 1) |
|
|
|
if CC.latActive: |
|
|
|
# max angle is 45 for angle-based cars |
|
|
|
steer = clip(self.sm['testJoystick'].axes[1], -1, 1) |
|
|
|
actuators.steer, actuators.steeringAngleDeg = steer, steer * 45. |
|
|
|
# max angle is 45 for angle-based cars |
|
|
|
|
|
|
|
actuators.steer, actuators.steeringAngleDeg = steer, steer * 45. |
|
|
|
|
|
|
|
|
|
|
|
lac_log.active = True |
|
|
|
lac_log.active = self.active |
|
|
|
lac_log.steeringAngleDeg = CS.steeringAngleDeg |
|
|
|
lac_log.steeringAngleDeg = CS.steeringAngleDeg |
|
|
|
lac_log.output = steer |
|
|
|
lac_log.output = actuators.steer |
|
|
|
lac_log.saturated = abs(steer) >= 0.9 |
|
|
|
lac_log.saturated = abs(actuators.steer) >= 0.9 |
|
|
|
|
|
|
|
|
|
|
|
# Send a "steering required alert" if saturation count has reached the limit |
|
|
|
# Send a "steering required alert" if saturation count has reached the limit |
|
|
|
if lac_log.active and lac_log.saturated and not CS.steeringPressed: |
|
|
|
if lac_log.active and lac_log.saturated and not CS.steeringPressed: |
|
|
|
@ -547,7 +557,7 @@ class Controls: |
|
|
|
cloudlog.error(f"actuators.{p} not finite {actuators.to_dict()}") |
|
|
|
cloudlog.error(f"actuators.{p} not finite {actuators.to_dict()}") |
|
|
|
setattr(actuators, p, 0.0) |
|
|
|
setattr(actuators, p, 0.0) |
|
|
|
|
|
|
|
|
|
|
|
return actuators, lac_log |
|
|
|
return CC, lac_log |
|
|
|
|
|
|
|
|
|
|
|
def update_button_timers(self, buttonEvents): |
|
|
|
def update_button_timers(self, buttonEvents): |
|
|
|
# increment timer for buttons still pressed |
|
|
|
# increment timer for buttons still pressed |
|
|
|
@ -559,14 +569,9 @@ class Controls: |
|
|
|
if b.type.raw in self.button_timers: |
|
|
|
if b.type.raw in self.button_timers: |
|
|
|
self.button_timers[b.type.raw] = 1 if b.pressed else 0 |
|
|
|
self.button_timers[b.type.raw] = 1 if b.pressed else 0 |
|
|
|
|
|
|
|
|
|
|
|
def publish_logs(self, CS, start_time, actuators, lac_log): |
|
|
|
def publish_logs(self, CS, start_time, CC, lac_log): |
|
|
|
"""Send actuators and hud commands to the car, send controlsstate and MPC logging""" |
|
|
|
"""Send actuators and hud commands to the car, send controlsstate and MPC logging""" |
|
|
|
|
|
|
|
|
|
|
|
CC = car.CarControl.new_message() |
|
|
|
|
|
|
|
CC.enabled = self.enabled |
|
|
|
|
|
|
|
CC.active = self.active |
|
|
|
|
|
|
|
CC.actuators = actuators |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
orientation_value = self.sm['liveLocationKalman'].orientationNED.value |
|
|
|
orientation_value = self.sm['liveLocationKalman'].orientationNED.value |
|
|
|
if len(orientation_value) > 2: |
|
|
|
if len(orientation_value) > 2: |
|
|
|
CC.roll = orientation_value[0] |
|
|
|
CC.roll = orientation_value[0] |
|
|
|
@ -587,7 +592,7 @@ class Controls: |
|
|
|
|
|
|
|
|
|
|
|
recent_blinker = (self.sm.frame - self.last_blinker_frame) * DT_CTRL < 5.0 # 5s blinker cooldown |
|
|
|
recent_blinker = (self.sm.frame - self.last_blinker_frame) * DT_CTRL < 5.0 # 5s blinker cooldown |
|
|
|
ldw_allowed = self.is_ldw_enabled and CS.vEgo > LDW_MIN_SPEED and not recent_blinker \ |
|
|
|
ldw_allowed = self.is_ldw_enabled and CS.vEgo > LDW_MIN_SPEED and not recent_blinker \ |
|
|
|
and not self.active and self.sm['liveCalibration'].calStatus == Calibration.CALIBRATED |
|
|
|
and not CC.latActive and self.sm['liveCalibration'].calStatus == Calibration.CALIBRATED |
|
|
|
|
|
|
|
|
|
|
|
model_v2 = self.sm['modelV2'] |
|
|
|
model_v2 = self.sm['modelV2'] |
|
|
|
desire_prediction = model_v2.meta.desirePrediction |
|
|
|
desire_prediction = model_v2.meta.desirePrediction |
|
|
|
@ -726,12 +731,12 @@ class Controls: |
|
|
|
self.prof.checkpoint("State transition") |
|
|
|
self.prof.checkpoint("State transition") |
|
|
|
|
|
|
|
|
|
|
|
# Compute actuators (runs PID loops and lateral MPC) |
|
|
|
# Compute actuators (runs PID loops and lateral MPC) |
|
|
|
actuators, lac_log = self.state_control(CS) |
|
|
|
CC, lac_log = self.state_control(CS) |
|
|
|
|
|
|
|
|
|
|
|
self.prof.checkpoint("State Control") |
|
|
|
self.prof.checkpoint("State Control") |
|
|
|
|
|
|
|
|
|
|
|
# Publish data |
|
|
|
# Publish data |
|
|
|
self.publish_logs(CS, start_time, actuators, lac_log) |
|
|
|
self.publish_logs(CS, start_time, CC, lac_log) |
|
|
|
self.prof.checkpoint("Sent") |
|
|
|
self.prof.checkpoint("Sent") |
|
|
|
|
|
|
|
|
|
|
|
self.update_button_timers(CS.buttonEvents) |
|
|
|
self.update_button_timers(CS.buttonEvents) |
|
|
|
|
Shane Smiskol
3 years ago