Log actuators after applying rate limits in CarController (#23230)
* return actuators from carcontroller * log it * pass to latcontrol * chrysler * gm * honda * more brands * rest of the brands * gm cleanup * hyundai cleanup * update ref * rename field * fix subaru * add types * more subaru fixes #23240 * consistent whitespace * bump cerealpull/23246/head
Willem Melching
GitHub
parent
a793b949be
commit
4f1eb4278a
2
cereal
2
cereal
|
|
@ -1 +1 @@
|
|||
Subproject commit e5a04ab458afd52cf630cc9e35ccdc10efba6688
|
||||
Subproject commit f1c5c8ef7cab05b66892d67b6992f5ac1238ad4f
|
||||
|
|
@ -1,3 +1,4 @@
|
|||
from cereal import car
|
||||
from selfdrive.car import apply_toyota_steer_torque_limits
|
||||
from selfdrive.car.chrysler.chryslercan import create_lkas_hud, create_lkas_command, \
|
||||
create_wheel_buttons
|
||||
|
|
@ -20,9 +21,8 @@ class CarController():
|
|||
# this seems needed to avoid steering faults and to force the sync with the EPS counter
|
||||
frame = CS.lkas_counter
|
||||
if self.prev_frame == frame:
|
||||
return []
|
||||
return car.CarControl.Actuators.new_message(), []
|
||||
|
||||
# *** compute control surfaces ***
|
||||
# steer torque
|
||||
new_steer = int(round(actuators.steer * CarControllerParams.STEER_MAX))
|
||||
apply_steer = apply_toyota_steer_torque_limits(new_steer, self.apply_steer_last,
|
||||
|
|
@ -67,4 +67,7 @@ class CarController():
|
|||
self.ccframe += 1
|
||||
self.prev_frame = frame
|
||||
|
||||
return can_sends
|
||||
new_actuators = actuators.copy()
|
||||
new_actuators.steer = apply_steer / CarControllerParams.STEER_MAX
|
||||
|
||||
return new_actuators, can_sends
|
||||
|
|
|
|||
|
|
@ -78,8 +78,6 @@ class CarInterface(CarInterfaceBase):
|
|||
def apply(self, c):
|
||||
|
||||
if (self.CS.frame == -1):
|
||||
return [] # if we haven't seen a frame 220, then do not update.
|
||||
return car.CarControl.Actuators.new_message(), [] # if we haven't seen a frame 220, then do not update.
|
||||
|
||||
can_sends = self.CC.update(c.enabled, self.CS, c.actuators, c.cruiseControl.cancel, c.hudControl.visualAlert)
|
||||
|
||||
return can_sends
|
||||
return self.CC.update(c.enabled, self.CS, c.actuators, c.cruiseControl.cancel, c.hudControl.visualAlert)
|
||||
|
|
|
|||
|
|
@ -83,4 +83,4 @@ class CarController():
|
|||
self.main_on_last = CS.out.cruiseState.available
|
||||
self.steer_alert_last = steer_alert
|
||||
|
||||
return can_sends
|
||||
return actuators, can_sends
|
||||
|
|
|
|||
|
|
@ -63,8 +63,8 @@ class CarInterface(CarInterfaceBase):
|
|||
# to be called @ 100hz
|
||||
def apply(self, c):
|
||||
|
||||
can_sends = self.CC.update(c.enabled, self.CS, self.frame, c.actuators,
|
||||
ret = self.CC.update(c.enabled, self.CS, self.frame, c.actuators,
|
||||
c.hudControl.visualAlert, c.cruiseControl.cancel)
|
||||
|
||||
self.frame += 1
|
||||
return can_sends
|
||||
return ret
|
||||
|
|
|
|||
|
|
@ -14,6 +14,9 @@ class CarController():
|
|||
def __init__(self, dbc_name, CP, VM):
|
||||
self.start_time = 0.
|
||||
self.apply_steer_last = 0
|
||||
self.apply_gas = 0
|
||||
self.apply_brake = 0
|
||||
|
||||
self.lka_steering_cmd_counter_last = -1
|
||||
self.lka_icon_status_last = (False, False)
|
||||
self.steer_rate_limited = False
|
||||
|
|
@ -53,22 +56,22 @@ class CarController():
|
|||
|
||||
can_sends.append(gmcan.create_steering_control(self.packer_pt, CanBus.POWERTRAIN, apply_steer, idx, lkas_enabled))
|
||||
|
||||
if not enabled:
|
||||
# Stock ECU sends max regen when not enabled.
|
||||
apply_gas = P.MAX_ACC_REGEN
|
||||
apply_brake = 0
|
||||
else:
|
||||
apply_gas = int(round(interp(actuators.accel, P.GAS_LOOKUP_BP, P.GAS_LOOKUP_V)))
|
||||
apply_brake = int(round(interp(actuators.accel, P.BRAKE_LOOKUP_BP, P.BRAKE_LOOKUP_V)))
|
||||
|
||||
# Gas/regen and brakes - all at 25Hz
|
||||
if (frame % 4) == 0:
|
||||
if not enabled:
|
||||
# Stock ECU sends max regen when not enabled.
|
||||
self.apply_gas = P.MAX_ACC_REGEN
|
||||
self.apply_brake = 0
|
||||
else:
|
||||
self.apply_gas = int(round(interp(actuators.accel, P.GAS_LOOKUP_BP, P.GAS_LOOKUP_V)))
|
||||
self.apply_brake = int(round(interp(actuators.accel, P.BRAKE_LOOKUP_BP, P.BRAKE_LOOKUP_V)))
|
||||
|
||||
idx = (frame // 4) % 4
|
||||
|
||||
at_full_stop = enabled and CS.out.standstill
|
||||
near_stop = enabled and (CS.out.vEgo < P.NEAR_STOP_BRAKE_PHASE)
|
||||
can_sends.append(gmcan.create_friction_brake_command(self.packer_ch, CanBus.CHASSIS, apply_brake, idx, near_stop, at_full_stop))
|
||||
can_sends.append(gmcan.create_gas_regen_command(self.packer_pt, CanBus.POWERTRAIN, apply_gas, idx, enabled, at_full_stop))
|
||||
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))
|
||||
|
||||
# Send dashboard UI commands (ACC status), 25hz
|
||||
if (frame % 4) == 0:
|
||||
|
|
@ -106,4 +109,9 @@ class CarController():
|
|||
can_sends.append(gmcan.create_lka_icon_command(CanBus.SW_GMLAN, lka_active, lka_critical, steer_alert))
|
||||
self.lka_icon_status_last = lka_icon_status
|
||||
|
||||
return can_sends
|
||||
new_actuators = actuators.copy()
|
||||
new_actuators.steer = self.apply_steer_last / P.STEER_MAX
|
||||
new_actuators.gas = self.apply_gas
|
||||
new_actuators.brake = self.apply_brake
|
||||
|
||||
return new_actuators, can_sends
|
||||
|
|
|
|||
|
|
@ -220,10 +220,10 @@ class CarInterface(CarInterfaceBase):
|
|||
# In GM, PCM faults out if ACC command overlaps user gas.
|
||||
enabled = c.enabled and not self.CS.out.gasPressed
|
||||
|
||||
can_sends = self.CC.update(enabled, self.CS, self.frame,
|
||||
c.actuators,
|
||||
hud_v_cruise, c.hudControl.lanesVisible,
|
||||
c.hudControl.leadVisible, c.hudControl.visualAlert)
|
||||
ret = self.CC.update(enabled, self.CS, self.frame,
|
||||
c.actuators,
|
||||
hud_v_cruise, c.hudControl.lanesVisible,
|
||||
c.hudControl.leadVisible, c.hudControl.visualAlert)
|
||||
|
||||
self.frame += 1
|
||||
return can_sends
|
||||
return ret
|
||||
|
|
|
|||
|
|
@ -105,6 +105,11 @@ class CarController():
|
|||
self.last_pump_ts = 0.
|
||||
self.packer = CANPacker(dbc_name)
|
||||
|
||||
self.accel = 0
|
||||
self.speed = 0
|
||||
self.gas = 0
|
||||
self.brake = 0
|
||||
|
||||
self.params = CarControllerParams(CP)
|
||||
|
||||
def update(self, enabled, active, CS, frame, actuators, pcm_cancel_cmd,
|
||||
|
|
@ -211,10 +216,9 @@ class CarController():
|
|||
ts = frame * DT_CTRL
|
||||
|
||||
if CS.CP.carFingerprint in HONDA_BOSCH:
|
||||
accel = clip(accel, P.BOSCH_ACCEL_MIN, P.BOSCH_ACCEL_MAX)
|
||||
bosch_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, bosch_gas, idx, stopping, starting, CS.CP.carFingerprint))
|
||||
|
||||
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, starting, 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))
|
||||
|
|
@ -224,6 +228,7 @@ class CarController():
|
|||
can_sends.append(hondacan.create_brake_command(self.packer, apply_brake, pump_on,
|
||||
pcm_override, pcm_cancel_cmd, fcw_display, idx, CS.CP.carFingerprint, CS.stock_brake))
|
||||
self.apply_brake_last = apply_brake
|
||||
self.brake = apply_brake / P.NIDEC_BRAKE_MAX
|
||||
|
||||
if CS.CP.enableGasInterceptor:
|
||||
# way too aggressive at low speed without this
|
||||
|
|
@ -233,17 +238,28 @@ class CarController():
|
|||
# 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:
|
||||
apply_gas = clip(gas_mult * (gas - brake + wind_brake*3/4), 0., 1.)
|
||||
self.gas = clip(gas_mult * (gas - brake + wind_brake*3/4), 0., 1.)
|
||||
else:
|
||||
apply_gas = 0.0
|
||||
can_sends.append(create_gas_interceptor_command(self.packer, apply_gas, idx))
|
||||
|
||||
hud = HUDData(int(pcm_accel), int(round(hud_v_cruise)), hud_car,
|
||||
hud_lanes, fcw_display, acc_alert, steer_required)
|
||||
self.gas = 0.0
|
||||
can_sends.append(create_gas_interceptor_command(self.packer, self.gas, idx))
|
||||
|
||||
# Send dashboard UI commands.
|
||||
if (frame % 10) == 0:
|
||||
idx = (frame//10) % 4
|
||||
hud = HUDData(int(pcm_accel), int(round(hud_v_cruise)), hud_car,
|
||||
hud_lanes, fcw_display, acc_alert, steer_required)
|
||||
can_sends.extend(hondacan.create_ui_commands(self.packer, CS.CP, pcm_speed, hud, CS.is_metric, idx, CS.stock_hud))
|
||||
|
||||
return can_sends
|
||||
if (CS.CP.openpilotLongitudinalControl) and (CS.CP.carFingerprint not in HONDA_BOSCH):
|
||||
self.speed = pcm_speed
|
||||
|
||||
if not CS.CP.enableGasInterceptor:
|
||||
self.gas = pcm_accel / 0xc6
|
||||
|
||||
new_actuators = actuators.copy()
|
||||
new_actuators.speed = self.speed
|
||||
new_actuators.accel = self.accel
|
||||
new_actuators.gas = self.gas
|
||||
new_actuators.brake = self.brake
|
||||
|
||||
return new_actuators, can_sends
|
||||
|
|
|
|||
|
|
@ -438,13 +438,13 @@ class CarInterface(CarInterfaceBase):
|
|||
else:
|
||||
hud_v_cruise = 255
|
||||
|
||||
can_sends = self.CC.update(c.enabled, c.active, self.CS, self.frame,
|
||||
c.actuators,
|
||||
c.cruiseControl.cancel,
|
||||
hud_v_cruise,
|
||||
c.hudControl.lanesVisible,
|
||||
hud_show_car=c.hudControl.leadVisible,
|
||||
hud_alert=c.hudControl.visualAlert)
|
||||
ret = self.CC.update(c.enabled, c.active, self.CS, self.frame,
|
||||
c.actuators,
|
||||
c.cruiseControl.cancel,
|
||||
hud_v_cruise,
|
||||
c.hudControl.lanesVisible,
|
||||
hud_show_car=c.hudControl.leadVisible,
|
||||
hud_alert=c.hudControl.visualAlert)
|
||||
|
||||
self.frame += 1
|
||||
return can_sends
|
||||
return ret
|
||||
|
|
|
|||
|
|
@ -44,6 +44,7 @@ class CarController():
|
|||
self.car_fingerprint = CP.carFingerprint
|
||||
self.steer_rate_limited = False
|
||||
self.last_resume_frame = 0
|
||||
self.accel = 0
|
||||
|
||||
def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd, visual_alert, hud_speed,
|
||||
left_lane, right_lane, left_lane_depart, right_lane_depart):
|
||||
|
|
@ -100,6 +101,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))
|
||||
self.accel = accel
|
||||
|
||||
# 20 Hz LFA MFA message
|
||||
if frame % 5 == 0 and self.car_fingerprint in [CAR.SONATA, CAR.PALISADE, CAR.IONIQ, CAR.KIA_NIRO_EV, CAR.KIA_NIRO_HEV_2021,
|
||||
|
|
@ -116,4 +118,8 @@ class CarController():
|
|||
if frame % 50 == 0 and CS.CP.openpilotLongitudinalControl:
|
||||
can_sends.append(create_frt_radar_opt(self.packer))
|
||||
|
||||
return can_sends
|
||||
new_actuators = actuators.copy()
|
||||
new_actuators.steer = apply_steer / self.p.STEER_MAX
|
||||
new_actuators.accel = self.accel
|
||||
|
||||
return new_actuators, can_sends
|
||||
|
|
|
|||
|
|
@ -347,8 +347,8 @@ class CarInterface(CarInterfaceBase):
|
|||
return self.CS.out
|
||||
|
||||
def apply(self, c):
|
||||
can_sends = self.CC.update(c.enabled, self.CS, self.frame, c.actuators,
|
||||
c.cruiseControl.cancel, c.hudControl.visualAlert, c.hudControl.setSpeed, c.hudControl.leftLaneVisible,
|
||||
c.hudControl.rightLaneVisible, c.hudControl.leftLaneDepart, c.hudControl.rightLaneDepart)
|
||||
ret = self.CC.update(c.enabled, self.CS, self.frame, c.actuators,
|
||||
c.cruiseControl.cancel, c.hudControl.visualAlert, c.hudControl.setSpeed, c.hudControl.leftLaneVisible,
|
||||
c.hudControl.rightLaneVisible, c.hudControl.leftLaneDepart, c.hudControl.rightLaneDepart)
|
||||
self.frame += 1
|
||||
return can_sends
|
||||
return ret
|
||||
|
|
|
|||
|
|
@ -1,6 +1,7 @@
|
|||
import os
|
||||
import time
|
||||
from typing import Dict
|
||||
from abc import abstractmethod
|
||||
from typing import Dict, Tuple, List
|
||||
|
||||
from cereal import car
|
||||
from common.kalman.simple_kalman import KF1D
|
||||
|
|
@ -48,8 +49,9 @@ class CarInterfaceBase():
|
|||
return ACCEL_MIN, ACCEL_MAX
|
||||
|
||||
@staticmethod
|
||||
@abstractmethod
|
||||
def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None):
|
||||
raise NotImplementedError
|
||||
pass
|
||||
|
||||
@staticmethod
|
||||
def init(CP, logcan, sendcan):
|
||||
|
|
@ -99,13 +101,13 @@ class CarInterfaceBase():
|
|||
ret.longitudinalActuatorDelayUpperBound = 0.15
|
||||
return ret
|
||||
|
||||
# returns a car.CarState, pass in car.CarControl
|
||||
def update(self, c, can_strings):
|
||||
raise NotImplementedError
|
||||
@abstractmethod
|
||||
def update(self, c: car.CarControl, can_strings: List[bytes]) -> car.CarState:
|
||||
pass
|
||||
|
||||
# return sendcan, pass in a car.CarControl
|
||||
def apply(self, c):
|
||||
raise NotImplementedError
|
||||
@abstractmethod
|
||||
def apply(self, c: car.CarControl) -> Tuple[car.CarControl.Actuators, List[bytes]]:
|
||||
pass
|
||||
|
||||
def create_common_events(self, cs_out, extra_gears=None, gas_resume_speed=-1, pcm_enable=True):
|
||||
events = Events()
|
||||
|
|
|
|||
|
|
@ -58,4 +58,8 @@ class CarController():
|
|||
# send steering command
|
||||
can_sends.append(mazdacan.create_steering_control(self.packer, CS.CP.carFingerprint,
|
||||
frame, apply_steer, CS.cam_lkas))
|
||||
return can_sends
|
||||
|
||||
new_actuators = c.actuators.copy()
|
||||
new_actuators.steer = apply_steer / CarControllerParams.STEER_MAX
|
||||
|
||||
return new_actuators, can_sends
|
||||
|
|
|
|||
|
|
@ -95,6 +95,6 @@ class CarInterface(CarInterfaceBase):
|
|||
return self.CS.out
|
||||
|
||||
def apply(self, c):
|
||||
can_sends = self.CC.update(c, self.CS, self.frame)
|
||||
ret = self.CC.update(c, self.CS, self.frame)
|
||||
self.frame += 1
|
||||
return can_sends
|
||||
return ret
|
||||
|
|
|
|||
|
|
@ -88,4 +88,5 @@ class CarInterface(CarInterfaceBase):
|
|||
|
||||
def apply(self, c):
|
||||
# in mock no carcontrols
|
||||
return []
|
||||
actuators = car.CarControl.Actuators.new_message()
|
||||
return actuators, []
|
||||
|
|
|
|||
|
|
@ -87,4 +87,7 @@ class CarController():
|
|||
self.packer, lkas_hud_info_msg, steer_hud_alert
|
||||
))
|
||||
|
||||
return can_sends
|
||||
new_actuators = actuators.copy()
|
||||
new_actuators.steeringAngleDeg = apply_angle
|
||||
|
||||
return new_actuators, can_sends
|
||||
|
|
|
|||
|
|
@ -78,9 +78,9 @@ class CarInterface(CarInterfaceBase):
|
|||
return self.CS.out
|
||||
|
||||
def apply(self, c):
|
||||
can_sends = self.CC.update(c.enabled, self.CS, self.frame, c.actuators,
|
||||
c.cruiseControl.cancel, c.hudControl.visualAlert,
|
||||
c.hudControl.leftLaneVisible, c.hudControl.rightLaneVisible,
|
||||
c.hudControl.leftLaneDepart, c.hudControl.rightLaneDepart)
|
||||
ret = self.CC.update(c.enabled, self.CS, self.frame, c.actuators,
|
||||
c.cruiseControl.cancel, c.hudControl.visualAlert,
|
||||
c.hudControl.leftLaneVisible, c.hudControl.rightLaneVisible,
|
||||
c.hudControl.leftLaneDepart, c.hudControl.rightLaneDepart)
|
||||
self.frame += 1
|
||||
return can_sends
|
||||
return ret
|
||||
|
|
|
|||
|
|
@ -72,4 +72,7 @@ class CarController():
|
|||
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))
|
||||
self.es_lkas_cnt = CS.es_lkas_msg["Counter"]
|
||||
|
||||
return can_sends
|
||||
new_actuators = actuators.copy()
|
||||
new_actuators.steer = self.apply_steer_last / self.p.STEER_MAX
|
||||
|
||||
return new_actuators, can_sends
|
||||
|
|
|
|||
|
|
@ -122,8 +122,8 @@ class CarInterface(CarInterfaceBase):
|
|||
return self.CS.out
|
||||
|
||||
def apply(self, c):
|
||||
can_sends = self.CC.update(c.enabled, self.CS, self.frame, c.actuators,
|
||||
c.cruiseControl.cancel, c.hudControl.visualAlert,
|
||||
c.hudControl.leftLaneVisible, c.hudControl.rightLaneVisible, c.hudControl.leftLaneDepart, c.hudControl.rightLaneDepart)
|
||||
ret = self.CC.update(c.enabled, self.CS, self.frame, c.actuators,
|
||||
c.cruiseControl.cancel, c.hudControl.visualAlert,
|
||||
c.hudControl.leftLaneVisible, c.hudControl.rightLaneVisible, c.hudControl.leftLaneDepart, c.hudControl.rightLaneDepart)
|
||||
self.frame += 1
|
||||
return can_sends
|
||||
return ret
|
||||
|
|
|
|||
|
|
@ -62,4 +62,7 @@ class CarController():
|
|||
|
||||
# TODO: HUD control
|
||||
|
||||
return can_sends
|
||||
new_actuators = actuators.copy()
|
||||
new_actuators.steeringAngleDeg = apply_angle
|
||||
|
||||
return actuators, can_sends
|
||||
|
|
|
|||
|
|
@ -71,6 +71,6 @@ class CarInterface(CarInterfaceBase):
|
|||
return self.CS.out
|
||||
|
||||
def apply(self, c):
|
||||
can_sends = self.CC.update(c.enabled, self.CS, self.frame, c.actuators, c.cruiseControl.cancel)
|
||||
ret = self.CC.update(c.enabled, self.CS, self.frame, c.actuators, c.cruiseControl.cancel)
|
||||
self.frame += 1
|
||||
return can_sends
|
||||
return ret
|
||||
|
|
|
|||
|
|
@ -19,12 +19,12 @@ class CarController():
|
|||
self.steer_rate_limited = False
|
||||
|
||||
self.packer = CANPacker(dbc_name)
|
||||
self.gas = 0
|
||||
self.accel = 0
|
||||
|
||||
def update(self, enabled, active, CS, frame, actuators, pcm_cancel_cmd, hud_alert,
|
||||
left_line, right_line, lead, left_lane_depart, right_lane_depart):
|
||||
|
||||
# *** compute control surfaces ***
|
||||
|
||||
# gas and brake
|
||||
if CS.CP.enableGasInterceptor and enabled:
|
||||
MAX_INTERCEPTOR_GAS = 0.5
|
||||
|
|
@ -96,6 +96,7 @@ class CarController():
|
|||
can_sends.append(create_acc_cancel_command(self.packer))
|
||||
elif CS.CP.openpilotLongitudinalControl:
|
||||
can_sends.append(create_accel_command(self.packer, pcm_accel_cmd, pcm_cancel_cmd, self.standstill_req, lead, CS.acc_type))
|
||||
self.accel = pcm_accel_cmd
|
||||
else:
|
||||
can_sends.append(create_accel_command(self.packer, 0, pcm_cancel_cmd, False, lead, CS.acc_type))
|
||||
|
||||
|
|
@ -103,6 +104,7 @@ class CarController():
|
|||
# send exactly zero if gas cmd is zero. Interceptor will send the max between read value and gas cmd.
|
||||
# This prevents unexpected pedal range rescaling
|
||||
can_sends.append(create_gas_interceptor_command(self.packer, interceptor_gas_cmd, frame // 2))
|
||||
self.gas = interceptor_gas_cmd
|
||||
|
||||
# ui mesg is at 100Hz but we send asap if:
|
||||
# - there is something to display
|
||||
|
|
@ -125,10 +127,14 @@ class CarController():
|
|||
if frame % 100 == 0 and CS.CP.enableDsu:
|
||||
can_sends.append(create_fcw_command(self.packer, fcw_alert))
|
||||
|
||||
#*** static msgs ***
|
||||
|
||||
# *** static msgs ***
|
||||
for (addr, cars, bus, fr_step, vl) in STATIC_DSU_MSGS:
|
||||
if frame % fr_step == 0 and CS.CP.enableDsu and CS.CP.carFingerprint in cars:
|
||||
can_sends.append(make_can_msg(addr, vl, bus))
|
||||
|
||||
return can_sends
|
||||
new_actuators = actuators.copy()
|
||||
new_actuators.steer = apply_steer / CarControllerParams.STEER_MAX
|
||||
new_actuators.accel = self.accel
|
||||
new_actuators.gas = self.gas
|
||||
|
||||
return new_actuators, can_sends
|
||||
|
|
|
|||
|
|
@ -312,12 +312,11 @@ class CarInterface(CarInterfaceBase):
|
|||
# pass in a car.CarControl
|
||||
# to be called @ 100hz
|
||||
def apply(self, c):
|
||||
|
||||
can_sends = self.CC.update(c.enabled, c.active, self.CS, self.frame,
|
||||
c.actuators, c.cruiseControl.cancel,
|
||||
c.hudControl.visualAlert, c.hudControl.leftLaneVisible,
|
||||
c.hudControl.rightLaneVisible, c.hudControl.leadVisible,
|
||||
c.hudControl.leftLaneDepart, c.hudControl.rightLaneDepart)
|
||||
ret = self.CC.update(c.enabled, c.active, self.CS, self.frame,
|
||||
c.actuators, c.cruiseControl.cancel,
|
||||
c.hudControl.visualAlert, c.hudControl.leftLaneVisible,
|
||||
c.hudControl.rightLaneVisible, c.hudControl.leadVisible,
|
||||
c.hudControl.leftLaneDepart, c.hudControl.rightLaneDepart)
|
||||
|
||||
self.frame += 1
|
||||
return can_sends
|
||||
return ret
|
||||
|
|
|
|||
|
|
@ -110,4 +110,7 @@ class CarController():
|
|||
self.graButtonStatesToSend = None
|
||||
self.graMsgSentCount = 0
|
||||
|
||||
return can_sends
|
||||
new_actuators = actuators.copy()
|
||||
new_actuators.steer = self.apply_steer_last / P.STEER_MAX
|
||||
|
||||
return new_actuators, can_sends
|
||||
|
|
|
|||
|
|
@ -216,11 +216,11 @@ class CarInterface(CarInterfaceBase):
|
|||
return self.CS.out
|
||||
|
||||
def apply(self, c):
|
||||
can_sends = self.CC.update(c.enabled, self.CS, self.frame, self.ext_bus, c.actuators,
|
||||
c.hudControl.visualAlert,
|
||||
c.hudControl.leftLaneVisible,
|
||||
c.hudControl.rightLaneVisible,
|
||||
c.hudControl.leftLaneDepart,
|
||||
c.hudControl.rightLaneDepart)
|
||||
ret = self.CC.update(c.enabled, self.CS, self.frame, self.ext_bus, c.actuators,
|
||||
c.hudControl.visualAlert,
|
||||
c.hudControl.leftLaneVisible,
|
||||
c.hudControl.rightLaneVisible,
|
||||
c.hudControl.leftLaneDepart,
|
||||
c.hudControl.rightLaneDepart)
|
||||
self.frame += 1
|
||||
return can_sends
|
||||
return ret
|
||||
|
|
|
|||
|
|
@ -159,6 +159,7 @@ class Controls:
|
|||
self.current_alert_types = [ET.PERMANENT]
|
||||
self.logged_comm_issue = False
|
||||
self.button_timers = {ButtonEvent.Type.decelCruise: 0, ButtonEvent.Type.accelCruise: 0}
|
||||
self.last_actuators = car.CarControl.Actuators.new_message()
|
||||
|
||||
# TODO: no longer necessary, aside from process replay
|
||||
self.sm['liveParameters'].valid = True
|
||||
|
|
@ -510,7 +511,7 @@ class Controls:
|
|||
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,
|
||||
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)
|
||||
else:
|
||||
lac_log = log.ControlsState.LateralDebugState.new_message()
|
||||
|
|
@ -621,8 +622,9 @@ class Controls:
|
|||
|
||||
if not self.read_only and self.initialized:
|
||||
# send car controls over can
|
||||
can_sends = self.CI.apply(CC)
|
||||
self.last_actuators, can_sends = self.CI.apply(CC)
|
||||
self.pm.send('sendcan', can_list_to_can_capnp(can_sends, msgtype='sendcan', valid=CS.canValid))
|
||||
CC.actuatorsOutput = self.last_actuators
|
||||
|
||||
force_decel = (self.sm['driverMonitoringState'].awarenessStatus < 0.) or \
|
||||
(self.state == State.softDisabling)
|
||||
|
|
|
|||
|
|
@ -10,7 +10,7 @@ class LatControlAngle():
|
|||
def reset(self):
|
||||
pass
|
||||
|
||||
def update(self, active, CS, CP, VM, params, desired_curvature, desired_curvature_rate):
|
||||
def update(self, active, CS, CP, VM, params, last_actuators, desired_curvature, desired_curvature_rate):
|
||||
angle_log = log.ControlsState.LateralAngleState.new_message()
|
||||
|
||||
if CS.vEgo < 0.3 or not active:
|
||||
|
|
@ -23,6 +23,6 @@ class LatControlAngle():
|
|||
|
||||
angle_log.saturated = False
|
||||
angle_log.steeringAngleDeg = float(CS.steeringAngleDeg)
|
||||
angle_log.steeringAngleDesiredDeg = angle_steers_des
|
||||
angle_log.steeringAngleDesiredDeg = angle_steers_des
|
||||
|
||||
return 0, float(angle_steers_des), angle_log
|
||||
|
|
|
|||
|
|
@ -82,7 +82,7 @@ class LatControlINDI():
|
|||
|
||||
return self.sat_count > self.sat_limit
|
||||
|
||||
def update(self, active, CS, CP, VM, params, curvature, curvature_rate):
|
||||
def update(self, active, CS, CP, VM, params, last_actuators, curvature, curvature_rate):
|
||||
self.speed = CS.vEgo
|
||||
# Update Kalman filter
|
||||
y = np.array([[math.radians(CS.steeringAngleDeg)], [math.radians(CS.steeringRateDeg)]])
|
||||
|
|
|
|||
|
|
@ -44,7 +44,7 @@ class LatControlLQR():
|
|||
|
||||
return self.sat_count > self.sat_limit
|
||||
|
||||
def update(self, active, CS, CP, VM, params, desired_curvature, desired_curvature_rate):
|
||||
def update(self, active, CS, CP, VM, params, last_actuators, desired_curvature, desired_curvature_rate):
|
||||
lqr_log = log.ControlsState.LateralLQRState.new_message()
|
||||
|
||||
steers_max = get_steer_max(CP, CS.vEgo)
|
||||
|
|
|
|||
|
|
@ -16,7 +16,7 @@ class LatControlPID():
|
|||
def reset(self):
|
||||
self.pid.reset()
|
||||
|
||||
def update(self, active, CS, CP, VM, params, desired_curvature, desired_curvature_rate):
|
||||
def update(self, active, CS, CP, VM, params, last_actuators, desired_curvature, desired_curvature_rate):
|
||||
pid_log = log.ControlsState.LateralPIDState.new_message()
|
||||
pid_log.steeringAngleDeg = float(CS.steeringAngleDeg)
|
||||
pid_log.steeringRateDeg = float(CS.steeringRateDeg)
|
||||
|
|
@ -24,7 +24,7 @@ class LatControlPID():
|
|||
angle_steers_des_no_offset = math.degrees(VM.get_steer_from_curvature(-desired_curvature, CS.vEgo))
|
||||
angle_steers_des = angle_steers_des_no_offset + params.angleOffsetDeg
|
||||
|
||||
pid_log.steeringAngleDesiredDeg = angle_steers_des
|
||||
pid_log.steeringAngleDesiredDeg = angle_steers_des
|
||||
pid_log.angleError = angle_steers_des - CS.steeringAngleDeg
|
||||
if CS.vEgo < 0.3 or not active:
|
||||
output_steer = 0.0
|
||||
|
|
|
|||
|
|
@ -1 +1 @@
|
|||
c84310c94ccab023e36d02e9b85430ad7d878868
|
||||
8118420b292f4c67ce7e196fc2121571da330e65
|
||||
Loading…
Reference in New Issue