mawei
/
openpilot0
mirror of https://github.com/commaai/openpilot.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

carControl: add long and lat active fields (#23859) 

* proof of concept

* actuators packet describes which actuators are active

* bump cereal

* fixes

* not needed for this PR

* Do Toyota

* add back controlsState.active

* bump cereal

* rest of cars

* in actuators

* add active back

* which

* use controlsState.active for now

* will make an issue

* Update selfdrive/controls/controlsd.py

Co-authored-by: Adeeb Shihadeh <adeebshihadeh@gmail.com>

* move local lat_active checks into CC.latActive

* remove redundant checks

* move comment

move comment

* fix joystick mode

* get enabled from carcontrol

* do standstill check in controlsd

* make sure we consider the gas press case for GM

* use CC.actuators

* fix

* capitalization

* Bump cereal

Bump cereal

* make intermediate actuators

* similar convention to before

* clean that up

* update refs

Co-authored-by: Adeeb Shihadeh <adeebshihadeh@gmail.com>
old-commit-hash: 629399a449
This commit is contained in:
Shane Smiskol 2022-03-13 20:58:44 -07:00 committed by GitHub
parent eabc0b5289
commit 3e61c7e6ae
21 changed files with 86 additions and 94 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
cereal

@ -1 +1 @@
Subproject commit 279311d0bde60e814a697da22c55c6abe8a3b03c
Subproject commit 84a1793eb4d09821f23613b98d80e2ce4ee57b9c

2
selfdrive/car/ford/interface.py
View File

@ -66,7 +66,7 @@ class CarInterface(CarInterfaceBase):
def apply(self, c):
ret = self.CC.update(c.enabled, self.CS, self.frame, c.actuators,
c.hudControl.visualAlert, c.cruiseControl.cancel)
c.hudControl.visualAlert, c.cruiseControl.cancel)
self.frame += 1
return ret

15
selfdrive/car/gm/carcontroller.py
View File

@ -27,8 +27,7 @@ class CarController():
self.packer_obj = CANPacker(DBC[CP.carFingerprint]['radar'])
self.packer_ch = CANPacker(DBC[CP.carFingerprint]['chassis'])
def update(self, c, enabled, CS, frame, actuators,
hud_v_cruise, hud_show_lanes, hud_show_car, hud_alert):
def update(self, c, CS, frame, actuators, hud_v_cruise, hud_show_lanes, hud_show_car, hud_alert):
P = self.params
@ -41,7 +40,7 @@ class CarController():
if CS.lka_steering_cmd_counter != self.lka_steering_cmd_counter_last:
self.lka_steering_cmd_counter_last = CS.lka_steering_cmd_counter
elif (frame % P.STEER_STEP) == 0:
lkas_enabled = c.active and not (CS.out.steerFaultTemporary or CS.out.steerFaultPermanent) and CS.out.vEgo > P.MIN_STEER_SPEED
lkas_enabled = c.latActive and CS.out.vEgo > P.MIN_STEER_SPEED
if lkas_enabled:
new_steer = int(round(actuators.steer * P.STEER_MAX))
apply_steer = apply_std_steer_torque_limits(new_steer, self.apply_steer_last, CS.out.steeringTorque, P)
@ -58,7 +57,7 @@ class CarController():
# Gas/regen and brakes - all at 25Hz
if (frame % 4) == 0:
if not c.active:
if not c.longActive:
# Stock ECU sends max regen when not enabled.
self.apply_gas = P.MAX_ACC_REGEN
self.apply_brake = 0
@ -68,15 +67,15 @@ class CarController():
idx = (frame // 4) % 4
at_full_stop = enabled and CS.out.standstill
near_stop = enabled and (CS.out.vEgo < P.NEAR_STOP_BRAKE_PHASE)
at_full_stop = c.longActive and CS.out.standstill
near_stop = c.longActive and (CS.out.vEgo < P.NEAR_STOP_BRAKE_PHASE)
can_sends.append(gmcan.create_friction_brake_command(self.packer_ch, CanBus.CHASSIS, self.apply_brake, idx, near_stop, at_full_stop))
can_sends.append(gmcan.create_gas_regen_command(self.packer_pt, CanBus.POWERTRAIN, self.apply_gas, idx, enabled, at_full_stop))
can_sends.append(gmcan.create_gas_regen_command(self.packer_pt, CanBus.POWERTRAIN, self.apply_gas, idx, c.longActive, at_full_stop))
# Send dashboard UI commands (ACC status), 25hz
if (frame % 4) == 0:
send_fcw = hud_alert == VisualAlert.fcw
can_sends.append(gmcan.create_acc_dashboard_command(self.packer_pt, CanBus.POWERTRAIN, enabled, hud_v_cruise * CV.MS_TO_KPH, hud_show_car, send_fcw))
can_sends.append(gmcan.create_acc_dashboard_command(self.packer_pt, CanBus.POWERTRAIN, c.longActive, hud_v_cruise * CV.MS_TO_KPH, hud_show_car, send_fcw))
# Radar needs to know current speed and yaw rate (50hz),
# and that ADAS is alive (10hz)

7
selfdrive/car/gm/interface.py
View File

@ -222,12 +222,7 @@ class CarInterface(CarInterfaceBase):
if hud_v_cruise > 70:
hud_v_cruise = 0
# For Openpilot, "enabled" includes pre-enable.
# In GM, PCM faults out if ACC command overlaps user gas.
enabled = c.enabled and not self.CS.out.gasPressed
ret = self.CC.update(c, enabled, self.CS, self.frame,
c.actuators,
ret = self.CC.update(c, self.CS, self.frame, c.actuators,
hud_v_cruise, hud_control.lanesVisible,
hud_control.leadVisible, hud_control.visualAlert)

14
selfdrive/car/honda/carcontroller.py
View File

@ -112,12 +112,12 @@ class CarController():
self.params = CarControllerParams(CP)
def update(self, enabled, active, CS, frame, actuators, pcm_cancel_cmd,
def update(self, c, CS, frame, actuators, pcm_cancel_cmd,
hud_v_cruise, hud_show_lanes, hud_show_car, hud_alert):
P = self.params
if active:
if c.longActive:
accel = actuators.accel
gas, brake = compute_gas_brake(actuators.accel, CS.out.vEgo, CS.CP.carFingerprint)
else:
@ -136,7 +136,7 @@ class CarController():
else:
hud_lanes = 0
if enabled:
if c.enabled:
if hud_show_car:
hud_car = 2
else:
@ -152,8 +152,6 @@ class CarController():
# steer torque is converted back to CAN reference (positive when steering right)
apply_steer = int(interp(-actuators.steer * P.STEER_MAX, P.STEER_LOOKUP_BP, P.STEER_LOOKUP_V))
lkas_active = active
# Send CAN commands.
can_sends = []
@ -165,7 +163,7 @@ class CarController():
# Send steering command.
idx = frame % 4
can_sends.append(hondacan.create_steering_control(self.packer, apply_steer,
lkas_active, CS.CP.carFingerprint, idx, CS.CP.openpilotLongitudinalControl))
c.latActive, CS.CP.carFingerprint, idx, CS.CP.openpilotLongitudinalControl))
stopping = actuators.longControlState == LongCtrlState.stopping
@ -217,7 +215,7 @@ class CarController():
if CS.CP.carFingerprint in HONDA_BOSCH:
self.accel = clip(accel, P.BOSCH_ACCEL_MIN, P.BOSCH_ACCEL_MAX)
self.gas = interp(accel, P.BOSCH_GAS_LOOKUP_BP, P.BOSCH_GAS_LOOKUP_V)
can_sends.extend(hondacan.create_acc_commands(self.packer, enabled, active, accel, self.gas, idx, stopping, CS.CP.carFingerprint))
can_sends.extend(hondacan.create_acc_commands(self.packer, c.enabled, c.longActive, accel, self.gas, idx, stopping, CS.CP.carFingerprint))
else:
apply_brake = clip(self.brake_last - wind_brake, 0.0, 1.0)
apply_brake = int(clip(apply_brake * P.NIDEC_BRAKE_MAX, 0, P.NIDEC_BRAKE_MAX - 1))
@ -236,7 +234,7 @@ class CarController():
# This prevents unexpected pedal range rescaling
# Sending non-zero gas when OP is not enabled will cause the PCM not to respond to throttle as expected
# when you do enable.
if active:
if c.longActive:
self.gas = clip(gas_mult * (gas - brake + wind_brake*3/4), 0., 1.)
else:
self.gas = 0.0

8
selfdrive/car/honda/interface.py
View File

@ -427,11 +427,9 @@ class CarInterface(CarInterfaceBase):
else:
hud_v_cruise = 255
ret = self.CC.update(c.enabled, c.active, self.CS, self.frame,
c.actuators,
c.cruiseControl.cancel,
hud_v_cruise,
hud_control.lanesVisible,
ret = self.CC.update(c, self.CS, self.frame,
c.actuators, c.cruiseControl.cancel,
hud_v_cruise, hud_control.lanesVisible,
hud_show_car=hud_control.leadVisible,
hud_alert=hud_control.visualAlert)

19
selfdrive/car/hyundai/carcontroller.py
View File

@ -46,23 +46,20 @@ class CarController():
self.last_resume_frame = 0
self.accel = 0
def update(self, c, enabled, CS, frame, actuators, pcm_cancel_cmd, visual_alert, hud_speed,
def update(self, c, CS, frame, actuators, pcm_cancel_cmd, visual_alert, hud_speed,
left_lane, right_lane, left_lane_depart, right_lane_depart):
# Steering Torque
new_steer = int(round(actuators.steer * self.p.STEER_MAX))
apply_steer = apply_std_steer_torque_limits(new_steer, self.apply_steer_last, CS.out.steeringTorque, self.p)
self.steer_rate_limited = new_steer != apply_steer
# disable when temp fault is active, or below LKA minimum speed
lkas_active = c.active and not CS.out.steerFaultTemporary and CS.out.vEgo >= CS.CP.minSteerSpeed
if not lkas_active:
if not c.latActive:
apply_steer = 0
self.apply_steer_last = apply_steer
sys_warning, sys_state, left_lane_warning, right_lane_warning = \
process_hud_alert(enabled, self.car_fingerprint, visual_alert,
process_hud_alert(c.enabled, self.car_fingerprint, visual_alert,
left_lane, right_lane, left_lane_depart, right_lane_depart)
can_sends = []
@ -72,8 +69,8 @@ class CarController():
if (frame % 100) == 0:
can_sends.append([0x7D0, 0, b"\x02\x3E\x80\x00\x00\x00\x00\x00", 0])
can_sends.append(create_lkas11(self.packer, frame, self.car_fingerprint, apply_steer, lkas_active,
CS.lkas11, sys_warning, sys_state, enabled,
can_sends.append(create_lkas11(self.packer, frame, self.car_fingerprint, apply_steer, c.latActive,
CS.lkas11, sys_warning, sys_state, c.enabled,
left_lane, right_lane,
left_lane_warning, right_lane_warning))
@ -89,7 +86,7 @@ class CarController():
if frame % 2 == 0 and CS.CP.openpilotLongitudinalControl:
lead_visible = False
accel = actuators.accel if c.active else 0
accel = actuators.accel if c.longActive else 0
jerk = clip(2.0 * (accel - CS.out.aEgo), -12.7, 12.7)
@ -100,7 +97,7 @@ class CarController():
stopping = (actuators.longControlState == LongCtrlState.stopping)
set_speed_in_units = hud_speed * (CV.MS_TO_MPH if CS.clu11["CF_Clu_SPEED_UNIT"] == 1 else CV.MS_TO_KPH)
can_sends.extend(create_acc_commands(self.packer, enabled, accel, jerk, int(frame / 2), lead_visible, set_speed_in_units, stopping))
can_sends.extend(create_acc_commands(self.packer, c.enabled, accel, jerk, int(frame / 2), lead_visible, set_speed_in_units, stopping))
self.accel = accel
# 20 Hz LFA MFA message
@ -108,7 +105,7 @@ class CarController():
CAR.IONIQ_EV_2020, CAR.IONIQ_PHEV, CAR.KIA_CEED, CAR.KIA_SELTOS, CAR.KONA_EV,
CAR.ELANTRA_2021, CAR.ELANTRA_HEV_2021, CAR.SONATA_HYBRID, CAR.KONA_HEV, CAR.SANTA_FE_2022,
CAR.KIA_K5_2021, CAR.IONIQ_HEV_2022, CAR.SANTA_FE_HEV_2022, CAR.GENESIS_G70_2020, CAR.SANTA_FE_PHEV_2022):
can_sends.append(create_lfahda_mfc(self.packer, enabled))
can_sends.append(create_lfahda_mfc(self.packer, c.enabled))
# 5 Hz ACC options
if frame % 20 == 0 and CS.CP.openpilotLongitudinalControl:

6
selfdrive/car/hyundai/interface.py
View File

@ -352,8 +352,8 @@ class CarInterface(CarInterfaceBase):
def apply(self, c):
hud_control = c.hudControl
ret = self.CC.update(c, c.enabled, self.CS, self.frame, c.actuators,
c.cruiseControl.cancel, hud_control.visualAlert, hud_control.setSpeed, hud_control.leftLaneVisible,
hud_control.rightLaneVisible, hud_control.leftLaneDepart, hud_control.rightLaneDepart)
ret = self.CC.update(c, self.CS, self.frame, c.actuators, c.cruiseControl.cancel, hud_control.visualAlert,
hud_control.setSpeed, hud_control.leftLaneVisible, hud_control.rightLaneVisible,
hud_control.leftLaneDepart, hud_control.rightLaneDepart)
self.frame += 1
return ret

2
selfdrive/car/mazda/carcontroller.py
View File

@ -19,7 +19,7 @@ class CarController():
apply_steer = 0
self.steer_rate_limited = False
if c.active:
if c.latActive:
# calculate steer and also set limits due to driver torque
new_steer = int(round(c.actuators.steer * CarControllerParams.STEER_MAX))
apply_steer = apply_std_steer_torque_limits(new_steer, self.apply_steer_last,

8
selfdrive/car/nissan/carcontroller.py
View File

@ -18,7 +18,7 @@ class CarController():
self.packer = CANPacker(dbc_name)
def update(self, c, enabled, CS, frame, actuators, cruise_cancel, hud_alert,
def update(self, c, CS, frame, actuators, cruise_cancel, hud_alert,
left_line, right_line, left_lane_depart, right_lane_depart):
can_sends = []
@ -30,7 +30,7 @@ class CarController():
steer_hud_alert = 1 if hud_alert in (VisualAlert.steerRequired, VisualAlert.ldw) else 0
if c.active:
if c.latActive:
# # windup slower
if self.last_angle * apply_angle > 0. and abs(apply_angle) > abs(self.last_angle):
angle_rate_lim = interp(CS.out.vEgo, CarControllerParams.ANGLE_DELTA_BP, CarControllerParams.ANGLE_DELTA_V)
@ -68,12 +68,12 @@ class CarController():
can_sends.append(nissancan.create_cancel_msg(self.packer, CS.cancel_msg, cruise_cancel))
can_sends.append(nissancan.create_steering_control(
self.packer, apply_angle, frame, enabled, self.lkas_max_torque))
self.packer, apply_angle, frame, c.enabled, self.lkas_max_torque))
if lkas_hud_msg and lkas_hud_info_msg:
if frame % 2 == 0:
can_sends.append(nissancan.create_lkas_hud_msg(
self.packer, lkas_hud_msg, enabled, left_line, right_line, left_lane_depart, right_lane_depart))
self.packer, lkas_hud_msg, c.enabled, left_line, right_line, left_lane_depart, right_lane_depart))
if frame % 50 == 0:
can_sends.append(nissancan.create_lkas_hud_info_msg(

2
selfdrive/car/nissan/interface.py
View File

@ -79,7 +79,7 @@ class CarInterface(CarInterfaceBase):
def apply(self, c):
hud_control = c.hudControl
ret = self.CC.update(c, c.enabled, self.CS, self.frame, c.actuators,
ret = self.CC.update(c, self.CS, self.frame, c.actuators,
c.cruiseControl.cancel, hud_control.visualAlert,
hud_control.leftLaneVisible, hud_control.rightLaneVisible,
hud_control.leftLaneDepart, hud_control.rightLaneDepart)

6
selfdrive/car/subaru/carcontroller.py
View File

@ -15,7 +15,7 @@ class CarController():
self.p = CarControllerParams(CP)
self.packer = CANPacker(DBC[CP.carFingerprint]['pt'])
def update(self, c, enabled, CS, frame, actuators, pcm_cancel_cmd, visual_alert, left_line, right_line, left_lane_depart, right_lane_depart):
def update(self, c, CS, frame, actuators, pcm_cancel_cmd, visual_alert, left_line, right_line, left_lane_depart, right_lane_depart):
can_sends = []
@ -30,7 +30,7 @@ class CarController():
apply_steer = apply_std_steer_torque_limits(new_steer, self.apply_steer_last, CS.out.steeringTorque, self.p)
self.steer_rate_limited = new_steer != apply_steer
if not c.active:
if not c.latActive:
apply_steer = 0
if CS.CP.carFingerprint in PREGLOBAL_CARS:
@ -69,7 +69,7 @@ class CarController():
self.es_distance_cnt = CS.es_distance_msg["Counter"]
if self.es_lkas_cnt != CS.es_lkas_msg["Counter"]:
can_sends.append(subarucan.create_es_lkas(self.packer, CS.es_lkas_msg, enabled, visual_alert, left_line, right_line, left_lane_depart, right_lane_depart))
can_sends.append(subarucan.create_es_lkas(self.packer, CS.es_lkas_msg, c.enabled, visual_alert, left_line, right_line, left_lane_depart, right_lane_depart))
self.es_lkas_cnt = CS.es_lkas_msg["Counter"]
new_actuators = actuators.copy()

2
selfdrive/car/subaru/interface.py
View File

@ -123,7 +123,7 @@ class CarInterface(CarInterfaceBase):
def apply(self, c):
hud_control = c.hudControl
ret = self.CC.update(c, c.enabled, self.CS, self.frame, c.actuators,
ret = self.CC.update(c, self.CS, self.frame, c.actuators,
c.cruiseControl.cancel, hud_control.visualAlert,
hud_control.leftLaneVisible, hud_control.rightLaneVisible, hud_control.leftLaneDepart, hud_control.rightLaneDepart)
self.frame += 1

4
selfdrive/car/tesla/carcontroller.py
View File

@ -12,12 +12,12 @@ class CarController():
self.pt_packer = CANPacker(DBC[CP.carFingerprint]['pt'])
self.tesla_can = TeslaCAN(self.packer, self.pt_packer)
def update(self, c, enabled, CS, frame, actuators, cruise_cancel):
def update(self, c, CS, frame, actuators, cruise_cancel):
can_sends = []
# Temp disable steering on a hands_on_fault, and allow for user override
hands_on_fault = (CS.steer_warning == "EAC_ERROR_HANDS_ON" and CS.hands_on_level >= 3)
lkas_enabled = c.active and (not hands_on_fault)
lkas_enabled = c.latActive and (not hands_on_fault)
if lkas_enabled:
apply_angle = actuators.steeringAngleDeg

2
selfdrive/car/tesla/interface.py
View File

@ -71,6 +71,6 @@ class CarInterface(CarInterfaceBase):
return self.CS.out
def apply(self, c):
ret = self.CC.update(c, c.enabled, self.CS, self.frame, c.actuators, c.cruiseControl.cancel)
ret = self.CC.update(c, self.CS, self.frame, c.actuators, c.cruiseControl.cancel)
self.frame += 1
return ret

10
selfdrive/car/toyota/carcontroller.py
View File

@ -22,11 +22,11 @@ class CarController():
self.gas = 0
self.accel = 0
def update(self, enabled, active, CS, frame, actuators, pcm_cancel_cmd, hud_alert,
def update(self, c, CS, frame, actuators, pcm_cancel_cmd, hud_alert,
left_line, right_line, lead, left_lane_depart, right_lane_depart):
# gas and brake
if CS.CP.enableGasInterceptor and active:
if CS.CP.enableGasInterceptor and c.longActive:
MAX_INTERCEPTOR_GAS = 0.5
# RAV4 has very sensitive gas pedal
if CS.CP.carFingerprint in (CAR.RAV4, CAR.RAV4H, CAR.HIGHLANDER, CAR.HIGHLANDERH):
@ -49,7 +49,7 @@ class CarController():
self.steer_rate_limited = new_steer != apply_steer
# Cut steering while we're in a known fault state (2s)
if not active or CS.steer_state in (9, 25):
if not c.latActive or CS.steer_state in (9, 25):
apply_steer = 0
apply_steer_req = 0
else:
@ -57,7 +57,7 @@ class CarController():
# TODO: probably can delete this. CS.pcm_acc_status uses a different signal
# than CS.cruiseState.enabled. confirm they're not meaningfully different
if not enabled and CS.pcm_acc_status:
if not c.enabled and CS.pcm_acc_status:
pcm_cancel_cmd = 1
# on entering standstill, send standstill request
@ -122,7 +122,7 @@ class CarController():
send_ui = True
if (frame % 100 == 0 or send_ui):
can_sends.append(create_ui_command(self.packer, steer_alert, pcm_cancel_cmd, left_line, right_line, left_lane_depart, right_lane_depart, enabled))
can_sends.append(create_ui_command(self.packer, steer_alert, pcm_cancel_cmd, left_line, right_line, left_lane_depart, right_lane_depart, c.enabled))
if frame % 100 == 0 and CS.CP.enableDsu:
can_sends.append(create_fcw_command(self.packer, fcw_alert))

2
selfdrive/car/toyota/interface.py
View File

@ -273,7 +273,7 @@ class CarInterface(CarInterfaceBase):
# to be called @ 100hz
def apply(self, c):
hud_control = c.hudControl
ret = self.CC.update(c.enabled, c.active, self.CS, self.frame,
ret = self.CC.update(c, self.CS, self.frame,
c.actuators, c.cruiseControl.cancel,
hud_control.visualAlert, hud_control.leftLaneVisible,
hud_control.rightLaneVisible, hud_control.leadVisible,

8
selfdrive/car/volkswagen/carcontroller.py
View File

@ -21,7 +21,7 @@ class CarController():
self.steer_rate_limited = False
def update(self, c, enabled, CS, frame, ext_bus, actuators, visual_alert, left_lane_visible, right_lane_visible, left_lane_depart, right_lane_depart):
def update(self, c, CS, frame, ext_bus, actuators, visual_alert, left_lane_visible, right_lane_visible, left_lane_depart, right_lane_depart):
""" Controls thread """
can_sends = []
@ -39,7 +39,7 @@ class CarController():
# torque value. Do that anytime we happen to have 0 torque, or failing that,
# when exceeding ~1/3 the 360 second timer.
if c.active and CS.out.vEgo > CS.CP.minSteerSpeed and not (CS.out.standstill or CS.out.steerFaultPermanent or CS.out.steerFaultTemporary):
if c.latActive:
new_steer = int(round(actuators.steer * P.STEER_MAX))
apply_steer = apply_std_steer_torque_limits(new_steer, self.apply_steer_last, CS.out.steeringTorque, P)
self.steer_rate_limited = new_steer != apply_steer
@ -77,7 +77,7 @@ class CarController():
else:
hud_alert = MQB_LDW_MESSAGES["none"]
can_sends.append(volkswagencan.create_mqb_hud_control(self.packer_pt, CANBUS.pt, enabled,
can_sends.append(volkswagencan.create_mqb_hud_control(self.packer_pt, CANBUS.pt, c.enabled,
CS.out.steeringPressed, hud_alert, left_lane_visible,
right_lane_visible, CS.ldw_stock_values,
left_lane_depart, right_lane_depart))
@ -92,7 +92,7 @@ class CarController():
# Cancel ACC if it's engaged with OP disengaged.
self.graButtonStatesToSend = BUTTON_STATES.copy()
self.graButtonStatesToSend["cancel"] = True
elif enabled and CS.esp_hold_confirmation:
elif c.enabled and CS.esp_hold_confirmation:
# Blip the Resume button if we're engaged at standstill.
# FIXME: This is a naive implementation, improve with visiond or radar input.
self.graButtonStatesToSend = BUTTON_STATES.copy()

2
selfdrive/car/volkswagen/interface.py
View File

@ -211,7 +211,7 @@ class CarInterface(CarInterfaceBase):
def apply(self, c):
hud_control = c.hudControl
ret = self.CC.update(c, c.enabled, self.CS, self.frame, self.ext_bus, c.actuators,
ret = self.CC.update(c, self.CS, self.frame, self.ext_bus, c.actuators,
hud_control.visualAlert,
hud_control.leftLaneVisible,
hud_control.rightLaneVisible,

57
selfdrive/controls/controlsd.py
View File

@ -459,7 +459,7 @@ class Controls:
self.v_cruise_kph = initialize_v_cruise(CS.vEgo, CS.buttonEvents, self.v_cruise_kph_last)
# 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:
self.current_alert_types.append(ET.WARNING)
@ -467,7 +467,7 @@ class Controls:
self.enabled = self.active or self.state == State.preEnabled
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
params = self.sm['liveParameters']
@ -478,7 +478,14 @@ class Controls:
lat_plan = self.sm['lateralPlan']
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
if CS.leftBlinker or CS.rightBlinker:
@ -486,37 +493,40 @@ class Controls:
# State specific actions
if not self.active:
if not CC.latActive:
self.LaC.reset()
if not CC.longActive:
self.LoC.reset(v_pid=CS.vEgo)
if not self.joystick_mode:
# accel PID loop
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
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
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,
lat_plan.psis,
lat_plan.curvatures,
lat_plan.curvatureRates)
actuators.steer, actuators.steeringAngleDeg, lac_log = self.LaC.update(lat_active, CS, self.CP, self.VM, params, self.last_actuators,
desired_curvature, desired_curvature_rate)
actuators.steer, actuators.steeringAngleDeg, lac_log = self.LaC.update(CC.latActive, CS, self.CP, self.VM,
params, self.last_actuators, desired_curvature,
desired_curvature_rate)
else:
lac_log = log.ControlsState.LateralDebugState.new_message()
if self.sm.rcv_frame['testJoystick'] > 0 and self.active:
actuators.accel = 4.0*clip(self.sm['testJoystick'].axes[0], -1, 1)
if self.sm.rcv_frame['testJoystick'] > 0:
if CC.longActive:
actuators.accel = 4.0*clip(self.sm['testJoystick'].axes[0], -1, 1)
steer = clip(self.sm['testJoystick'].axes[1], -1, 1)
# max angle is 45 for angle-based cars
actuators.steer, actuators.steeringAngleDeg = steer, steer * 45.
if CC.latActive:
steer = clip(self.sm['testJoystick'].axes[1], -1, 1)
# 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.output = steer
lac_log.saturated = abs(steer) >= 0.9
lac_log.output = actuators.steer
lac_log.saturated = abs(actuators.steer) >= 0.9
# Send a "steering required alert" if saturation count has reached the limit
if lac_log.active and lac_log.saturated and not CS.steeringPressed:
@ -540,7 +550,7 @@ class Controls:
cloudlog.error(f"actuators.{p} not finite {actuators.to_dict()}")
setattr(actuators, p, 0.0)
return actuators, lac_log
return CC, lac_log
def update_button_timers(self, buttonEvents):
# increment timer for buttons still pressed
@ -552,14 +562,9 @@ class Controls:
if b.type.raw in self.button_timers:
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"""
CC = car.CarControl.new_message()
CC.enabled = self.enabled
CC.active = self.active
CC.actuators = actuators
orientation_value = self.sm['liveLocationKalman'].orientationNED.value
if len(orientation_value) > 2:
CC.roll = orientation_value[0]
@ -580,7 +585,7 @@ class Controls:
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 \
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']
desire_prediction = model_v2.meta.desirePrediction
@ -719,12 +724,12 @@ class Controls:
self.prof.checkpoint("State transition")
# 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")
# 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.update_button_timers(CS.buttonEvents)

2
selfdrive/test/process_replay/ref_commit
View File

@ -1 +1 @@
595f118aa4f6eebacaef4db77179c108ec725483
c6d7afb13c183852f8d61d5f79495b828ed401ed