def update(self, sendcan, enabled, CS, frame, actuators,
pcm_cancel_cmd, hud_alert, audible_alert, forwarding_camera,
left_line, right_line, lead, left_lane_depart, right_lane_depart):
# *** compute control surfaces ***
# gas and brake
apply_gas = clip(actuators.gas, 0., 1.)
if CS.CP.enableGasInterceptor:
# send only negative accel if interceptor is detected. otherwise, send the regular value
# +0.06 offset to reduce ABS pump usage when OP is engaged
apply_accel = 0.06 - actuators.brake
else:
apply_accel = actuators.gas - actuators.brake
apply_accel, self.accel_steady = accel_hysteresis(apply_accel, self.accel_steady, enabled)
apply_accel = clip(apply_accel * ACCEL_SCALE, ACCEL_MIN, ACCEL_MAX)
# steer torque
apply_steer = int(round(actuators.steer * SteerLimitParams.STEER_MAX))
apply_steer = apply_toyota_steer_torque_limits(apply_steer, self.last_steer, CS.steer_torque_motor, SteerLimitParams)
# only cut torque when steer state is a known fault
if CS.steer_state in [9, 25]:
self.last_fault_frame = frame
# Cut steering for 2s after fault
if not enabled or (frame - self.last_fault_frame < 200):
apply_steer = 0
apply_steer_req = 0
else:
apply_steer_req = 1
self.steer_angle_enabled, self.ipas_reset_counter = \
ipas_state_transition(self.steer_angle_enabled, enabled, CS.ipas_active, self.ipas_reset_counter)
#print("{0} {1} {2}".format(self.steer_angle_enabled, self.ipas_reset_counter, CS.ipas_active))
# steer angle
if self.steer_angle_enabled and CS.ipas_active:
apply_angle = actuators.steerAngle
angle_lim = interp(CS.v_ego, ANGLE_MAX_BP, ANGLE_MAX_V)
apply_angle = clip(apply_angle, -angle_lim, angle_lim)
# windup slower
if self.last_angle * apply_angle > 0. and abs(apply_angle) > abs(self.last_angle):
angle_rate_lim = interp(CS.v_ego, ANGLE_DELTA_BP, ANGLE_DELTA_V)
else:
angle_rate_lim = interp(CS.v_ego, ANGLE_DELTA_BP, ANGLE_DELTA_VU)
apply_angle = clip(apply_angle, self.last_angle - angle_rate_lim, self.last_angle + angle_rate_lim)
else:
apply_angle = CS.angle_steers
if not enabled and CS.pcm_acc_status:
# send pcm acc cancel cmd if drive is disabled but pcm is still on, or if the system can't be activated
pcm_cancel_cmd = 1
# on entering standstill, send standstill request
if CS.standstill and not self.last_standstill:
self.standstill_req = True
if CS.pcm_acc_status != 8:
# pcm entered standstill or it's disabled
self.standstill_req = False
self.last_steer = apply_steer
self.last_angle = apply_angle
self.last_accel = apply_accel
self.last_standstill = CS.standstill
can_sends = []
#*** control msgs ***
#print("steer {0} {1} {2} {3}".format(apply_steer, min_lim, max_lim, CS.steer_torque_motor)
# toyota can trace shows this message at 42Hz, with counter adding alternatively 1 and 2;
# sending it at 100Hz seem to allow a higher rate limit, as the rate limit seems imposed
# on consecutive messages
if ECU.CAM in self.fake_ecus:
if self.angle_control:
can_sends.append(create_steer_command(self.packer, 0., 0, frame))
else:
can_sends.append(create_steer_command(self.packer, apply_steer, apply_steer_req, frame))
if self.angle_control:
can_sends.append(create_ipas_steer_command(self.packer, apply_angle, self.steer_angle_enabled,
ECU.APGS in self.fake_ecus))
elif ECU.APGS in self.fake_ecus:
can_sends.append(create_ipas_steer_command(self.packer, 0, 0, True))
# accel cmd comes from DSU, but we can spam can to cancel the system even if we are using lat only control
if (frame % 3 == 0 and ECU.DSU in self.fake_ecus) or (pcm_cancel_cmd and ECU.CAM in self.fake_ecus):
lead = lead or CS.v_ego < 12. # at low speed we always assume the lead is present do ACC can be engaged
if ECU.DSU in self.fake_ecus:
can_sends.append(create_accel_command(self.packer, apply_accel, pcm_cancel_cmd, self.standstill_req, lead))
else:
can_sends.append(create_accel_command(self.packer, 0, pcm_cancel_cmd, False, lead))
if (frame % 2 == 0) and (CS.CP.enableGasInterceptor):
# send exactly zero if apply_gas is zero. Interceptor will send the max between read value and apply_gas.
# This prevents unexpected pedal range rescaling
can_sends.append(create_gas_command(self.packer, apply_gas, frame//2))
if frame % 10 == 0 and ECU.CAM in self.fake_ecus and not forwarding_camera:
for addr in TARGET_IDS:
can_sends.append(create_video_target(frame//10, addr))
# ui mesg is at 100Hz but we send asap if:
# - there is something to display
# - there is something to stop displaying
alert_out = process_hud_alert(hud_alert, audible_alert)
steer, fcw, sound1, sound2 = alert_out
if (any(alert_out) and not self.alert_active) or \
(not any(alert_out) and self.alert_active):
send_ui = True
self.alert_active = not self.alert_active
else:
send_ui = False
if (frame % 100 == 0 or send_ui) and ECU.CAM in self.fake_ecus:
can_sends.append(create_ui_command(self.packer, steer, sound1, sound2, left_line, right_line, left_lane_depart, right_lane_depart))
if frame % 100 == 0 and ECU.DSU in self.fake_ecus:
can_sends.append(create_fcw_command(self.packer, fcw))
#*** static msgs ***
for (addr, ecu, cars, bus, fr_step, vl) in STATIC_MSGS:
if frame % fr_step == 0 and ecu in self.fake_ecus and self.car_fingerprint in cars and not (ecu == ECU.CAM and forwarding_camera):
# special cases
if fr_step == 5 and ecu == ECU.CAM and bus == 1:
cnt = (((frame // 5) % 7) + 1) << 5
vl = chr(cnt) + vl
elif addr in (0x489, 0x48a) and bus == 0:
# add counter for those 2 messages (last 4 bits)
cnt = ((frame // 100) % 0xf) + 1
if addr == 0x48a:
# 0x48a has a 8 preceding the counter
cnt += 1 << 7
vl += chr(cnt)
can_sends.append(make_can_msg(addr, vl, bus, False))
sendcan.send(can_list_to_can_capnp(can_sends, msgtype='sendcan').to_bytes())
def update(self, cp, cp_cam, cp_body):
ret = car.CarState.new_message()
# car params
v_weight_v = [
0., 1.
] # don't trust smooth speed at low values to avoid premature zero snapping
v_weight_bp = [
1., 6.
] # smooth blending, below ~0.6m/s the smooth speed snaps to zero
# update prevs, update must run once per loop
self.prev_cruise_buttons = self.cruise_buttons
self.prev_cruise_setting = self.cruise_setting
# ******************* parse out can *******************
# TODO: find wheels moving bit in dbc
if self.CP.carFingerprint in (CAR.ACCORD, CAR.ACCORDH, CAR.CIVIC_BOSCH,
CAR.CIVIC_BOSCH_DIESEL, CAR.CRV_HYBRID,
CAR.INSIGHT, CAR.ACURA_RDX_3G):
ret.standstill = cp.vl["ENGINE_DATA"]["XMISSION_SPEED"] < 0.1
ret.doorOpen = bool(cp.vl["SCM_FEEDBACK"]["DRIVERS_DOOR_OPEN"])
elif self.CP.carFingerprint == CAR.ODYSSEY_CHN:
ret.standstill = cp.vl["ENGINE_DATA"]["XMISSION_SPEED"] < 0.1
ret.doorOpen = bool(cp.vl["SCM_BUTTONS"]["DRIVERS_DOOR_OPEN"])
elif self.CP.carFingerprint == CAR.HRV:
ret.doorOpen = bool(cp.vl["SCM_BUTTONS"]["DRIVERS_DOOR_OPEN"])
else:
ret.standstill = not cp.vl["STANDSTILL"]["WHEELS_MOVING"]
ret.doorOpen = any([
cp.vl["DOORS_STATUS"]["DOOR_OPEN_FL"],
cp.vl["DOORS_STATUS"]["DOOR_OPEN_FR"],
cp.vl["DOORS_STATUS"]["DOOR_OPEN_RL"],
cp.vl["DOORS_STATUS"]["DOOR_OPEN_RR"]
])
ret.seatbeltUnlatched = bool(
cp.vl["SEATBELT_STATUS"]["SEATBELT_DRIVER_LAMP"]
or not cp.vl["SEATBELT_STATUS"]["SEATBELT_DRIVER_LATCHED"])
steer_status = self.steer_status_values[cp.vl["STEER_STATUS"]
["STEER_STATUS"]]
ret.steerError = steer_status not in [
"NORMAL", "NO_TORQUE_ALERT_1", "NO_TORQUE_ALERT_2",
"LOW_SPEED_LOCKOUT", "TMP_FAULT"
]
# NO_TORQUE_ALERT_2 can be caused by bump OR steering nudge from driver
self.steer_not_allowed = steer_status not in [
"NORMAL", "NO_TORQUE_ALERT_2"
]
# LOW_SPEED_LOCKOUT is not worth a warning
ret.steerWarning = steer_status not in [
"NORMAL", "LOW_SPEED_LOCKOUT", "NO_TORQUE_ALERT_2"
]
if not self.CP.openpilotLongitudinalControl:
self.brake_error = 0
else:
self.brake_error = cp.vl["STANDSTILL"]["BRAKE_ERROR_1"] or cp.vl[
"STANDSTILL"]["BRAKE_ERROR_2"]
ret.espDisabled = cp.vl["VSA_STATUS"]["ESP_DISABLED"] != 0
speed_factor = SPEED_FACTOR.get(self.CP.carFingerprint, 1.)
ret.wheelSpeeds.fl = cp.vl["WHEEL_SPEEDS"][
"WHEEL_SPEED_FL"] * CV.KPH_TO_MS * speed_factor
ret.wheelSpeeds.fr = cp.vl["WHEEL_SPEEDS"][
"WHEEL_SPEED_FR"] * CV.KPH_TO_MS * speed_factor
ret.wheelSpeeds.rl = cp.vl["WHEEL_SPEEDS"][
"WHEEL_SPEED_RL"] * CV.KPH_TO_MS * speed_factor
ret.wheelSpeeds.rr = cp.vl["WHEEL_SPEEDS"][
"WHEEL_SPEED_RR"] * CV.KPH_TO_MS * speed_factor
v_wheel = (ret.wheelSpeeds.fl + ret.wheelSpeeds.fr +
ret.wheelSpeeds.rl + ret.wheelSpeeds.rr) / 4.
# blend in transmission speed at low speed, since it has more low speed accuracy
v_weight = interp(v_wheel, v_weight_bp, v_weight_v)
ret.vEgoRaw = (1. - v_weight) * cp.vl["ENGINE_DATA"][
"XMISSION_SPEED"] * CV.KPH_TO_MS * speed_factor + v_weight * v_wheel
ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw)
ret.steeringAngleDeg = cp.vl["STEERING_SENSORS"]["STEER_ANGLE"]
ret.steeringRateDeg = cp.vl["STEERING_SENSORS"]["STEER_ANGLE_RATE"]
self.cruise_setting = cp.vl["SCM_BUTTONS"]["CRUISE_SETTING"]
self.cruise_buttons = cp.vl["SCM_BUTTONS"]["CRUISE_BUTTONS"]
ret.leftBlinker, ret.rightBlinker = self.update_blinker_from_stalk(
250, cp.vl["SCM_FEEDBACK"]["LEFT_BLINKER"],
cp.vl["SCM_FEEDBACK"]["RIGHT_BLINKER"])
self.brake_hold = cp.vl["VSA_STATUS"]["BRAKE_HOLD_ACTIVE"]
if self.CP.carFingerprint in (CAR.CIVIC, CAR.ODYSSEY, CAR.CRV_5G,
CAR.ACCORD, CAR.ACCORDH, CAR.CIVIC_BOSCH,
CAR.CIVIC_BOSCH_DIESEL, CAR.CRV_HYBRID,
CAR.INSIGHT, CAR.ACURA_RDX_3G):
self.park_brake = cp.vl["EPB_STATUS"]["EPB_STATE"] != 0
main_on = cp.vl["SCM_FEEDBACK"]["MAIN_ON"]
elif self.CP.carFingerprint == CAR.ODYSSEY_CHN:
self.park_brake = cp.vl["EPB_STATUS"]["EPB_STATE"] != 0
main_on = cp.vl["SCM_BUTTONS"]["MAIN_ON"]
else:
self.park_brake = 0 # TODO
main_on = cp.vl["SCM_BUTTONS"]["MAIN_ON"]
gear = int(cp.vl[self.gearbox_msg]["GEAR_SHIFTER"])
ret.gearShifter = self.parse_gear_shifter(
self.shifter_values.get(gear, None))
self.pedal_gas = cp.vl["POWERTRAIN_DATA"]["PEDAL_GAS"]
# crv doesn't include cruise control
if self.CP.carFingerprint in (CAR.CRV, CAR.CRV_EU, CAR.HRV,
CAR.ODYSSEY, CAR.ACURA_RDX,
CAR.RIDGELINE, CAR.PILOT_2019,
CAR.ODYSSEY_CHN):
ret.gas = self.pedal_gas / 256.
else:
ret.gas = cp.vl["GAS_PEDAL_2"]["CAR_GAS"] / 256.
# this is a hack for the interceptor. This is now only used in the simulation
# TODO: Replace tests by toyota so this can go away
if self.CP.enableGasInterceptor:
self.user_gas = (cp.vl["GAS_SENSOR"]["INTERCEPTOR_GAS"] +
cp.vl["GAS_SENSOR"]["INTERCEPTOR_GAS2"]) / 2.
self.user_gas_pressed = self.user_gas > 1e-5 # this works because interceptor read < 0 when pedal position is 0. Once calibrated, this will change
ret.gasPressed = self.user_gas_pressed
else:
ret.gasPressed = self.pedal_gas > 1e-5
ret.steeringTorque = cp.vl["STEER_STATUS"]["STEER_TORQUE_SENSOR"]
ret.steeringTorqueEps = cp.vl["STEER_MOTOR_TORQUE"]["MOTOR_TORQUE"]
ret.steeringPressed = abs(ret.steeringTorque) > STEER_THRESHOLD.get(
self.CP.carFingerprint, 1200)
if self.CP.carFingerprint in HONDA_BOSCH:
if not self.CP.openpilotLongitudinalControl:
ret.cruiseState.nonAdaptive = cp.vl["ACC_HUD"][
"CRUISE_CONTROL_LABEL"] != 0
ret.cruiseState.standstill = cp.vl["ACC_HUD"][
"CRUISE_SPEED"] == 252.
# On set, cruise set speed pulses between 254~255 and the set speed prev is set to avoid this.
ret.cruiseState.speed = self.v_cruise_pcm_prev if cp.vl[
"ACC_HUD"]["CRUISE_SPEED"] > 160.0 else cp.vl["ACC_HUD"][
"CRUISE_SPEED"] * CV.KPH_TO_MS
self.v_cruise_pcm_prev = ret.cruiseState.speed
else:
ret.cruiseState.speedOffset = calc_cruise_offset(
cp.vl["CRUISE_PARAMS"]["CRUISE_SPEED_OFFSET"], ret.vEgo)
ret.cruiseState.speed = cp.vl["CRUISE"][
"CRUISE_SPEED_PCM"] * CV.KPH_TO_MS
self.brake_switch = cp.vl["POWERTRAIN_DATA"]["BRAKE_SWITCH"] != 0
if self.CP.carFingerprint in HONDA_BOSCH_ALT_BRAKE_SIGNAL:
ret.brakePressed = cp.vl["BRAKE_MODULE"]["BRAKE_PRESSED"] != 0
else:
# brake switch has shown some single time step noise, so only considered when
# switch is on for at least 2 consecutive CAN samples
# panda safety only checks BRAKE_PRESSED signal
ret.brakePressed = bool(
cp.vl["POWERTRAIN_DATA"]["BRAKE_PRESSED"]
or (self.brake_switch and self.brake_switch_prev
and cp.ts["POWERTRAIN_DATA"]["BRAKE_SWITCH"] !=
self.brake_switch_prev_ts))
self.brake_switch_prev = self.brake_switch
self.brake_switch_prev_ts = cp.ts["POWERTRAIN_DATA"][
"BRAKE_SWITCH"]
ret.brake = cp.vl["VSA_STATUS"]["USER_BRAKE"]
ret.cruiseState.enabled = cp.vl["POWERTRAIN_DATA"]["ACC_STATUS"] != 0
ret.cruiseState.available = bool(main_on)
# Gets rid of Pedal Grinding noise when brake is pressed at slow speeds for some models
if self.CP.carFingerprint in (CAR.PILOT, CAR.PILOT_2019,
CAR.RIDGELINE):
if ret.brake > 0.05:
ret.brakePressed = True
# TODO: discover the CAN msg that has the imperial unit bit for all other cars
self.is_metric = not cp.vl["HUD_SETTING"][
"IMPERIAL_UNIT"] if self.CP.carFingerprint in (
CAR.CIVIC) else False
if self.CP.carFingerprint in HONDA_BOSCH:
ret.stockAeb = (not self.CP.openpilotLongitudinalControl) and bool(
cp.vl["ACC_CONTROL"]["AEB_STATUS"]
and cp.vl["ACC_CONTROL"]["ACCEL_COMMAND"] < -1e-5)
else:
ret.stockAeb = bool(
cp_cam.vl["BRAKE_COMMAND"]["AEB_REQ_1"]
and cp_cam.vl["BRAKE_COMMAND"]["COMPUTER_BRAKE"] > 1e-5)
if self.CP.carFingerprint in HONDA_BOSCH:
self.stock_hud = False
ret.stockFcw = False
else:
ret.stockFcw = cp_cam.vl["BRAKE_COMMAND"]["FCW"] != 0
self.stock_hud = cp_cam.vl["ACC_HUD"]
self.stock_brake = cp_cam.vl["BRAKE_COMMAND"]
if self.CP.enableBsm and self.CP.carFingerprint in (CAR.CRV_5G, ):
# BSM messages are on B-CAN, requires a panda forwarding B-CAN messages to CAN 0
# more info here: https://github.com/commaai/openpilot/pull/1867
ret.leftBlindspot = cp_body.vl["BSM_STATUS_LEFT"]["BSM_ALERT"] == 1
ret.rightBlindspot = cp_body.vl["BSM_STATUS_RIGHT"][
"BSM_ALERT"] == 1
return ret
def G(self):
return interp(self.speed, self._G[0], self._G[1])
def k_p(self): return interp(self.speed, self._k_p[0], self._k_p[1])
def update(self, sm, pm, CP, VM):
v_ego = sm['carState'].vEgo
angle_steers = sm['carState'].steeringAngle
active = sm['controlsState'].active
angle_offset = sm['liveParameters'].angleOffset
# Run MPC
self.angle_steers_des_prev = self.angle_steers_des_mpc
# Update vehicle model
x = max(sm['liveParameters'].stiffnessFactor, 0.1)
sr = max(sm['liveParameters'].steerRatio, 0.1)
VM.update_params(x, sr)
curvature_factor = VM.curvature_factor(v_ego)
# Get steerRatio and steerRateCost from kegman.json every x seconds
self.mpc_frame += 1
if self.mpc_frame % 500 == 0:
# live tuning through /data/openpilot/tune.py overrides interface.py settings
kegman = kegman_conf()
if kegman.conf['tuneGernby'] == "1":
self.steerRateCost = float(kegman.conf['steerRateCost'])
if self.steerRateCost != self.steerRateCost_prev:
self.setup_mpc()
self.steerRateCost_prev = self.steerRateCost
self.sR = [
float(kegman.conf['steerRatio']),
(float(kegman.conf['steerRatio']) +
float(kegman.conf['sR_boost']))
]
self.sRBP = [
float(kegman.conf['sR_BP0']),
float(kegman.conf['sR_BP1'])
]
self.sR_time = int(float(kegman.conf['sR_time'])) * 100
self.mpc_frame = 0
if v_ego > 11.111:
# boost steerRatio by boost amount if desired steer angle is high
self.steerRatio_new = interp(abs(angle_steers), self.sRBP, self.sR)
self.sR_delay_counter += 1
if self.sR_delay_counter % self.sR_time != 0:
if self.steerRatio_new > self.steerRatio:
self.steerRatio = self.steerRatio_new
else:
self.steerRatio = self.steerRatio_new
self.sR_delay_counter = 0
else:
self.steerRatio = self.sR[0]
#print("steerRatio = ", self.steerRatio)
self.LP.parse_model(sm['model'])
# Lane change logic
one_blinker = sm['carState'].leftBlinker != sm['carState'].rightBlinker
below_lane_change_speed = v_ego < self.alc_min_speed
if sm['carState'].leftBlinker:
self.lane_change_direction = LaneChangeDirection.left
elif sm['carState'].rightBlinker:
self.lane_change_direction = LaneChangeDirection.right
if (not active) or (self.lane_change_timer > LANE_CHANGE_TIME_MAX) or (
not self.lane_change_enabled):
self.lane_change_state = LaneChangeState.off
self.lane_change_direction = LaneChangeDirection.none
else:
torque_applied = sm['carState'].steeringPressed and \
((sm['carState'].steeringTorque > 0 and self.lane_change_direction == LaneChangeDirection.left) or
(sm['carState'].steeringTorque < 0 and self.lane_change_direction == LaneChangeDirection.right))
blindspot_detected = (
(sm['carState'].leftBlindspot
and self.lane_change_direction == LaneChangeDirection.left) or
(sm['carState'].rightBlindspot
and self.lane_change_direction == LaneChangeDirection.right))
lane_change_prob = self.LP.l_lane_change_prob + self.LP.r_lane_change_prob
# State transitions
# off
if self.lane_change_state == LaneChangeState.off and one_blinker and not self.prev_one_blinker and not below_lane_change_speed:
self.lane_change_state = LaneChangeState.preLaneChange
self.lane_change_ll_prob = 1.0
# pre
elif self.lane_change_state == LaneChangeState.preLaneChange:
if not one_blinker or below_lane_change_speed:
self.lane_change_state = LaneChangeState.off
elif torque_applied and not blindspot_detected:
self.lane_change_state = LaneChangeState.laneChangeStarting
# starting
elif self.lane_change_state == LaneChangeState.laneChangeStarting:
# fade out over .5s
self.lane_change_ll_prob = max(
self.lane_change_ll_prob - 2 * DT_MDL, 0.0)
# 98% certainty
if lane_change_prob < 0.02 and self.lane_change_ll_prob < 0.01:
self.lane_change_state = LaneChangeState.laneChangeFinishing
# finishing
elif self.lane_change_state == LaneChangeState.laneChangeFinishing:
# fade in laneline over 1s
self.lane_change_ll_prob = min(
self.lane_change_ll_prob + DT_MDL, 1.0)
if one_blinker and self.lane_change_ll_prob > 0.99:
self.lane_change_state = LaneChangeState.preLaneChange
elif self.lane_change_ll_prob > 0.99:
self.lane_change_state = LaneChangeState.off
if self.lane_change_state in [
LaneChangeState.off, LaneChangeState.preLaneChange
]:
self.lane_change_timer = 0.0
else:
self.lane_change_timer += DT_MDL
self.prev_one_blinker = one_blinker
desire = DESIRES[self.lane_change_direction][self.lane_change_state]
# Turn off lanes during lane change
if desire == log.PathPlan.Desire.laneChangeRight or desire == log.PathPlan.Desire.laneChangeLeft:
self.LP.l_prob *= self.lane_change_ll_prob
self.LP.r_prob *= self.lane_change_ll_prob
self.libmpc.init_weights(MPC_COST_LAT.PATH / 3.0,
MPC_COST_LAT.LANE, MPC_COST_LAT.HEADING,
self.steer_rate_cost)
else:
self.libmpc.init_weights(MPC_COST_LAT.PATH, MPC_COST_LAT.LANE,
MPC_COST_LAT.HEADING,
self.steer_rate_cost)
self.LP.update_d_poly(v_ego)
# TODO: Check for active, override, and saturation
# if active:
# self.path_offset_i += self.LP.d_poly[3] / (60.0 * 20.0)
# self.path_offset_i = clip(self.path_offset_i, -0.5, 0.5)
# self.LP.d_poly[3] += self.path_offset_i
# else:
# self.path_offset_i = 0.0
# account for actuation delay
self.cur_state = calc_states_after_delay(self.cur_state, v_ego,
angle_steers - angle_offset,
curvature_factor,
self.steerRatio,
CP.steerActuatorDelay)
v_ego_mpc = max(v_ego, 5.0) # avoid mpc roughness due to low speed
self.libmpc.run_mpc(self.cur_state, self.mpc_solution,
list(self.LP.l_poly), list(self.LP.r_poly),
list(self.LP.d_poly), self.LP.l_prob,
self.LP.r_prob, curvature_factor, v_ego_mpc,
self.LP.lane_width)
# reset to current steer angle if not active or overriding
if active:
delta_desired = self.mpc_solution[0].delta[1]
rate_desired = math.degrees(self.mpc_solution[0].rate[0] *
self.steerRatio)
else:
delta_desired = math.radians(angle_steers -
angle_offset) / self.steerRatio
rate_desired = 0.0
self.cur_state[0].delta = delta_desired
self.angle_steers_des_mpc = float(
math.degrees(delta_desired * self.steerRatio) + angle_offset)
# Check for infeasable MPC solution
mpc_nans = any(math.isnan(x) for x in self.mpc_solution[0].delta)
t = sec_since_boot()
if mpc_nans:
self.libmpc.init(MPC_COST_LAT.PATH, MPC_COST_LAT.LANE,
MPC_COST_LAT.HEADING, self.steerRateCost)
self.cur_state[0].delta = math.radians(
angle_steers - angle_offset) / self.steerRatio
if t > self.last_cloudlog_t + 5.0:
self.last_cloudlog_t = t
cloudlog.warning("Lateral mpc - nan: True")
if self.mpc_solution[
0].cost > 20000. or mpc_nans: # TODO: find a better way to detect when MPC did not converge
self.solution_invalid_cnt += 1
else:
self.solution_invalid_cnt = 0
plan_solution_valid = self.solution_invalid_cnt < 2
plan_send = messaging.new_message('pathPlan')
plan_send.valid = sm.all_alive_and_valid(service_list=[
'carState', 'controlsState', 'liveParameters', 'model'
])
plan_send.pathPlan.laneWidth = float(self.LP.lane_width)
plan_send.pathPlan.dPoly = [float(x) for x in self.LP.d_poly]
plan_send.pathPlan.lPoly = [float(x) for x in self.LP.l_poly]
plan_send.pathPlan.lProb = float(self.LP.l_prob)
plan_send.pathPlan.rPoly = [float(x) for x in self.LP.r_poly]
plan_send.pathPlan.rProb = float(self.LP.r_prob)
plan_send.pathPlan.angleSteers = float(self.angle_steers_des_mpc)
plan_send.pathPlan.rateSteers = float(rate_desired)
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 = desire
plan_send.pathPlan.laneChangeState = self.lane_change_state
plan_send.pathPlan.laneChangeDirection = self.lane_change_direction
pm.send('pathPlan', plan_send)
if LOG_MPC:
dat = messaging.new_message('liveMpc')
dat.liveMpc.x = list(self.mpc_solution[0].x)
dat.liveMpc.y = list(self.mpc_solution[0].y)
dat.liveMpc.psi = list(self.mpc_solution[0].psi)
dat.liveMpc.delta = list(self.mpc_solution[0].delta)
dat.liveMpc.cost = self.mpc_solution[0].cost
pm.send('liveMpc', dat)
def get_steer_max(CP, v_ego): return interp(v_ego, CP.steerMaxBP, CP.steerMaxV)
def update(self, active, CS, CP, VM, params, curvature, curvature_rate):
self.speed = CS.vEgo
self.RC = interp(self.speed, self._RC[0], self._RC[1])
self.G = interp(self.speed, self._G[0], self._G[1])
self.outer_loop_gain = interp(self.speed, self._outer_loop_gain[0],
self._outer_loop_gain[1])
self.inner_loop_gain = interp(self.speed, self._inner_loop_gain[0],
self._inner_loop_gain[1])
if self.live_tune_enabled:
self.live_tune(CP)
# Update Kalman filter
y = np.array([[math.radians(CS.steeringAngleDeg)],
[math.radians(CS.steeringRateDeg)]])
self.x = np.dot(self.A_K, self.x) + np.dot(self.K, y)
indi_log = log.ControlsState.LateralINDIState.new_message()
indi_log.steeringAngleDeg = math.degrees(self.x[0])
indi_log.steeringRateDeg = math.degrees(self.x[1])
indi_log.steeringAccelDeg = math.degrees(self.x[2])
steers_des = VM.get_steer_from_curvature(-curvature, CS.vEgo)
steers_des += math.radians(params.angleOffsetDeg)
if CS.vEgo < 0.3 or not active:
indi_log.active = False
self.output_steer = 0.0
self.delayed_output = 0.0
else:
rate_des = VM.get_steer_from_curvature(-curvature_rate, CS.vEgo)
# Expected actuator value
alpha = 1. - DT_CTRL / (self.RC + DT_CTRL)
self.delayed_output = self.delayed_output * alpha + self.output_steer * (
1. - alpha)
# Compute acceleration error
rate_sp = self.outer_loop_gain * (steers_des -
self.x[0]) + rate_des
accel_sp = self.inner_loop_gain * (rate_sp - self.x[1])
accel_error = accel_sp - self.x[2]
# Compute change in actuator
g_inv = 1. / self.G
delta_u = g_inv * accel_error
# If steering pressed, only allow wind down
if CS.steeringPressed and (delta_u * self.output_steer > 0):
delta_u = 0
# Enforce rate limit
if self.enforce_rate_limit:
steer_max = float(CarControllerParams.STEER_MAX)
new_output_steer_cmd = steer_max * (self.delayed_output +
delta_u)
prev_output_steer_cmd = steer_max * self.output_steer
new_output_steer_cmd = apply_toyota_steer_torque_limits(
new_output_steer_cmd, prev_output_steer_cmd,
prev_output_steer_cmd, CarControllerParams)
self.output_steer = new_output_steer_cmd / steer_max
else:
self.output_steer = self.delayed_output + delta_u
steers_max = get_steer_max(CP, CS.vEgo)
self.output_steer = clip(self.output_steer, -steers_max,
steers_max)
indi_log.active = True
indi_log.rateSetPoint = float(rate_sp)
indi_log.accelSetPoint = float(accel_sp)
indi_log.accelError = float(accel_error)
indi_log.delayedOutput = float(self.delayed_output)
indi_log.delta = float(delta_u)
indi_log.output = float(self.output_steer)
check_saturation = (
CS.vEgo >
10.) and not CS.steeringRateLimited and not CS.steeringPressed
indi_log.saturated = self._check_saturation(
self.output_steer, check_saturation, steers_max)
return float(self.output_steer), float(steers_des), indi_log
def update(self, enabled, CS, frame, actuators, \
hud_v_cruise, hud_show_lanes, hud_show_car, hud_alert):
P = self.params
# Send CAN commands.
can_sends = []
### STEER ###
if (frame % P.STEER_STEP) == 0:
lkas_enabled = enabled and not CS.steer_warning and CS.lkMode and CS.out.vEgo > P.MIN_STEER_SPEED
if lkas_enabled:
new_steer = 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
else:
apply_steer = 0
self.apply_steer_last = apply_steer
idx = (frame // P.STEER_STEP) % 4
can_sends.append(gmcan.create_steering_control(self.packer_pt,
CanBus.POWERTRAIN, apply_steer, idx, lkas_enabled))
### GAS/BRAKE ###
# no output if not enabled, but keep sending keepalive messages
# treat pedals as one
final_pedal = actuators.gas - actuators.brake
# *** apply pedal hysteresis ***
final_brake, self.brake_steady = actuator_hystereses(
final_pedal, self.pedal_steady)
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(final_pedal, P.GAS_LOOKUP_BP, P.GAS_LOOKUP_V)))
apply_brake = int(round(interp(final_pedal, P.BRAKE_LOOKUP_BP, P.BRAKE_LOOKUP_V)))
# Gas/regen and brakes - all at 25Hz
if (frame % 4) == 0:
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))
at_full_stop = enabled and CS.out.standstill
can_sends.append(gmcan.create_gas_regen_command(self.packer_pt, CanBus.POWERTRAIN, apply_gas, idx, enabled, at_full_stop))
# Send dashboard UI commands (ACC status), 25hz
follow_level = CS.get_follow_level()
if (frame % 4) == 0:
can_sends.append(gmcan.create_acc_dashboard_command(self.packer_pt, CanBus.POWERTRAIN, enabled, hud_v_cruise * CV.MS_TO_KPH, hud_show_car, follow_level))
# Radar needs to know current speed and yaw rate (50hz),
# and that ADAS is alive (10hz)
time_and_headlights_step = 10
tt = frame * DT_CTRL
if frame % time_and_headlights_step == 0:
idx = (frame // time_and_headlights_step) % 4
can_sends.append(gmcan.create_adas_time_status(CanBus.OBSTACLE, int((tt - self.start_time) * 60), idx))
can_sends.append(gmcan.create_adas_headlights_status(CanBus.OBSTACLE))
speed_and_accelerometer_step = 2
if frame % speed_and_accelerometer_step == 0:
idx = (frame // speed_and_accelerometer_step) % 4
can_sends.append(gmcan.create_adas_steering_status(CanBus.OBSTACLE, idx))
can_sends.append(gmcan.create_adas_accelerometer_speed_status(CanBus.OBSTACLE, CS.out.vEgo, idx))
if frame % P.ADAS_KEEPALIVE_STEP == 0:
can_sends += gmcan.create_adas_keepalive(CanBus.POWERTRAIN)
# Show green icon when LKA torque is applied, and
# alarming orange icon when approaching torque limit.
# If not sent again, LKA icon disappears in about 5 seconds.
# Conveniently, sending camera message periodically also works as a keepalive.
lka_active = CS.lkas_status == 1
lka_critical = lka_active and abs(actuators.steer) > 0.9
lka_icon_status = (lka_active, lka_critical)
if frame % P.CAMERA_KEEPALIVE_STEP == 0 \
or lka_icon_status != self.lka_icon_status_last:
steer_alert = hud_alert == VisualAlert.steerRequired
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
def _get_TR(self):
x_vel = [
0.0, 1.8627, 3.7253, 5.588, 7.4507, 9.3133, 11.5598, 13.645,
22.352, 31.2928, 33.528, 35.7632, 40.2336
] # velocities
profile_mod_x = [2.2352, 13.4112, 24.5872, 35.7632
] # profile mod speeds, mph: [5., 30., 55., 80.]
if self.dp_dynamic_follow == PROFILE_AUTO: # decide which profile to use, model profile will be updated before this
# df is 0 = traffic, 1 = relaxed, 2 = roadtrip, 3 = auto
# dp is 0 = off, 1 = short, 2 = normal, 3 = long, 4 = auto
# if it's model profile, we need to convert it
if self.model_profile is None:
# when its none, we use normal instead
df_profile = PROFILE_NORMAL
else:
df_profile = self.model_profile + 1
else:
df_profile = self.dp_dynamic_follow
if df_profile == PROFILE_LONG:
y_dist = [
1.3978, 1.4132, 1.4318, 1.4536, 1.485, 1.5229, 1.5819, 1.6203,
1.7238, 1.8231, 1.8379, 1.8495, 1.8535
] # TRs
profile_mod_pos = [0.92, 0.7, 0.25, 0.15]
profile_mod_neg = [1.1, 1.3, 2.0, 2.3]
elif df_profile == PROFILE_SHORT: # for in congested traffic
x_vel = [
0.0, 1.892, 3.7432, 5.8632, 8.0727, 10.7301, 14.343, 17.6275,
22.4049, 28.6752, 34.8858, 40.35
]
# y_dist = [1.3781, 1.3791, 1.3802, 1.3825, 1.3984, 1.4249, 1.4194, 1.3162, 1.1916, 1.0145, 0.9855, 0.9562] # original
# y_dist = [1.3781, 1.3791, 1.3112, 1.2442, 1.2306, 1.2112, 1.2775, 1.1977, 1.0963, 0.9435, 0.9067, 0.8749] # avg. 7.3 ft closer from 18 to 90 mph
y_dist = [
1.3781, 1.3791, 1.3457, 1.3134, 1.3145, 1.318, 1.3485, 1.257,
1.144, 0.979, 0.9461, 0.9156
]
profile_mod_pos = [1.05, 1.55, 2.6, 3.75]
profile_mod_neg = [0.84, .275, 0.1, 0.05]
elif df_profile == PROFILE_NORMAL: # default to relaxed/stock
y_dist = [
1.385, 1.394, 1.406, 1.421, 1.444, 1.474, 1.516, 1.534, 1.546,
1.568, 1.579, 1.593, 1.614
]
profile_mod_pos = [1.0] * 4
profile_mod_neg = [1.0] * 4
else:
raise Exception('Unknown profile type: {}'.format(df_profile))
# Global df mod
profile_mod_pos, profile_mod_neg, y_dist = self.global_profile_mod(
profile_mod_x, profile_mod_pos, profile_mod_neg, x_vel, y_dist)
# Profile modifications - Designed so that each profile reacts similarly to changing lead dynamics
profile_mod_pos = interp(self.car_data.v_ego, profile_mod_x,
profile_mod_pos)
profile_mod_neg = interp(self.car_data.v_ego, profile_mod_x,
profile_mod_neg)
if self.car_data.v_ego > self.sng_speed: # keep sng distance until we're above sng speed again
self.sng = False
if (self.car_data.v_ego >= self.sng_speed
or self.df_data.v_egos[0]['v_ego'] >= self.car_data.v_ego
) and not self.sng:
# if above 15 mph OR we're decelerating to a stop, keep shorter TR. when we reaccelerate, use sng_TR and slowly decrease
TR = interp(self.car_data.v_ego, x_vel, y_dist)
else: # this allows us to get closer to the lead car when stopping, while being able to have smooth stop and go when reaccelerating
self.sng = True
x = [
self.sng_speed * 0.7, self.sng_speed
] # decrease TR between 12.6 and 18 mph from 1.8s to defined TR above at 18mph while accelerating
y = [self.sng_TR, interp(self.sng_speed, x_vel, y_dist)]
TR = interp(self.car_data.v_ego, x, y)
TR_mods = []
# Dynamic follow modifications (the secret sauce)
x = [
-26.8224, -20.0288, -15.6871, -11.1965, -7.8645, -4.9472, -3.0541,
-2.2244, -1.5045, -0.7908, -0.3196, 0.0, 0.5588, 1.3682, 1.898,
2.7316, 4.4704
] # relative velocity values
y = [
.76, 0.62323, 0.49488, 0.40656, 0.32227, 0.23914, 0.12269, 0.10483,
0.08074, 0.04886, 0.0072, 0.0, -0.05648, -0.0792, -0.15675,
-0.23289, -0.315
] # modification values
TR_mods.append(
interp(self.lead_data.v_lead - self.car_data.v_ego, x, y))
x = [
-4.4795, -2.8122, -1.5727, -1.1129, -0.6611, -0.2692, 0.0, 0.1466,
0.5144, 0.6903, 0.9302
] # lead acceleration values
y = [
0.24, 0.16, 0.092, 0.0515, 0.0305, 0.022, 0.0, -0.0153, -0.042,
-0.053, -0.059
] # modification values
TR_mods.append(interp(self.lead_data.a_lead, x, y))
rel_accel_mod = self._calculate_relative_accel_new()
if rel_accel_mod is not None: # if available
deadzone = 2 * CV.MPH_TO_MS
if self.lead_data.v_lead - deadzone > self.car_data.v_ego:
TR_mods.append(rel_accel_mod)
x = [self.sng_speed / 5.0,
self.sng_speed] # as we approach 0, apply x% more distance
y = [1.05, 1.0]
profile_mod_pos *= interp(self.car_data.v_ego, x,
y) # but only for currently positive mods
TR_mod = sum([
mod * profile_mod_neg if mod < 0 else mod * profile_mod_pos
for mod in TR_mods
]) # alter TR modification according to profile
TR += TR_mod
if self.car_data.left_blinker or self.car_data.right_blinker and df_profile != self.df_profiles.traffic:
x = [8.9408, 22.352, 31.2928] # 20, 50, 70 mph
y = [1.0, .75, .65]
TR *= interp(self.car_data.v_ego, x,
y) # reduce TR when changing lanes
return float(clip(TR, self.min_TR, 2.7))
def update(self, enabled, CS, frame, actuators, cruise_cancel, hud_alert,
left_line, right_line, left_lane_depart, right_lane_depart):
""" Controls thread """
# Send CAN commands.
can_sends = []
### STEER ###
acc_active = bool(CS.out.cruiseState.enabled)
lkas_hud_msg = CS.lkas_hud_msg
lkas_hud_info_msg = CS.lkas_hud_info_msg
apply_angle = actuators.steerAngle
steer_hud_alert = 1 if hud_alert == VisualAlert.steerRequired else 0
if enabled:
# # 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)
else:
angle_rate_lim = interp(CS.out.vEgo, CarControllerParams.ANGLE_DELTA_BP, CarControllerParams.ANGLE_DELTA_VU)
apply_angle = clip(apply_angle, self.last_angle - angle_rate_lim, self.last_angle + angle_rate_lim)
# Max torque from driver before EPS will give up and not apply torque
if not bool(CS.out.steeringPressed):
self.lkas_max_torque = CarControllerParams.LKAS_MAX_TORQUE
else:
# Scale max torque based on how much torque the driver is applying to the wheel
self.lkas_max_torque = max(
# Scale max torque down to half LKAX_MAX_TORQUE as a minimum
CarControllerParams.LKAS_MAX_TORQUE * 0.5,
# Start scaling torque at STEER_THRESHOLD
CarControllerParams.LKAS_MAX_TORQUE - 0.6 * max(0, abs(CS.out.steeringTorque) - CarControllerParams.STEER_THRESHOLD)
)
else:
apply_angle = CS.out.steeringAngle
self.lkas_max_torque = 0
self.last_angle = apply_angle
if not enabled and acc_active:
# send acc cancel cmd if drive is disabled but pcm is still on, or if the system can't be activated
cruise_cancel = 1
if self.CP.carFingerprint in [CAR.ROGUE, CAR.XTRAIL] and cruise_cancel:
can_sends.append(nissancan.create_acc_cancel_cmd(self.packer, CS.cruise_throttle_msg, frame))
# TODO: Find better way to cancel!
# For some reason spamming the cancel button is unreliable on the Leaf
# We now cancel by making propilot think the seatbelt is unlatched,
# this generates a beep and a warning message every time you disengage
if self.CP.carFingerprint == CAR.LEAF and frame % 2 == 0:
can_sends.append(nissancan.create_cancel_msg(self.packer, CS.cancel_msg, cruise_cancel))
can_sends.append(nissancan.create_steering_control(
self.packer, self.car_fingerprint, apply_angle, frame, enabled, self.lkas_max_torque))
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))
if frame % 50 == 0:
can_sends.append(nissancan.create_lkas_hud_info_msg(
self.packer, lkas_hud_info_msg, steer_hud_alert
))
return can_sends
def update(self, enabled, CS, frame, actuators, \
pcm_speed, pcm_override, pcm_cancel_cmd, pcm_accel, \
hud_v_cruise, hud_show_lanes, hud_show_car, hud_alert):
P = self.params
# *** apply brake hysteresis ***
brake, self.braking, self.brake_steady = actuator_hystereses(
actuators.brake, self.braking, self.brake_steady, CS.out.vEgo,
CS.CP.carFingerprint)
# *** no output if not enabled ***
if not enabled and CS.out.cruiseState.enabled:
# send pcm acc cancel cmd if drive is disabled but pcm is still on, or if the system can't be activated
pcm_cancel_cmd = True
# *** rate limit after the enable check ***
self.brake_last = rate_limit(brake, self.brake_last, -2., DT_CTRL)
# vehicle hud display, wait for one update from 10Hz 0x304 msg
if hud_show_lanes:
hud_lanes = 1
else:
hud_lanes = 0
if enabled:
if hud_show_car:
hud_car = 2
else:
hud_car = 1
else:
hud_car = 0
fcw_display, steer_required, acc_alert = process_hud_alert(hud_alert)
hud = HUDData(int(pcm_accel), int(round(hud_v_cruise)), hud_car,
hud_lanes, fcw_display, acc_alert, steer_required)
# **** process the car messages ****
if CS.CP.carFingerprint in HONDA_BOSCH:
stopping = 0
starting = 0
accel = actuators.gas - actuators.brake
if accel < 0 and CS.out.vEgo < 0.3:
# prevent rolling backwards
stopping = 1
accel = -1.0
elif accel > 0 and CS.out.vEgo < 0.3:
starting = 1
apply_accel = interp(accel, BOSCH_ACCEL_LOOKUP_BP,
BOSCH_ACCEL_LOOKUP_V)
print("%s: %s" % (str(apply_accel), str(CS.out.aEgo)))
apply_gas = interp(accel, BOSCH_GAS_LOOKUP_BP, BOSCH_GAS_LOOKUP_V)
else:
apply_gas = clip(actuators.gas, 0., 1.)
apply_brake = int(
clip(self.brake_last * P.BRAKE_MAX, 0, P.BRAKE_MAX - 1))
# 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 = enabled and not CS.steer_not_allowed
# Send CAN commands.
can_sends = []
if CS.CP.carFingerprint in HONDA_BOSCH and CS.CP.openpilotLongitudinalControl:
# TODO: radar disable hacked together to see if it works
if (frame % 10) == 0:
# tester present - w/ no response (keeps radar disabled)
can_sends.append([
0x18DAB0F1, 0, b"\x02\x3E\x80\x00\x00\x00\x00\x00",
1 if CS.CP.isPandaBlack else 0
])
# 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.isPandaBlack, CS.CP.openpilotLongitudinalControl))
# Send dashboard UI commands.
if (frame % 10) == 0:
idx = (frame // 10) % 4
can_sends.extend(
hondacan.create_ui_commands(self.packer, pcm_speed, hud,
CS.CP.carFingerprint, CS.is_metric,
idx, CS.CP.isPandaBlack,
CS.CP.openpilotLongitudinalControl,
CS.stock_hud))
if not CS.CP.openpilotLongitudinalControl:
if (frame % 2) == 0:
idx = frame // 2
can_sends.append(
hondacan.create_bosch_supplemental_1(
self.packer, CS.CP.carFingerprint, idx,
CS.CP.isPandaBlack))
# If using stock ACC, spam cancel command to kill gas when OP disengages.
if pcm_cancel_cmd:
can_sends.append(
hondacan.spam_buttons_command(self.packer,
CruiseButtons.CANCEL, idx,
CS.CP.carFingerprint,
CS.CP.isPandaBlack))
elif CS.out.cruiseState.standstill:
can_sends.append(
hondacan.spam_buttons_command(self.packer,
CruiseButtons.RES_ACCEL, idx,
CS.CP.carFingerprint,
CS.CP.isPandaBlack))
else:
# Send gas and brake commands.
if (frame % 2) == 0:
idx = frame // 2
ts = frame * DT_CTRL
if CS.CP.carFingerprint in HONDA_BOSCH:
can_sends.extend(
hondacan.create_acc_commands(self.packer, enabled,
apply_accel, apply_gas,
idx, stopping, starting,
CS.CP.carFingerprint,
CS.CP.isPandaBlack))
else:
pump_on, self.last_pump_ts = brake_pump_hysteresis(
apply_brake, self.apply_brake_last, self.last_pump_ts,
ts)
can_sends.append(
hondacan.create_brake_command(
self.packer, apply_brake, pump_on, pcm_override,
pcm_cancel_cmd, hud.fcw, idx, CS.CP.carFingerprint,
CS.CP.isPandaBlack, CS.stock_brake))
self.apply_brake_last = apply_brake
if CS.CP.enableGasInterceptor:
# send exactly zero if apply_gas is zero. Interceptor will send the max between read value and apply_gas.
# This prevents unexpected pedal range rescaling
can_sends.append(
create_gas_command(self.packer, apply_gas, idx))
return can_sends
def state_control(self, CS):
"""Given the state, this function returns an actuators packet"""
lat_plan = self.sm['lateralPlan']
long_plan = self.sm['longitudinalPlan']
anglesteer_current = CS.steeringAngleDeg
anglesteer_desire = lat_plan.steerAngleDesireDeg
output_scale = lat_plan.outputScale
live_sr = Params().get_bool('OpkrLiveSteerRatio')
if not live_sr:
angle_diff = abs(anglesteer_desire) - abs(anglesteer_current)
if abs(output_scale) >= self.CP.steerMaxV[0] and CS.vEgo > 8:
self.new_steerRatio_prev = interp(angle_diff, self.angle_differ_range, self.steerRatio_range)
if self.new_steerRatio_prev > self.new_steerRatio:
self.new_steerRatio = self.new_steerRatio_prev
else:
self.mpc_frame += 1
if self.mpc_frame % 100 == 0:
self.new_steerRatio -= 0.1
if self.new_steerRatio <= self.CP.steerRatio:
self.new_steerRatio = self.CP.steerRatio
self.mpc_frame = 0
# Update VehicleModel
params = self.sm['liveParameters']
x = max(params.stiffnessFactor, 0.1)
if live_sr:
sr = max(params.steerRatio, 0.1)
else:
sr = max(self.new_steerRatio, 0.1)
self.VM.update_params(x, sr)
self.steerRatio_to_send = sr
actuators = car.CarControl.Actuators.new_message()
if CS.leftBlinker or CS.rightBlinker:
self.last_blinker_frame = self.sm.frame
# State specific actions
if not self.active:
self.LaC.reset()
self.LoC.reset(v_pid=CS.vEgo)
long_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(long_plan_age, LON_MPC_STEP + DT_CTRL) + DT_CTRL
a_acc_sol = long_plan.aStart + (dt / LON_MPC_STEP) * (long_plan.aTarget - long_plan.aStart)
v_acc_sol = long_plan.vStart + dt * (a_acc_sol + long_plan.aStart) / 2.0
extras_loc = {'lead_one': self.sm['radarState'].leadOne, 'has_lead': long_plan.hasLead}
# Gas/Brake PID loop
actuators.gas, actuators.brake = self.LoC.update(self.active and CS.cruiseState.speed > 1., CS, v_acc_sol, long_plan.vTargetFuture, long_plan.aTarget, a_acc_sol, self.CP, long_plan.hasLead, self.sm['radarState'], long_plan.longitudinalPlanSource, extras_loc)
# Steering PID loop and lateral MPC
actuators.steer, actuators.steeringAngleDeg, lac_log = self.LaC.update(self.active, CS, self.CP, self.VM, params, lat_plan)
# Check for difference between desired angle and angle for angle based control
angle_control_saturated = self.CP.steerControlType == car.CarParams.SteerControlType.angle and \
abs(actuators.steeringAngleDeg - CS.steeringAngleDeg) > STEER_ANGLE_SATURATION_THRESHOLD
if angle_control_saturated and not CS.steeringPressed and self.active:
self.saturated_count += 1
else:
self.saturated_count = 0
# Send a "steering required alert" if saturation count has reached the limit
if (lac_log.saturated and not CS.steeringPressed) or \
(self.saturated_count > STEER_ANGLE_SATURATION_TIMEOUT):
if len(lat_plan.dPathPoints):
# Check if we deviated from the path
left_deviation = actuators.steer > 0 and lat_plan.dPathPoints[0] < -0.1
right_deviation = actuators.steer < 0 and lat_plan.dPathPoints[0] > 0.1
if left_deviation or right_deviation:
self.events.add(EventName.steerSaturated)
return actuators, v_acc_sol, a_acc_sol, lac_log
def update(self, cp, cp_cam):
# car params
v_weight_v = [
0., 1.
] # don't trust smooth speed at low values to avoid premature zero snapping
v_weight_bp = [
1., 6.
] # smooth blending, below ~0.6m/s the smooth speed snaps to zero
# update prevs, update must run once per loop
self.prev_cruise_buttons = self.cruise_buttons
self.prev_lead_distance = self.lead_distance
self.prev_left_blinker_on = self.left_blinker_on
self.prev_cruise_setting = self.cruise_setting
self.prev_left_blinker_on = self.left_blinker_on
self.prev_right_blinker_on = self.right_blinker_on
# ******************* parse out can *******************
if self.CP.carFingerprint in (
CAR.ACCORD, CAR.ACCORD_15, CAR.ACCORDH,
CAR.INSIGHT): # TODO: find wheels moving bit in dbc
self.standstill = cp.vl["ENGINE_DATA"]['XMISSION_SPEED'] < 0.1
self.door_all_closed = not cp.vl["SCM_FEEDBACK"][
'DRIVERS_DOOR_OPEN']
self.lead_distance = cp.vl["RADAR_HUD"]['LEAD_DISTANCE']
elif self.CP.carFingerprint in (CAR.CIVIC_BOSCH, CAR.CRV_HYBRID):
self.standstill = cp.vl["ENGINE_DATA"]['XMISSION_SPEED'] < 0.1
self.door_all_closed = not cp.vl["SCM_FEEDBACK"][
'DRIVERS_DOOR_OPEN']
elif self.CP.carFingerprint == CAR.ODYSSEY_CHN:
self.standstill = cp.vl["ENGINE_DATA"]['XMISSION_SPEED'] < 0.1
self.door_all_closed = not cp.vl["SCM_BUTTONS"]['DRIVERS_DOOR_OPEN']
else:
self.standstill = not cp.vl["STANDSTILL"]['WHEELS_MOVING']
self.door_all_closed = not any([
cp.vl["DOORS_STATUS"]['DOOR_OPEN_FL'],
cp.vl["DOORS_STATUS"]['DOOR_OPEN_FR'],
cp.vl["DOORS_STATUS"]['DOOR_OPEN_RL'],
cp.vl["DOORS_STATUS"]['DOOR_OPEN_RR']
])
self.seatbelt = not cp.vl["SEATBELT_STATUS"][
'SEATBELT_DRIVER_LAMP'] and cp.vl["SEATBELT_STATUS"][
'SEATBELT_DRIVER_LATCHED']
steer_status = self.steer_status_values[cp.vl["STEER_STATUS"]
['STEER_STATUS']]
self.steer_error = steer_status not in [
'NORMAL', 'NO_TORQUE_ALERT_1', 'NO_TORQUE_ALERT_2',
'LOW_SPEED_LOCKOUT', 'TMP_FAULT'
]
# NO_TORQUE_ALERT_2 can be caused by bump OR steering nudge from driver
self.steer_not_allowed = steer_status not in [
'NORMAL', 'NO_TORQUE_ALERT_2'
]
# LOW_SPEED_LOCKOUT is not worth a warning
self.steer_warning = steer_status not in [
'NORMAL', 'LOW_SPEED_LOCKOUT', 'NO_TORQUE_ALERT_2'
]
if self.CP.radarOffCan:
self.brake_error = 0
else:
self.brake_error = cp.vl["STANDSTILL"]['BRAKE_ERROR_1'] or cp.vl[
"STANDSTILL"]['BRAKE_ERROR_2']
self.esp_disabled = cp.vl["VSA_STATUS"]['ESP_DISABLED']
speed_factor = SPEED_FACTOR[self.CP.carFingerprint]
self.v_wheel_fl = cp.vl["WHEEL_SPEEDS"][
'WHEEL_SPEED_FL'] * CV.KPH_TO_MS * speed_factor
self.v_wheel_fr = cp.vl["WHEEL_SPEEDS"][
'WHEEL_SPEED_FR'] * CV.KPH_TO_MS * speed_factor
self.v_wheel_rl = cp.vl["WHEEL_SPEEDS"][
'WHEEL_SPEED_RL'] * CV.KPH_TO_MS * speed_factor
self.v_wheel_rr = cp.vl["WHEEL_SPEEDS"][
'WHEEL_SPEED_RR'] * CV.KPH_TO_MS * speed_factor
v_wheel = (self.v_wheel_fl + self.v_wheel_fr + self.v_wheel_rl +
self.v_wheel_rr) / 4.
# blend in transmission speed at low speed, since it has more low speed accuracy
self.v_weight = interp(v_wheel, v_weight_bp, v_weight_v)
self.v_ego_raw = (1. - self.v_weight) * cp.vl["ENGINE_DATA"]['XMISSION_SPEED'] * CV.KPH_TO_MS * speed_factor + \
self.v_weight * v_wheel
self.v_ego, self.a_ego = self.update_speed_kf(self.v_ego_raw)
# this is a hack for the interceptor. This is now only used in the simulation
# TODO: Replace tests by toyota so this can go away
if self.CP.enableGasInterceptor:
self.user_gas = (cp.vl["GAS_SENSOR"]['INTERCEPTOR_GAS'] +
cp.vl["GAS_SENSOR"]['INTERCEPTOR_GAS2']) / 2.
self.user_gas_pressed = self.user_gas > 0 # this works because interceptor read < 0 when pedal position is 0. Once calibrated, this will change
self.gear = 0 if self.CP.carFingerprint == CAR.CIVIC else cp.vl[
"GEARBOX"]['GEAR']
self.angle_steers = cp.vl["STEERING_SENSORS"]['STEER_ANGLE']
self.angle_steers_rate = cp.vl["STEERING_SENSORS"]['STEER_ANGLE_RATE']
#self.cruise_setting = cp.vl["SCM_BUTTONS"]['CRUISE_SETTING']
self.cruise_buttons = cp.vl["SCM_BUTTONS"]['CRUISE_BUTTONS']
self.blinker_on = cp.vl["SCM_FEEDBACK"]['LEFT_BLINKER'] or cp.vl[
"SCM_FEEDBACK"]['RIGHT_BLINKER']
self.left_blinker_on = cp.vl["SCM_FEEDBACK"]['LEFT_BLINKER']
self.right_blinker_on = cp.vl["SCM_FEEDBACK"]['RIGHT_BLINKER']
self.brake_hold = cp.vl["VSA_STATUS"]['BRAKE_HOLD_ACTIVE']
if self.CP.carFingerprint in (CAR.CIVIC, CAR.ODYSSEY, CAR.CRV_5G,
CAR.ACCORD, CAR.ACCORD_15, CAR.ACCORDH,
CAR.CIVIC_BOSCH, CAR.INSIGHT,
CAR.CRV_HYBRID):
self.park_brake = cp.vl["EPB_STATUS"]['EPB_STATE'] != 0
self.main_on = cp.vl["SCM_FEEDBACK"]['MAIN_ON']
elif self.CP.carFingerprint == CAR.ODYSSEY_CHN:
self.park_brake = cp.vl["EPB_STATUS"]['EPB_STATE'] != 0
self.main_on = cp.vl["SCM_BUTTONS"]['MAIN_ON']
else:
self.park_brake = 0 # TODO
self.main_on = cp.vl["SCM_BUTTONS"]['MAIN_ON']
can_gear_shifter = int(cp.vl["GEARBOX"]['GEAR_SHIFTER'])
self.gear_shifter = self.parse_gear_shifter(
self.shifter_values.get(can_gear_shifter, None))
self.pedal_gas = cp.vl["POWERTRAIN_DATA"]['PEDAL_GAS']
# crv doesn't include cruise control
if self.CP.carFingerprint in (CAR.CRV, CAR.ODYSSEY, CAR.ACURA_RDX,
CAR.RIDGELINE, CAR.PILOT_2019,
CAR.ODYSSEY_CHN):
self.car_gas = self.pedal_gas
else:
self.car_gas = cp.vl["GAS_PEDAL_2"]['CAR_GAS']
self.steer_torque_driver = cp.vl["STEER_STATUS"]['STEER_TORQUE_SENSOR']
self.steer_torque_motor = cp.vl["STEER_MOTOR_TORQUE"]['MOTOR_TORQUE']
self.steer_override = abs(
self.steer_torque_driver) > STEER_THRESHOLD[self.CP.carFingerprint]
self.brake_switch = cp.vl["POWERTRAIN_DATA"]['BRAKE_SWITCH']
if self.CP.radarOffCan:
self.cruise_mode = cp.vl["ACC_HUD"]['CRUISE_CONTROL_LABEL']
self.stopped = cp.vl["ACC_HUD"]['CRUISE_SPEED'] == 252.
self.cruise_speed_offset = calc_cruise_offset(0, self.v_ego)
if self.CP.carFingerprint in (CAR.CIVIC_BOSCH, CAR.ACCORDH,
CAR.INSIGHT, CAR.CRV_HYBRID):
self.brake_switch = cp.vl["POWERTRAIN_DATA"]['BRAKE_SWITCH']
self.brake_pressed = cp.vl["POWERTRAIN_DATA"]['BRAKE_PRESSED'] or \
(self.brake_switch and self.brake_switch_prev and \
cp.ts["POWERTRAIN_DATA"]['BRAKE_SWITCH'] != self.brake_switch_ts)
self.brake_switch_prev = self.brake_switch
self.brake_switch_ts = cp.ts["POWERTRAIN_DATA"]['BRAKE_SWITCH']
if self.CP.carFingerprint in (CAR.CIVIC_BOSCH):
self.hud_lead = cp.vl["ACC_HUD"]['HUD_LEAD']
else:
self.brake_pressed = cp.vl["BRAKE_MODULE"]['BRAKE_PRESSED']
# On set, cruise set speed pulses between 254~255 and the set speed prev is set to avoid this.
self.v_cruise_pcm = self.v_cruise_pcm_prev if cp.vl["ACC_HUD"][
'CRUISE_SPEED'] > 160.0 else cp.vl["ACC_HUD"]['CRUISE_SPEED']
self.v_cruise_pcm_prev = self.v_cruise_pcm
else:
self.brake_switch = cp.vl["POWERTRAIN_DATA"]['BRAKE_SWITCH']
self.cruise_speed_offset = calc_cruise_offset(
cp.vl["CRUISE_PARAMS"]['CRUISE_SPEED_OFFSET'], self.v_ego)
self.v_cruise_pcm = cp.vl["CRUISE"]['CRUISE_SPEED_PCM']
# brake switch has shown some single time step noise, so only considered when
# switch is on for at least 2 consecutive CAN samples
self.brake_pressed = cp.vl["POWERTRAIN_DATA"]['BRAKE_PRESSED'] or \
(self.brake_switch and self.brake_switch_prev and \
cp.ts["POWERTRAIN_DATA"]['BRAKE_SWITCH'] != self.brake_switch_ts)
self.brake_switch_prev = self.brake_switch
self.brake_switch_ts = cp.ts["POWERTRAIN_DATA"]['BRAKE_SWITCH']
self.user_brake = cp.vl["VSA_STATUS"]['USER_BRAKE']
self.pcm_acc_status = cp.vl["POWERTRAIN_DATA"]['ACC_STATUS']
# Gets rid of Pedal Grinding noise when brake is pressed at slow speeds for some models
if self.CP.carFingerprint in (CAR.PILOT, CAR.PILOT_2018,
CAR.PILOT_2019, CAR.RIDGELINE):
if self.user_brake > 0.05:
self.brake_pressed = 1
# when user presses distance button on steering wheel
if self.cruise_setting == 3:
if cp.vl["SCM_BUTTONS"]["CRUISE_SETTING"] == 0:
self.trMode = (self.trMode + 1) % 4
self.kegman = kegman_conf()
self.kegman.conf['lastTrMode'] = str(
self.trMode) # write last distance bar setting to file
self.kegman.write_config(self.kegman.conf)
# when user presses LKAS button on steering wheel
if self.cruise_setting == 1:
if cp.vl["SCM_BUTTONS"]["CRUISE_SETTING"] == 0:
if self.lkMode:
self.lkMode = False
else:
self.lkMode = True
self.prev_cruise_setting = self.cruise_setting
self.cruise_setting = cp.vl["SCM_BUTTONS"]['CRUISE_SETTING']
self.read_distance_lines = self.trMode + 1
if self.read_distance_lines != self.read_distance_lines_prev:
self.read_distance_lines_prev = self.read_distance_lines
# TODO: discover the CAN msg that has the imperial unit bit for all other cars
self.is_metric = not cp.vl["HUD_SETTING"][
'IMPERIAL_UNIT'] if self.CP.carFingerprint in (
CAR.CIVIC) else False
if self.CP.carFingerprint in HONDA_BOSCH:
self.stock_aeb = bool(
cp_cam.vl["ACC_CONTROL"]["AEB_STATUS"]
and cp_cam.vl["ACC_CONTROL"]["ACCEL_COMMAND"] < -1e-5)
else:
self.stock_aeb = bool(
cp_cam.vl["BRAKE_COMMAND"]["AEB_REQ_1"]
and cp_cam.vl["BRAKE_COMMAND"]["COMPUTER_BRAKE"] > 1e-5)
if self.CP.carFingerprint in HONDA_BOSCH:
self.stock_hud = False
self.stock_fcw = False
else:
#self.stock_fcw = bool(cp_cam.vl["BRAKE_COMMAND"]["FCW"] != 0)
self.stock_fcw = False # Disable stock FCW because it's too bloody sensitive
self.stock_hud = cp_cam.vl["ACC_HUD"]
self.stock_brake = cp_cam.vl["BRAKE_COMMAND"]
def update(self, sm, CP):
v_ego = sm['carState'].vEgo
active = sm['controlsState'].active
measured_curvature = sm['controlsState'].curvature
md = sm['modelV2']
self.LP.parse_model(sm['modelV2'])
if len(md.position.x) == TRAJECTORY_SIZE and len(md.orientation.x) == TRAJECTORY_SIZE:
self.path_xyz = np.column_stack([md.position.x, md.position.y, md.position.z])
self.t_idxs = np.array(md.position.t)
self.plan_yaw = list(md.orientation.z)
if len(md.orientation.xStd) == TRAJECTORY_SIZE:
self.path_xyz_stds = np.column_stack([md.position.xStd, md.position.yStd, md.position.zStd])
# Lane change logic
one_blinker = sm['carState'].leftBlinker != sm['carState'].rightBlinker
below_lane_change_speed = v_ego < LANE_CHANGE_SPEED_MIN
if sm['carState'].leftBlinker:
self.lane_change_direction = LaneChangeDirection.left
elif sm['carState'].rightBlinker:
self.lane_change_direction = LaneChangeDirection.right
if (not active) or (self.lane_change_timer > LANE_CHANGE_TIME_MAX):
self.lane_change_state = LaneChangeState.off
self.lane_change_direction = LaneChangeDirection.none
else:
torque_applied = sm['carState'].steeringPressed and \
((sm['carState'].steeringTorque > 0 and self.lane_change_direction == LaneChangeDirection.left) or
(sm['carState'].steeringTorque < 0 and self.lane_change_direction == LaneChangeDirection.right))
blindspot_detected = ((sm['carState'].leftBlindspot and self.lane_change_direction == LaneChangeDirection.left) or
(sm['carState'].rightBlindspot and self.lane_change_direction == LaneChangeDirection.right))
lane_change_prob = self.LP.l_lane_change_prob + self.LP.r_lane_change_prob
# State transitions
# off
if self.lane_change_state == LaneChangeState.off and one_blinker and not self.prev_one_blinker and not below_lane_change_speed:
self.lane_change_state = LaneChangeState.preLaneChange
self.lane_change_ll_prob = 1.0
# pre
elif self.lane_change_state == LaneChangeState.preLaneChange:
if not one_blinker or below_lane_change_speed:
self.lane_change_state = LaneChangeState.off
elif torque_applied and not blindspot_detected:
self.lane_change_state = LaneChangeState.laneChangeStarting
# starting
elif self.lane_change_state == LaneChangeState.laneChangeStarting:
# fade out over .5s
self.lane_change_ll_prob = max(self.lane_change_ll_prob - 2*DT_MDL, 0.0)
# 98% certainty
if lane_change_prob < 0.02 and self.lane_change_ll_prob < 0.01:
self.lane_change_state = LaneChangeState.laneChangeFinishing
# finishing
elif self.lane_change_state == LaneChangeState.laneChangeFinishing:
# fade in laneline over 1s
self.lane_change_ll_prob = min(self.lane_change_ll_prob + DT_MDL, 1.0)
if one_blinker and self.lane_change_ll_prob > 0.99:
self.lane_change_state = LaneChangeState.preLaneChange
elif self.lane_change_ll_prob > 0.99:
self.lane_change_state = LaneChangeState.off
if self.lane_change_state in [LaneChangeState.off, LaneChangeState.preLaneChange]:
self.lane_change_timer = 0.0
else:
self.lane_change_timer += DT_MDL
self.prev_one_blinker = one_blinker
self.desire = DESIRES[self.lane_change_direction][self.lane_change_state]
# Turn off lanes during lane change
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
if self.use_lanelines:
std_cost_mult = np.clip(abs(self.path_xyz[0,1]/self.path_xyz_stds[0,1]), 0.5, 5.0)
d_path_xyz = self.LP.get_d_path(v_ego, self.t_idxs, self.path_xyz)
else:
std_cost_mult = 1.0
d_path_xyz = self.path_xyz
y_pts = np.interp(v_ego * self.t_idxs[:MPC_N + 1], np.linalg.norm(d_path_xyz, axis=1), d_path_xyz[:,1])
heading_pts = np.interp(v_ego * self.t_idxs[:MPC_N + 1], np.linalg.norm(self.path_xyz, axis=1), self.plan_yaw)
self.y_pts = y_pts
assert len(y_pts) == MPC_N + 1
assert len(heading_pts) == MPC_N + 1
self.libmpc.set_weights(std_cost_mult*MPC_COST_LAT.PATH, 0.0, CP.steerRateCost)
self.libmpc.run_mpc(self.cur_state, self.mpc_solution,
float(v_ego),
CAR_ROTATION_RADIUS,
list(y_pts),
list(heading_pts))
# init state for next
self.cur_state.x = 0.0
self.cur_state.y = 0.0
self.cur_state.psi = 0.0
self.cur_state.curvature = interp(DT_MDL, self.t_idxs[:MPC_N + 1], self.mpc_solution.curvature)
# TODO this needs more thought, use .2s extra for now to estimate other delays
delay = CP.steerActuatorDelay + .2
current_curvature = self.mpc_solution.curvature[0]
psi = interp(delay, self.t_idxs[:MPC_N + 1], self.mpc_solution.psi)
next_curvature_rate = self.mpc_solution.curvature_rate[0]
# MPC can plan to turn the wheel and turn back before t_delay. This means
# in high delay cases some corrections never even get commanded. So just use
# psi to calculate a simple linearization of desired curvature
curvature_diff_from_psi = psi / (max(v_ego, 1e-1) * delay) - current_curvature
next_curvature = current_curvature + 2 * curvature_diff_from_psi
self.desired_curvature = next_curvature
self.desired_curvature_rate = next_curvature_rate
max_curvature_rate = interp(v_ego, MAX_CURVATURE_RATE_SPEEDS, MAX_CURVATURE_RATES)
self.safe_desired_curvature_rate = clip(self.desired_curvature_rate,
-max_curvature_rate,
max_curvature_rate)
self.safe_desired_curvature = clip(self.desired_curvature,
self.safe_desired_curvature - max_curvature_rate/DT_MDL,
self.safe_desired_curvature + max_curvature_rate/DT_MDL)
# Check for infeasable MPC solution
mpc_nans = any(math.isnan(x) for x in self.mpc_solution.curvature)
t = sec_since_boot()
if mpc_nans:
self.libmpc.init()
self.cur_state.curvature = measured_curvature
if t > self.last_cloudlog_t + 5.0:
self.last_cloudlog_t = t
cloudlog.warning("Lateral mpc - nan: True")
if self.mpc_solution[0].cost > 20000. or mpc_nans: # TODO: find a better way to detect when MPC did not converge
self.solution_invalid_cnt += 1
else:
self.solution_invalid_cnt = 0
def update(self, active, v_ego, brake_pressed, standstill,
cruise_standstill, v_cruise, v_target, v_target_future,
a_target, CP, gas_interceptor, gas_button_status, decelForTurn,
longitudinalPlanSource, lead_one, gas_pressed):
"""Update longitudinal control. This updates the state machine and runs a PID loop"""
# Actuation limits
if lead_one is not None:
self.last_lead = lead_one
self.none_count = 0
else:
self.none_count = clip(self.none_count + 1, 0, 10)
#gas_max = interp(v_ego, CP.gasMaxBP, CP.gasMaxV)
gas_max = self.dynamic_gas(v_ego, gas_interceptor, gas_button_status)
brake_max = interp(v_ego, CP.brakeMaxBP, CP.brakeMaxV)
# Update state machine
output_gb = self.last_output_gb
self.long_control_state = long_control_state_trans(
active, self.long_control_state, v_ego, v_target_future,
self.v_pid, output_gb, brake_pressed, cruise_standstill)
v_ego_pid = max(
v_ego, MIN_CAN_SPEED
) # Without this we get jumps, CAN bus reports 0 when speed < 0.3
if self.long_control_state == LongCtrlState.off:
self.v_pid = v_ego_pid
self.pid.reset()
output_gb = 0.
# tracking objects and driving
elif self.long_control_state == LongCtrlState.pid:
self.v_pid = v_target
self.pid.pos_limit = gas_max
self.pid.neg_limit = -brake_max
# Toyota starts braking more when it thinks you want to stop
# Freeze the integrator so we don't accelerate to compensate, and don't allow positive acceleration
prevent_overshoot = not CP.stoppingControl and v_ego < 1.5 and v_target_future < 0.7
deadzone = interp(v_ego_pid, CP.longitudinalTuning.deadzoneBP,
CP.longitudinalTuning.deadzoneV)
if longitudinalPlanSource == 'cruise':
if decelForTurn and not self.lastdecelForTurn:
self.lastdecelForTurn = True
self.pid._k_p = (CP.longitudinalTuning.kpBP, [
x * 0 for x in CP.longitudinalTuning.kpV
])
self.pid._k_i = (CP.longitudinalTuning.kiBP, [
x * 0 for x in CP.longitudinalTuning.kiV
])
self.pid.i = 0.0
self.pid.k_f = 1.0
if self.lastdecelForTurn and not decelForTurn:
self.lastdecelForTurn = False
self.pid._k_p = (CP.longitudinalTuning.kpBP,
CP.longitudinalTuning.kpV)
self.pid._k_i = (CP.longitudinalTuning.kiBP,
CP.longitudinalTuning.kiV)
self.pid.k_f = 1.0
else:
self.lastdecelForTurn = False
self.pid._k_p = (CP.longitudinalTuning.kpBP,
CP.longitudinalTuning.kpV)
self.pid._k_i = (CP.longitudinalTuning.kiBP,
CP.longitudinalTuning.kiV)
self.pid.k_f = 1.0
if gas_pressed or brake_pressed:
if not self.freeze:
self.pid.i = 0.0
self.freeze = True
else:
if self.freeze:
self.freeze = False
output_gb = self.pid.update(self.v_pid,
v_ego_pid,
speed=v_ego_pid,
deadzone=deadzone,
feedforward=a_target,
freeze_integrator=(prevent_overshoot
or gas_pressed
or brake_pressed))
if prevent_overshoot:
output_gb = min(output_gb, 0.0)
# Intention is to stop, switch to a different brake control until we stop
elif self.long_control_state == LongCtrlState.stopping:
# Keep applying brakes until the car is stopped
if not standstill or output_gb > -BRAKE_STOPPING_TARGET:
output_gb -= STOPPING_BRAKE_RATE / RATE
output_gb = clip(output_gb, -brake_max, gas_max)
self.v_pid = v_ego
self.pid.reset()
# Intention is to move again, release brake fast before handing control to PID
elif self.long_control_state == LongCtrlState.starting:
if output_gb < -0.2:
output_gb += STARTING_BRAKE_RATE / RATE
self.v_pid = v_ego
self.pid.reset()
self.last_output_gb = output_gb
final_gas = clip(output_gb, 0., gas_max)
final_brake = -clip(output_gb, -brake_max, 0.)
return final_gas, final_brake
def update(self, enabled, 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 = enabled and not CS.out.steerWarning and CS.out.vEgo >= CS.CP.minSteerSpeed
if not lkas_active:
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,
left_lane, right_lane, left_lane_depart, right_lane_depart)
can_sends = []
# tester present - w/ no response (keeps radar disabled)
if CS.CP.openpilotLongitudinalControl:
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, left_lane, right_lane,
left_lane_warning, right_lane_warning))
if not CS.CP.openpilotLongitudinalControl:
if pcm_cancel_cmd:
can_sends.append(
create_clu11(self.packer, frame, CS.clu11, Buttons.CANCEL))
elif CS.out.cruiseState.standstill:
# send resume at a max freq of 10Hz
if (frame - self.last_resume_frame) * DT_CTRL > 0.1:
# send 25 messages at a time to increases the likelihood of resume being accepted
can_sends.extend([
create_clu11(self.packer, frame, CS.clu11,
Buttons.RES_ACCEL)
] * 25)
self.last_resume_frame = frame
if frame % 2 == 0 and CS.CP.openpilotLongitudinalControl:
lead_visible = False
accel = actuators.accel if enabled else 0
jerk = clip(2.0 * (accel - CS.out.aEgo), -12.7, 12.7)
if accel < 0:
accel = interp(accel - CS.out.aEgo, [-1.0, -0.5],
[2 * accel, accel])
accel = clip(accel, CarControllerParams.ACCEL_MIN,
CarControllerParams.ACCEL_MAX)
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, 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))
# 5 Hz ACC options
if frame % 20 == 0 and CS.CP.openpilotLongitudinalControl:
can_sends.extend(create_acc_opt(self.packer))
# 2 Hz front radar options
if frame % 50 == 0 and CS.CP.openpilotLongitudinalControl:
can_sends.append(create_frt_radar_opt(self.packer))
new_actuators = actuators.copy()
new_actuators.steer = apply_steer / self.p.STEER_MAX
new_actuators.accel = self.accel
return new_actuators, can_sends
def dynamic_gas(self, v_ego, gas_interceptor, gas_button_status):
dynamic = False
if gas_interceptor:
if gas_button_status == 0:
dynamic = True
x = [
0.0, 1.4082, 2.80311, 4.22661, 5.38271, 6.16561, 7.24781,
8.28308, 10.24465, 12.96402, 15.42303, 18.11903, 20.11703,
24.46614, 29.05805, 32.71015, 35.76326
]
y = [
0.2, 0.20443, 0.21592, 0.23334, 0.25734, 0.27916, 0.3229,
0.34784, 0.36765, 0.38, 0.396, 0.409, 0.425, 0.478, 0.55,
0.621, 0.7
]
elif gas_button_status == 1:
y = [0.25, 0.9, 0.9]
elif gas_button_status == 2:
y = [0.2, 0.5, 0.7]
else:
if gas_button_status == 0:
dynamic = True
x = [
0.0, 1.4082, 2.80311, 4.22661, 5.38271, 6.16561, 7.24781,
8.28308, 10.24465, 12.96402, 15.42303, 18.11903, 20.11703,
24.46614, 29.05805, 32.71015, 35.76326
]
y = [
0.35, 0.47, 0.43, 0.35, 0.3, 0.3, 0.3229, 0.34784, 0.36765,
0.38, 0.396, 0.409, 0.425, 0.478, 0.55, 0.621, 0.7
]
elif gas_button_status == 1:
y = [0.9, 0.95, 0.99]
elif gas_button_status == 2:
y = [0.25, 0.2, 0.2]
if not dynamic:
x = [0., 9., 35.] # default BP values
accel = interp(v_ego, x, y)
if self.none_count < 10 and self.last_lead is not None and self.last_lead.status: # if returned nones is less than 10, last lead is not none, and last lead's status is true assume lead
v_rel = self.last_lead.vRel
#a_lead = self.last_lead.aLeadK # to use later
#x_lead = self.last_lead.dRel
else:
v_rel = None
#a_lead = None
#x_lead = None
if dynamic and v_rel is not None: # dynamic gas profile specific operations, and if lead
if v_ego < 6.7056: # if under 15 mph
x = [1.61479, 1.99067, 2.28537, 2.49888, 2.6312, 2.68224]
y = [
-accel, -(accel / 1.06), -(accel / 1.2), -(accel / 1.8),
-(accel / 4.4), 0
] # array that matches current chosen accel value
accel += interp(v_rel, x, y)
else:
x = [-0.89408, 0, 0.89408, 4.4704]
y = [-.15, -.05, .005, .05]
accel += interp(v_rel, x, y)
min_return = 0.0
max_return = 1.0
return round(max(min(accel, max_return), min_return),
5) # ensure we return a value between range
def update(self, enabled, CS, frame, actuators, \
hud_v_cruise, hud_show_lanes, hud_show_car, hud_alert):
P = self.params
# Send CAN commands.
can_sends = []
canbus = self.canbus
alert_out = process_hud_alert(hud_alert)
steer = alert_out
alert_out = process_hud_alert(hud_alert)
steer = alert_out
### STEER ###
if (frame % P.STEER_STEP) == 0:
lkas_enabled = enabled and not CS.steer_not_allowed and CS.lkMode and CS.v_ego > P.MIN_STEER_SPEED and not CS.left_blinker_on and not CS.right_blinker_on
if lkas_enabled:
apply_steer = actuators.steer * P.STEER_MAX
apply_steer = apply_std_steer_torque_limits(
apply_steer, self.apply_steer_last, CS.steer_torque_driver,
P)
else:
apply_steer = 0
self.apply_steer_last = apply_steer
idx = (frame // P.STEER_STEP) % 4
if self.car_fingerprint in SUPERCRUISE_CARS:
can_sends += gmcan.create_steering_control_ct6(
self.packer_pt, canbus, apply_steer, CS.v_ego, idx,
lkas_enabled)
else:
can_sends.append(
gmcan.create_steering_control(self.packer_pt,
canbus.powertrain,
apply_steer, idx,
lkas_enabled))
### GAS/BRAKE ###
if self.car_fingerprint not in SUPERCRUISE_CARS:
# no output if not enabled, but keep sending keepalive messages
# treat pedals as one
final_pedal = actuators.gas - actuators.brake
# *** apply pedal hysteresis ***
final_brake, self.brake_steady = actuator_hystereses(
final_pedal, self.pedal_steady)
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(final_pedal, P.GAS_LOOKUP_BP,
P.GAS_LOOKUP_V)))
apply_brake = int(
round(
interp(final_pedal, P.BRAKE_LOOKUP_BP,
P.BRAKE_LOOKUP_V)))
# Gas/regen and brakes - all at 25Hz
if (frame % 4) == 0:
idx = (frame // 4) % 4
car_stopping = apply_gas < P.ZERO_GAS
standstill = CS.pcm_acc_status == AccState.STANDSTILL
at_full_stop = enabled and standstill and car_stopping
near_stop = enabled and (
CS.v_ego < P.NEAR_STOP_BRAKE_PHASE) and car_stopping
can_sends.append(
gmcan.create_friction_brake_command(
self.packer_ch, canbus.chassis, apply_brake, idx,
near_stop, at_full_stop))
# Auto-resume from full stop by resetting ACC control
acc_enabled = enabled
if standstill and not car_stopping:
acc_enabled = False
can_sends.append(
gmcan.create_gas_regen_command(self.packer_pt,
canbus.powertrain,
apply_gas, idx, acc_enabled,
at_full_stop))
# Send dashboard UI commands (ACC status), 25hz
follow_level = CS.get_follow_level()
if (frame % 4) == 0:
send_fcw = 0
if self.fcw_count > 0:
self.fcw_count -= 1
send_fcw = 0x3
can_sends.append(
gmcan.create_acc_dashboard_command(
self.packer_pt, canbus.powertrain, enabled,
hud_v_cruise * CV.MS_TO_KPH, hud_show_car,
follow_level, send_fcw))
# Radar needs to know current speed and yaw rate (50hz),
# and that ADAS is alive (10hz)
time_and_headlights_step = 10
tt = frame * DT_CTRL
if frame % time_and_headlights_step == 0:
idx = (frame // time_and_headlights_step) % 4
can_sends.append(
gmcan.create_adas_time_status(
canbus.obstacle, int((tt - self.start_time) * 60),
idx))
can_sends.append(
gmcan.create_adas_headlights_status(canbus.obstacle))
speed_and_accelerometer_step = 2
if frame % speed_and_accelerometer_step == 0:
idx = (frame // speed_and_accelerometer_step) % 4
can_sends.append(
gmcan.create_adas_steering_status(canbus.obstacle, idx))
can_sends.append(
gmcan.create_adas_accelerometer_speed_status(
canbus.obstacle, CS.v_ego, idx))
if frame % P.ADAS_KEEPALIVE_STEP == 0:
can_sends += gmcan.create_adas_keepalive(canbus.powertrain)
# Show green icon when LKA torque is applied, and
# alarming orange icon when approaching torque limit.
# If not sent again, LKA icon disappears in about 5 seconds.
# Conveniently, sending camera message periodically also works as a keepalive.
lka_active = CS.lkas_status == 1
lka_critical = lka_active and abs(actuators.steer) > 0.9
lka_icon_status = (lka_active, lka_critical)
if frame % P.CAMERA_KEEPALIVE_STEP == 0 \
or lka_icon_status != self.lka_icon_status_last:
can_sends.append(
gmcan.create_lka_icon_command(canbus.sw_gmlan, lka_active,
lka_critical, steer))
self.lka_icon_status_last = lka_icon_status
return can_sends
def update(self, enabled, CS, frame, actuators, pcm_speed, pcm_override,
pcm_cancel_cmd, pcm_accel, hud_v_cruise, hud_show_lanes,
hud_show_car, hud_alert):
P = self.params
# *** apply brake hysteresis ***
brake, self.braking, self.brake_steady = actuator_hystereses(
actuators.brake, self.braking, self.brake_steady, CS.out.vEgo,
CS.CP.carFingerprint)
# *** no output if not enabled ***
if not enabled and CS.out.cruiseState.enabled:
# send pcm acc cancel cmd if drive is disabled but pcm is still on, or if the system can't be activated
pcm_cancel_cmd = True
# *** rate limit after the enable check ***
self.brake_last = rate_limit(brake, self.brake_last, -2., DT_CTRL)
# vehicle hud display, wait for one update from 10Hz 0x304 msg
if hud_show_lanes:
hud_lanes = 1
else:
hud_lanes = 0
if enabled:
if hud_show_car:
hud_car = 2
else:
hud_car = 1
else:
hud_car = 0
fcw_display, steer_required, acc_alert = process_hud_alert(hud_alert)
hud = HUDData(int(pcm_accel), int(round(hud_v_cruise)), hud_car,
hud_lanes, fcw_display, acc_alert, steer_required)
# **** process the car messages ****
# 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 = enabled and not CS.steer_not_allowed
# Send CAN commands.
can_sends = []
# 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))
# Send dashboard UI commands.
if (frame % 10) == 0:
idx = (frame // 10) % 4
can_sends.extend(
hondacan.create_ui_commands(self.packer, pcm_speed, hud,
CS.CP.carFingerprint, CS.is_metric,
idx,
CS.CP.openpilotLongitudinalControl,
CS.stock_hud))
if not CS.CP.openpilotLongitudinalControl:
if (frame % 2) == 0:
idx = frame // 2
can_sends.append(
hondacan.create_bosch_supplemental_1(
self.packer, CS.CP.carFingerprint, idx))
# If using stock ACC, spam cancel command to kill gas when OP disengages.
if pcm_cancel_cmd:
can_sends.append(
hondacan.spam_buttons_command(self.packer,
CruiseButtons.CANCEL, idx,
CS.CP.carFingerprint))
elif CS.out.cruiseState.standstill:
can_sends.append(
hondacan.spam_buttons_command(self.packer,
CruiseButtons.RES_ACCEL, idx,
CS.CP.carFingerprint))
else:
# Send gas and brake commands.
if (frame % 2) == 0:
idx = (frame /
2) % 4 #Clarity: Why do we need this? -wirelessnet2
ts = frame * DT_CTRL
if CS.CP.carFingerprint in HONDA_BOSCH:
pass # TODO: implement
else:
apply_gas = clip(actuators.gas, 0., 1.)
apply_brake = int(
clip(self.brake_last * P.BRAKE_MAX, 0,
P.BRAKE_MAX - 1))
pump_on, self.last_pump_ts = brake_pump_hysteresis(
apply_brake, self.apply_brake_last, self.last_pump_ts,
ts)
can_sends.extend(
hondacan.create_brake_command(
self.packer,
apply_brake, #Clarity: We don't use comma's brake pump algo because it casues jerky braking on Clarity. -wirelessnet2
pcm_override,
pcm_cancel_cmd,
hud.fcw,
idx,
CS.CP.carFingerprint,
CS.stock_brake))
self.apply_brake_last = apply_brake
if CS.CP.enableGasInterceptor:
# send exactly zero if apply_gas is zero. Interceptor will send the max between read value and apply_gas.
# This prevents unexpected pedal range rescaling
can_sends.append(
create_gas_command(self.packer, apply_gas, idx))
#Clarity: This allows us to manually drive the radar since we don't have a factory ADAS camera to do so. -wirelessnet2
# radar at 20Hz, but these msgs need to be sent at 50Hz on ilx (seems like an Acura bug)
radar_send_step = 5
# if (frame % radar_send_step) == 0:
idx = (frame / radar_send_step) % 4
can_sends.extend(
hondacan.create_radar_commands(self.packer, CS.vEgoRawKph,
idx))
return can_sends
def update(self, cp):
# copy can_valid
self.can_valid = cp.can_valid
# car params
v_weight_v = [
0., 1.
] # don't trust smooth speed at low values to avoid premature zero snapping
v_weight_bp = [
1., 6.
] # smooth blending, below ~0.6m/s the smooth speed snaps to zero
# update prevs, update must run once per loop
self.prev_cruise_buttons = self.cruise_buttons
self.prev_cruise_setting = self.cruise_setting
self.prev_blinker_on = self.blinker_on
self.prev_left_blinker_on = self.left_blinker_on
self.prev_right_blinker_on = self.right_blinker_on
# ******************* parse out can *******************
if self.CP.carFingerprint in (CAR.ACCORD):
self.door_all_closed = not cp.vl["SCM_FEEDBACK"][
'DRIVERS_DOOR_OPEN']
else:
self.door_all_closed = not any([
cp.vl["DOORS_STATUS"]['DOOR_OPEN_FL'],
cp.vl["DOORS_STATUS"]['DOOR_OPEN_FR'],
cp.vl["DOORS_STATUS"]['DOOR_OPEN_RL'],
cp.vl["DOORS_STATUS"]['DOOR_OPEN_RR']
])
self.seatbelt = not cp.vl["SEATBELT_STATUS"][
'SEATBELT_DRIVER_LAMP'] and cp.vl["SEATBELT_STATUS"][
'SEATBELT_DRIVER_LATCHED']
# 2 = temporary; 3 = TBD; 4 = temporary, hit a bump; 5 = (permanent); 6 = temporary; 7 = (permanent)
# TODO: Use values from DBC to parse this field
self.steer_error = cp.vl["STEER_STATUS"]['STEER_STATUS'] not in [
0, 2, 3, 4, 6
]
self.steer_not_allowed = cp.vl["STEER_STATUS"]['STEER_STATUS'] != 0
self.steer_warning = cp.vl["STEER_STATUS"]['STEER_STATUS'] not in [
0, 3
] # 3 is low speed lockout, not worth a warning
self.brake_error = cp.vl["STANDSTILL"]['BRAKE_ERROR_1'] or cp.vl[
"STANDSTILL"]['BRAKE_ERROR_2']
self.esp_disabled = cp.vl["VSA_STATUS"]['ESP_DISABLED']
# calc best v_ego estimate, by averaging two opposite corners
self.v_wheel_fl = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_FL'] * CV.KPH_TO_MS
self.v_wheel_fr = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_FR'] * CV.KPH_TO_MS
self.v_wheel_rl = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_RL'] * CV.KPH_TO_MS
self.v_wheel_rr = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_RR'] * CV.KPH_TO_MS
self.v_wheel = float(
np.mean([
self.v_wheel_fl, self.v_wheel_fr, self.v_wheel_rl,
self.v_wheel_rr
]))
# blend in transmission speed at low speed, since it has more low speed accuracy
self.v_weight = interp(self.v_wheel, v_weight_bp, v_weight_v)
speed = (1. - self.v_weight) * cp.vl["ENGINE_DATA"][
'XMISSION_SPEED'] * CV.KPH_TO_MS + self.v_weight * self.v_wheel
if abs(
speed - self.v_ego
) > 2.0: # Prevent large accelerations when car starts at non zero speed
self.v_ego_x = np.matrix([[speed], [0.0]])
self.v_ego_raw = speed
v_ego_x = self.v_ego_kf.update(speed)
self.v_ego = float(v_ego_x[0])
self.a_ego = float(v_ego_x[1])
# this is a hack for the interceptor. This is now only used in the simulation
# TODO: Replace tests by toyota so this can go away
if self.CP.enableGasInterceptor:
self.user_gas = cp.vl["GAS_SENSOR"]['INTERCEPTOR_GAS']
self.user_gas_pressed = self.user_gas > 0 # this works because interceptor read < 0 when pedal position is 0. Once calibrated, this will change
can_gear_shifter = cp.vl["GEARBOX"]['GEAR_SHIFTER']
self.gear = 0 if self.CP.carFingerprint == CAR.CIVIC else cp.vl[
"GEARBOX"]['GEAR']
self.angle_steers = cp.vl["STEERING_SENSORS"]['STEER_ANGLE']
self.angle_steers_rate = cp.vl["STEERING_SENSORS"]['STEER_ANGLE_RATE']
self.cruise_setting = cp.vl["SCM_BUTTONS"]['CRUISE_SETTING']
self.cruise_buttons = cp.vl["SCM_BUTTONS"]['CRUISE_BUTTONS']
self.blinker_on = cp.vl["SCM_FEEDBACK"]['LEFT_BLINKER'] or cp.vl[
"SCM_FEEDBACK"]['RIGHT_BLINKER']
self.left_blinker_on = cp.vl["SCM_FEEDBACK"]['LEFT_BLINKER']
self.right_blinker_on = cp.vl["SCM_FEEDBACK"]['RIGHT_BLINKER']
if self.CP.carFingerprint in (CAR.CIVIC, CAR.ODYSSEY, CAR.CRV_5G,
CAR.ACCORD, CAR.CIVIC_HATCH):
self.park_brake = cp.vl["EPB_STATUS"]['EPB_STATE'] != 0
self.brake_hold = cp.vl["VSA_STATUS"]['BRAKE_HOLD_ACTIVE']
self.main_on = cp.vl["SCM_FEEDBACK"]['MAIN_ON']
else:
self.park_brake = 0 # TODO
self.brake_hold = 0 # TODO
self.main_on = cp.vl["SCM_BUTTONS"]['MAIN_ON']
self.gear_shifter = parse_gear_shifter(can_gear_shifter,
self.CP.carFingerprint)
self.pedal_gas = cp.vl["POWERTRAIN_DATA"]['PEDAL_GAS']
# crv doesn't include cruise control
if self.CP.carFingerprint in (CAR.CRV, CAR.ODYSSEY, CAR.ACURA_RDX,
CAR.RIDGELINE):
self.car_gas = self.pedal_gas
else:
self.car_gas = cp.vl["GAS_PEDAL_2"]['CAR_GAS']
#rdx has different steer override threshold
if self.CP.carFingerprint in (CAR.ACURA_RDX):
self.steer_override = abs(
cp.vl["STEER_STATUS"]['STEER_TORQUE_SENSOR']) > 400
else:
self.steer_override = abs(
cp.vl["STEER_STATUS"]['STEER_TORQUE_SENSOR']) > 1200
self.steer_torque_driver = cp.vl["STEER_STATUS"]['STEER_TORQUE_SENSOR']
self.brake_switch = cp.vl["POWERTRAIN_DATA"]['BRAKE_SWITCH']
if self.CP.radarOffCan:
self.stopped = cp.vl["ACC_HUD"]['CRUISE_SPEED'] == 252.
self.cruise_speed_offset = calc_cruise_offset(0, self.v_ego)
if self.CP.carFingerprint == CAR.CIVIC_HATCH:
self.brake_switch = cp.vl["POWERTRAIN_DATA"]['BRAKE_SWITCH']
self.brake_pressed = cp.vl["POWERTRAIN_DATA"]['BRAKE_PRESSED'] or \
(self.brake_switch and self.brake_switch_prev and \
cp.ts["POWERTRAIN_DATA"]['BRAKE_SWITCH'] != self.brake_switch_ts)
self.brake_switch_prev = self.brake_switch
self.brake_switch_ts = cp.ts["POWERTRAIN_DATA"]['BRAKE_SWITCH']
else:
self.brake_pressed = cp.vl["BRAKE_MODULE"]['BRAKE_PRESSED']
# On set, cruise set speed pulses between 254~255 and the set speed prev is set to avoid this.
self.v_cruise_pcm = self.v_cruise_pcm_prev if cp.vl["ACC_HUD"][
'CRUISE_SPEED'] > 160.0 else cp.vl["ACC_HUD"]['CRUISE_SPEED']
self.v_cruise_pcm_prev = self.v_cruise_pcm
else:
self.brake_switch = cp.vl["POWERTRAIN_DATA"]['BRAKE_SWITCH']
self.cruise_speed_offset = calc_cruise_offset(
cp.vl["CRUISE_PARAMS"]['CRUISE_SPEED_OFFSET'], self.v_ego)
self.v_cruise_pcm = cp.vl["CRUISE"]['CRUISE_SPEED_PCM']
# brake switch has shown some single time step noise, so only considered when
# switch is on for at least 2 consecutive CAN samples
self.brake_pressed = cp.vl["POWERTRAIN_DATA"]['BRAKE_PRESSED'] or \
(self.brake_switch and self.brake_switch_prev and \
cp.ts["POWERTRAIN_DATA"]['BRAKE_SWITCH'] != self.brake_switch_ts)
self.brake_switch_prev = self.brake_switch
self.brake_switch_ts = cp.ts["POWERTRAIN_DATA"]['BRAKE_SWITCH']
self.user_brake = cp.vl["VSA_STATUS"]['USER_BRAKE']
self.standstill = not cp.vl["STANDSTILL"]['WHEELS_MOVING']
self.pcm_acc_status = cp.vl["POWERTRAIN_DATA"]['ACC_STATUS']
self.hud_lead = cp.vl["ACC_HUD"]['HUD_LEAD']
def update(self, active, CS, v_target, v_target_future, a_target, CP):
"""Update longitudinal control. This updates the state machine and runs a PID loop"""
# Actuation limits
gas_max = interp(CS.vEgo, CP.gasMaxBP, CP.gasMaxV)
brake_max = interp(CS.vEgo, CP.brakeMaxBP, CP.brakeMaxV)
# Update state machine
output_gb = self.last_output_gb
self.long_control_state = long_control_state_trans(
active, self.long_control_state, CS.vEgo, v_target_future,
self.v_pid, output_gb, CS.brakePressed, CS.cruiseState.standstill)
v_ego_pid = max(
CS.vEgo, MIN_CAN_SPEED
) # Without this we get jumps, CAN bus reports 0 when speed < 0.3
if self.long_control_state == LongCtrlState.off or CS.gasPressed:
self.v_pid = v_ego_pid
self.pid.reset()
output_gb = 0.
# tracking objects and driving
elif self.long_control_state == LongCtrlState.pid:
self.v_pid = v_target
self.pid.pos_limit = gas_max
self.pid.neg_limit = -brake_max
# Toyota starts braking more when it thinks you want to stop
# Freeze the integrator so we don't accelerate to compensate, and don't allow positive acceleration
prevent_overshoot = not CP.stoppingControl and CS.vEgo < 1.5 and v_target_future < 0.7
deadzone = interp(v_ego_pid, CP.longitudinalTuning.deadzoneBP,
CP.longitudinalTuning.deadzoneV)
output_gb = self.pid.update(self.v_pid,
v_ego_pid,
speed=v_ego_pid,
deadzone=deadzone,
feedforward=a_target,
freeze_integrator=prevent_overshoot)
if prevent_overshoot:
output_gb = min(output_gb, 0.0)
# Intention is to stop, switch to a different brake control until we stop
elif self.long_control_state == LongCtrlState.stopping:
# Keep applying brakes until the car is stopped
if not CS.standstill or output_gb > -BRAKE_STOPPING_TARGET:
output_gb -= STOPPING_BRAKE_RATE / RATE
output_gb = clip(output_gb, -brake_max, gas_max)
self.v_pid = CS.vEgo
self.pid.reset()
# Intention is to move again, release brake fast before handing control to PID
elif self.long_control_state == LongCtrlState.starting:
if output_gb < -0.2:
output_gb += STARTING_BRAKE_RATE / RATE
self.v_pid = CS.vEgo
self.pid.reset()
self.last_output_gb = output_gb
final_gas = clip(output_gb, 0., gas_max)
final_brake = -clip(output_gb, -brake_max, 0.)
return final_gas, final_brake
def update(self, CS, lead):
v_ego = CS.vEgo
# Setup current mpc state
self.cur_state[0].x_ego = 0.0
if lead is not None and lead.status:
x_lead = lead.dRel
v_lead = max(0.0, lead.vLead)
a_lead = lead.aLeadK
if (v_lead < 0.1 or -a_lead / 2.0 > v_lead):
v_lead = 0.0
a_lead = 0.0
self.a_lead_tau = lead.aLeadTau
self.new_lead = False
if not self.prev_lead_status or abs(x_lead -
self.prev_lead_x) > 2.5:
self.libmpc.init_with_simulation(self.v_mpc, x_lead, v_lead,
a_lead, self.a_lead_tau)
self.new_lead = True
self.prev_lead_status = True
self.prev_lead_x = x_lead
self.cur_state[0].x_l = x_lead
self.cur_state[0].v_l = v_lead
else:
self.prev_lead_status = False
# Fake a fast lead car, so mpc keeps running
self.cur_state[0].x_l = 50.0
self.cur_state[0].v_l = v_ego + 10.0
a_lead = 0.0
self.a_lead_tau = _LEAD_ACCEL_TAU
# Calculate mpc
t = sec_since_boot()
# scc smoother
cruise_gap = int(clip(CS.cruiseGap, 1., 4.))
TR = interp(float(cruise_gap), [1., 2., 3., 4.], [1.2, 1.5, 1.8, 2.2])
#TR = interp(v_ego, [3., 30.], [1., 2.5])
if self.cruise_gap != cruise_gap:
self.cruise_gap = cruise_gap
self.libmpc.init(MPC_COST_LONG.TTC, MPC_COST_LONG.DISTANCE,
MPC_COST_LONG.ACCELERATION, MPC_COST_LONG.JERK)
self.cur_state[0].v_ego = v_ego
self.cur_state[0].a_ego = 0.0
self.v_mpc = v_ego
self.a_mpc = CS.aEgo
self.prev_lead_status = False
return
self.n_its = self.libmpc.run_mpc(self.cur_state, self.mpc_solution,
self.a_lead_tau, a_lead, TR)
self.duration = int((sec_since_boot() - t) * 1e9)
# Get solution. MPC timestep is 0.2 s, so interpolation to 0.05 s is needed
self.v_mpc = self.mpc_solution[0].v_ego[1]
self.a_mpc = self.mpc_solution[0].a_ego[1]
self.v_mpc_future = self.mpc_solution[0].v_ego[10]
# Reset if NaN or goes through lead car
crashing = any(lead - ego < -50 for (
lead,
ego) in zip(self.mpc_solution[0].x_l, self.mpc_solution[0].x_ego))
nans = any(math.isnan(x) for x in self.mpc_solution[0].v_ego)
backwards = min(self.mpc_solution[0].v_ego) < -0.01
if ((backwards or crashing) and self.prev_lead_status) or nans:
if t > self.last_cloudlog_t + 5.0:
self.last_cloudlog_t = t
cloudlog.warning(
"Longitudinal mpc %d reset - backwards: %s crashing: %s nan: %s"
% (self.mpc_id, backwards, crashing, nans))
self.libmpc.init(MPC_COST_LONG.TTC, MPC_COST_LONG.DISTANCE,
MPC_COST_LONG.ACCELERATION, MPC_COST_LONG.JERK)
self.cur_state[0].v_ego = v_ego
self.cur_state[0].a_ego = 0.0
self.v_mpc = v_ego
self.a_mpc = CS.aEgo
self.prev_lead_status = False
def k_i(self): return interp(self.speed, self._k_i[0], self._k_i[1])
def update(self, pm, CS, lead, v_cruise_setpoint):
v_ego = CS.vEgo
# Setup current mpc state
self.cur_state[0].x_ego = 0.0
if lead is not None and lead.status:
x_lead = max(0, lead.dRel - STOPPING_DISTANCE) # increase stopping distance to car by X [m]
v_lead = max(0.0, lead.vLead)
a_lead = lead.aLeadK
if (v_lead < 0.1 or -a_lead / 2.0 > v_lead):
v_lead = 0.0
a_lead = 0.0
self.a_lead_tau = max(lead.aLeadTau, (a_lead ** 2 * math.pi) / (2 * (v_lead + 0.01) ** 2))
self.new_lead = False
if not self.prev_lead_status or abs(x_lead - self.prev_lead_x) > 2.5:
self.libmpc.init_with_simulation(self.v_mpc, x_lead, v_lead, a_lead, self.a_lead_tau)
self.new_lead = True
self.prev_lead_status = True
self.prev_lead_x = x_lead
self.cur_state[0].x_l = x_lead
self.cur_state[0].v_l = v_lead
else:
self.prev_lead_status = False
# Fake a fast lead car, so mpc keeps running
self.cur_state[0].x_l = 50.0
self.cur_state[0].v_l = v_ego + 10.0
a_lead = 0.0
v_lead = 0.0
self.a_lead_tau = _LEAD_ACCEL_TAU
# Calculate conditions
self.v_rel = v_lead - v_ego # calculate relative velocity vs lead car
# Is the car running surface street speeds?
if v_ego < CITY_SPEED:
self.street_speed = 1
else:
self.street_speed = 0
# Live Tuning of breakpoints for braking profile change
self.bp_counter += 1
if self.bp_counter % 500 == 0:
kegman = kegman_conf()
self.oneBarBP = [float(kegman.conf['1barBP0']), float(kegman.conf['1barBP1'])]
self.twoBarBP = [float(kegman.conf['2barBP0']), float(kegman.conf['2barBP1'])]
self.threeBarBP = [float(kegman.conf['3barBP0']), float(kegman.conf['3barBP1'])]
self.oneBarProfile = [ONE_BAR_DISTANCE, float(kegman.conf['1barMax'])]
self.twoBarProfile = [TWO_BAR_DISTANCE, float(kegman.conf['2barMax'])]
self.threeBarProfile = [THREE_BAR_DISTANCE, float(kegman.conf['3barMax'])]
self.oneBarHwy = [ONE_BAR_DISTANCE, ONE_BAR_DISTANCE+float(kegman.conf['1barHwy'])]
self.twoBarHwy = [TWO_BAR_DISTANCE, TWO_BAR_DISTANCE+float(kegman.conf['2barHwy'])]
self.threeBarHwy = [THREE_BAR_DISTANCE, THREE_BAR_DISTANCE+float(kegman.conf['3barHwy'])]
self.bp_counter = 0
# Calculate mpc
# Adjust distance from lead car when distance button pressed
if CS.readdistancelines == 1:
#if self.street_speed and (self.lead_car_gap_shrinking or self.tailgating):
if self.street_speed:
TR = interp(-self.v_rel, self.oneBarBP, self.oneBarProfile)
else:
TR = interp(-self.v_rel, H_ONE_BAR_PROFILE_BP, self.oneBarHwy)
if CS.readdistancelines != self.lastTR:
self.libmpc.init(MPC_COST_LONG.TTC, 1.0, MPC_COST_LONG.ACCELERATION, MPC_COST_LONG.JERK)
self.lastTR = CS.readdistancelines
elif CS.readdistancelines == 2:
#if self.street_speed and (self.lead_car_gap_shrinking or self.tailgating):
if self.street_speed:
TR = interp(-self.v_rel, self.twoBarBP, self.twoBarProfile)
else:
TR = interp(-self.v_rel, H_TWO_BAR_PROFILE_BP, self.twoBarHwy)
if CS.readdistancelines != self.lastTR:
self.libmpc.init(MPC_COST_LONG.TTC, MPC_COST_LONG.DISTANCE, MPC_COST_LONG.ACCELERATION, MPC_COST_LONG.JERK)
self.lastTR = CS.readdistancelines
elif CS.readdistancelines == 3:
if self.street_speed:
#if self.street_speed and (self.lead_car_gap_shrinking or self.tailgating):
TR = interp(-self.v_rel, self.threeBarBP, self.threeBarProfile)
else:
TR = interp(-self.v_rel, H_THREE_BAR_PROFILE_BP, self.threeBarHwy)
if CS.readdistancelines != self.lastTR:
self.libmpc.init(MPC_COST_LONG.TTC, MPC_COST_LONG.DISTANCE, MPC_COST_LONG.ACCELERATION, MPC_COST_LONG.JERK)
self.lastTR = CS.readdistancelines
elif CS.readdistancelines == 4:
TR = FOUR_BAR_DISTANCE
if CS.readdistancelines != self.lastTR:
self.libmpc.init(MPC_COST_LONG.TTC, MPC_COST_LONG.DISTANCE, MPC_COST_LONG.ACCELERATION, MPC_COST_LONG.JERK)
self.lastTR = CS.readdistancelines
else:
TR = TWO_BAR_DISTANCE # if readdistancelines != 1,2,3,4
self.libmpc.init(MPC_COST_LONG.TTC, MPC_COST_LONG.DISTANCE, MPC_COST_LONG.ACCELERATION, MPC_COST_LONG.JERK)
t = sec_since_boot()
n_its = self.libmpc.run_mpc(self.cur_state, self.mpc_solution, self.a_lead_tau, a_lead, TR)
duration = int((sec_since_boot() - t) * 1e9)
if LOG_MPC:
self.send_mpc_solution(pm, n_its, duration)
# Get solution. MPC timestep is 0.2 s, so interpolation to 0.05 s is needed
self.v_mpc = self.mpc_solution[0].v_ego[1]
self.a_mpc = self.mpc_solution[0].a_ego[1]
self.v_mpc_future = self.mpc_solution[0].v_ego[10]
# Reset if NaN or goes through lead car
crashing = any(lead - ego < -50 for (lead, ego) in zip(self.mpc_solution[0].x_l, self.mpc_solution[0].x_ego))
nans = any(math.isnan(x) for x in self.mpc_solution[0].v_ego)
backwards = min(self.mpc_solution[0].v_ego) < -0.01
if ((backwards or crashing) and self.prev_lead_status) or nans:
if t > self.last_cloudlog_t + 5.0:
self.last_cloudlog_t = t
cloudlog.warning("Longitudinal mpc %d reset - backwards: %s crashing: %s nan: %s" % (
self.mpc_id, backwards, crashing, nans))
self.libmpc.init(MPC_COST_LONG.TTC, MPC_COST_LONG.DISTANCE,
MPC_COST_LONG.ACCELERATION, MPC_COST_LONG.JERK)
self.cur_state[0].v_ego = v_ego
self.cur_state[0].a_ego = 0.0
self.v_mpc = v_ego
self.a_mpc = CS.aEgo
self.prev_lead_status = False
def update(self, cp, cp_cam, cp_body):
ret = car.CarState.new_message()
# car params
v_weight_v = [
0., 1.
] # don't trust smooth speed at low values to avoid premature zero snapping
v_weight_bp = [
1., 6.
] # smooth blending, below ~0.6m/s the smooth speed snaps to zero
# update prevs, update must run once per loop
self.prev_cruise_buttons = self.cruise_buttons
self.prev_lead_distance = self.lead_distance
self.prev_cruise_setting = self.cruise_setting
# ******************* parse out can *******************
# TODO: find wheels moving bit in dbc
if self.CP.carFingerprint in (CAR.ACCORD, CAR.ACCORD_15, CAR.ACCORDH,
CAR.CIVIC_BOSCH, CAR.CIVIC_BOSCH_DIESEL,
CAR.CRV_HYBRID, CAR.INSIGHT,
CAR.ACURA_RDX_3G):
ret.standstill = cp.vl["ENGINE_DATA"]['XMISSION_SPEED'] < 0.1
ret.doorOpen = bool(cp.vl["SCM_FEEDBACK"]['DRIVERS_DOOR_OPEN'])
elif self.CP.carFingerprint == CAR.ODYSSEY_CHN:
ret.standstill = cp.vl["ENGINE_DATA"]['XMISSION_SPEED'] < 0.1
ret.doorOpen = bool(cp.vl["SCM_BUTTONS"]['DRIVERS_DOOR_OPEN'])
elif self.CP.carFingerprint == CAR.HRV:
ret.doorOpen = bool(cp.vl["SCM_BUTTONS"]['DRIVERS_DOOR_OPEN'])
else:
ret.standstill = not cp.vl["STANDSTILL"]['WHEELS_MOVING']
ret.doorOpen = any([
cp.vl["DOORS_STATUS"]['DOOR_OPEN_FL'],
cp.vl["DOORS_STATUS"]['DOOR_OPEN_FR'],
cp.vl["DOORS_STATUS"]['DOOR_OPEN_RL'],
cp.vl["DOORS_STATUS"]['DOOR_OPEN_RR']
])
ret.seatbeltUnlatched = bool(
cp.vl["SEATBELT_STATUS"]['SEATBELT_DRIVER_LAMP']
or not cp.vl["SEATBELT_STATUS"]['SEATBELT_DRIVER_LATCHED'])
steer_status = self.steer_status_values[cp.vl["STEER_STATUS"]
['STEER_STATUS']]
ret.steerError = steer_status not in [
'NORMAL', 'NO_TORQUE_ALERT_1', 'NO_TORQUE_ALERT_2',
'LOW_SPEED_LOCKOUT', 'TMP_FAULT'
]
# NO_TORQUE_ALERT_2 can be caused by bump OR steering nudge from driver
self.steer_not_allowed = steer_status not in [
'NORMAL', 'NO_TORQUE_ALERT_2'
]
# LOW_SPEED_LOCKOUT is not worth a warning
ret.steerWarning = steer_status not in [
'NORMAL', 'LOW_SPEED_LOCKOUT', 'NO_TORQUE_ALERT_2'
]
if not self.CP.openpilotLongitudinalControl:
self.brake_error = 0
else:
self.brake_error = cp.vl["STANDSTILL"]['BRAKE_ERROR_1'] or cp.vl[
"STANDSTILL"]['BRAKE_ERROR_2']
ret.espDisabled = cp.vl["VSA_STATUS"]['ESP_DISABLED'] != 0
speed_factor = SPEED_FACTOR[self.CP.carFingerprint]
ret.wheelSpeeds.fl = cp.vl["WHEEL_SPEEDS"][
'WHEEL_SPEED_FL'] * CV.KPH_TO_MS * speed_factor
ret.wheelSpeeds.fr = cp.vl["WHEEL_SPEEDS"][
'WHEEL_SPEED_FR'] * CV.KPH_TO_MS * speed_factor
ret.wheelSpeeds.rl = cp.vl["WHEEL_SPEEDS"][
'WHEEL_SPEED_RL'] * CV.KPH_TO_MS * speed_factor
ret.wheelSpeeds.rr = cp.vl["WHEEL_SPEEDS"][
'WHEEL_SPEED_RR'] * CV.KPH_TO_MS * speed_factor
v_wheel = (ret.wheelSpeeds.fl + ret.wheelSpeeds.fr +
ret.wheelSpeeds.rl + ret.wheelSpeeds.rr) / 4.
# blend in transmission speed at low speed, since it has more low speed accuracy
v_weight = interp(v_wheel, v_weight_bp, v_weight_v)
ret.vEgoRaw = (1. - v_weight) * cp.vl["ENGINE_DATA"][
'XMISSION_SPEED'] * CV.KPH_TO_MS * speed_factor + v_weight * v_wheel
ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw)
ret.steeringAngleDeg = cp.vl["STEERING_SENSORS"]['STEER_ANGLE']
ret.steeringRateDeg = cp.vl["STEERING_SENSORS"]['STEER_ANGLE_RATE']
#self.cruise_setting = cp.vl["SCM_BUTTONS"]['CRUISE_SETTING']
self.cruise_buttons = cp.vl["SCM_BUTTONS"]['CRUISE_BUTTONS']
ret.leftBlinker = cp.vl["SCM_FEEDBACK"]['LEFT_BLINKER'] != 0
ret.rightBlinker = cp.vl["SCM_FEEDBACK"]['RIGHT_BLINKER'] != 0
self.brake_hold = cp.vl["VSA_STATUS"]['BRAKE_HOLD_ACTIVE']
self.engineRPM = cp.vl["POWERTRAIN_DATA"]['ENGINE_RPM']
if self.CP.carFingerprint in (CAR.CIVIC, CAR.ODYSSEY, CAR.CRV_5G,
CAR.ACCORD, CAR.ACCORD_15, CAR.ACCORDH,
CAR.CIVIC_BOSCH, CAR.CIVIC_BOSCH_DIESEL,
CAR.CRV_HYBRID, CAR.INSIGHT,
CAR.ACURA_RDX_3G):
self.park_brake = cp.vl["EPB_STATUS"]['EPB_STATE'] != 0
main_on = cp.vl["SCM_FEEDBACK"]['MAIN_ON']
elif self.CP.carFingerprint == CAR.ODYSSEY_CHN:
self.park_brake = cp.vl["EPB_STATUS"]['EPB_STATE'] != 0
main_on = cp.vl["SCM_BUTTONS"]['MAIN_ON']
else:
self.park_brake = 0 # TODO
main_on = cp.vl["SCM_BUTTONS"]['MAIN_ON']
gear = int(cp.vl["GEARBOX"]['GEAR_SHIFTER'])
ret.gearShifter = self.parse_gear_shifter(
self.shifter_values.get(gear, None))
self.pedal_gas = cp.vl["POWERTRAIN_DATA"]['PEDAL_GAS']
# crv doesn't include cruise control
if self.CP.carFingerprint in (CAR.CRV, CAR.CRV_EU, CAR.HRV,
CAR.ODYSSEY, CAR.ACURA_RDX,
CAR.RIDGELINE, CAR.PILOT_2019,
CAR.ODYSSEY_CHN):
ret.gas = self.pedal_gas / 256.
else:
ret.gas = cp.vl["GAS_PEDAL_2"]['CAR_GAS'] / 256.
# this is a hack for the interceptor. This is now only used in the simulation
# TODO: Replace tests by toyota so this can go away
if self.CP.enableGasInterceptor:
self.user_gas = (cp.vl["GAS_SENSOR"]['INTERCEPTOR_GAS'] +
cp.vl["GAS_SENSOR"]['INTERCEPTOR_GAS2']) / 2.
self.user_gas_pressed = self.user_gas > 1e-5 # this works because interceptor read < 0 when pedal position is 0. Once calibrated, this will change
ret.gasPressed = self.user_gas_pressed
else:
ret.gasPressed = self.pedal_gas > 1e-5
ret.steeringTorque = cp.vl["STEER_STATUS"]['STEER_TORQUE_SENSOR']
ret.steeringTorqueEps = cp.vl["STEER_MOTOR_TORQUE"]['MOTOR_TORQUE']
ret.steeringPressed = abs(
ret.steeringTorque) > STEER_THRESHOLD[self.CP.carFingerprint]
self.brake_switch = cp.vl["POWERTRAIN_DATA"]['BRAKE_SWITCH'] != 0
if self.CP.carFingerprint in HONDA_BOSCH:
self.cruise_mode = cp.vl["ACC_HUD"]['CRUISE_CONTROL_LABEL']
ret.cruiseState.standstill = cp.vl["ACC_HUD"][
'CRUISE_SPEED'] == 252.
ret.cruiseState.speedOffset = calc_cruise_offset(0, ret.vEgo)
if self.CP.carFingerprint in (CAR.CIVIC_BOSCH,
CAR.CIVIC_BOSCH_DIESEL, CAR.ACCORDH,
CAR.CRV_HYBRID, CAR.INSIGHT):
ret.brakePressed = cp.vl["POWERTRAIN_DATA"]['BRAKE_PRESSED'] != 0 or \
(self.brake_switch and self.brake_switch_prev and
cp.ts["POWERTRAIN_DATA"]['BRAKE_SWITCH'] != self.brake_switch_ts)
self.brake_switch_prev = self.brake_switch
self.brake_switch_ts = cp.ts["POWERTRAIN_DATA"]['BRAKE_SWITCH']
if self.CP.carFingerprint in (CAR.CIVIC_BOSCH):
self.hud_lead = cp.vl["ACC_HUD"]['HUD_LEAD']
else:
ret.brakePressed = cp.vl["BRAKE_MODULE"]['BRAKE_PRESSED'] != 0
# On set, cruise set speed pulses between 254~255 and the set speed prev is set to avoid this.
ret.cruiseState.speed = self.v_cruise_pcm_prev if cp.vl["ACC_HUD"][
'CRUISE_SPEED'] > 160.0 else cp.vl["ACC_HUD"][
'CRUISE_SPEED'] * CV.KPH_TO_MS
self.v_cruise_pcm_prev = ret.cruiseState.speed
else:
ret.cruiseState.speedOffset = calc_cruise_offset(
cp.vl["CRUISE_PARAMS"]['CRUISE_SPEED_OFFSET'], ret.vEgo)
ret.cruiseState.speed = cp.vl["CRUISE"][
'CRUISE_SPEED_PCM'] * CV.KPH_TO_MS
# brake switch has shown some single time step noise, so only considered when
# switch is on for at least 2 consecutive CAN samples
ret.brakePressed = bool(
cp.vl["POWERTRAIN_DATA"]['BRAKE_PRESSED']
or (self.brake_switch and self.brake_switch_prev
and cp.ts["POWERTRAIN_DATA"]['BRAKE_SWITCH'] !=
self.brake_switch_ts))
self.brake_switch_prev = self.brake_switch
self.brake_switch_ts = cp.ts["POWERTRAIN_DATA"]['BRAKE_SWITCH']
ret.brake = cp.vl["VSA_STATUS"]['USER_BRAKE']
ret.cruiseState.enabled = cp.vl["POWERTRAIN_DATA"]['ACC_STATUS'] != 0
# when user presses distance button on steering wheel
if self.cruise_setting == 3:
if cp.vl["SCM_BUTTONS"]["CRUISE_SETTING"] == 0:
self.trMode = (self.trMode + 1) % 4
self.kegman = kegman_conf()
self.kegman.conf['lastTrMode'] = str(
self.trMode) # write last distance bar setting to file
self.kegman.write_config(self.kegman.conf)
# when user presses LKAS button on steering wheel
if self.cruise_setting == 1:
if cp.vl["SCM_BUTTONS"]["CRUISE_SETTING"] == 0:
if self.lkMode:
self.lkMode = False
else:
self.lkMode = True
self.prev_cruise_setting = self.cruise_setting
self.cruise_setting = cp.vl["SCM_BUTTONS"]['CRUISE_SETTING']
self.read_distance_lines = self.trMode + 1
if self.read_distance_lines != self.read_distance_lines_prev:
self.read_distance_lines_prev = self.read_distance_lines
ret.cruiseState.available = bool(main_on)
ret.cruiseState.nonAdaptive = self.cruise_mode != 0
if self.CP.carFingerprint in (CAR.PILOT, CAR.PILOT_2019,
CAR.RIDGELINE):
if ret.brake > 0.05:
ret.brakePressed = True
elif self.CP.carFingerprint in (CAR.ODYSSEY, CAR.ODYSSEY_CHN):
if ret.brake > 0.1:
ret.brakePressed = True
# TODO: discover the CAN msg that has the imperial unit bit for all other cars
self.is_metric = not cp.vl["HUD_SETTING"][
'IMPERIAL_UNIT'] if self.CP.carFingerprint in (
CAR.CIVIC) else False
if self.CP.carFingerprint in HONDA_BOSCH:
ret.stockAeb = bool(
cp_cam.vl["ACC_CONTROL"]["AEB_STATUS"]
and cp_cam.vl["ACC_CONTROL"]["ACCEL_COMMAND"] < -1e-5)
else:
ret.stockAeb = bool(
cp_cam.vl["BRAKE_COMMAND"]["AEB_REQ_1"]
and cp_cam.vl["BRAKE_COMMAND"]["COMPUTER_BRAKE"] > 1e-5)
if self.CP.carFingerprint in HONDA_BOSCH:
self.stock_hud = False
ret.stockFcw = False
else:
#ret.stockFcw = cp_cam.vl["BRAKE_COMMAND"]["FCW"] != 0
ret.stockFcw = False
self.stock_hud = cp_cam.vl["ACC_HUD"]
self.stock_brake = cp_cam.vl["BRAKE_COMMAND"]
if self.CP.carFingerprint in (CAR.CRV_5G, ):
# BSM messages are on B-CAN, requires a panda forwarding B-CAN messages to CAN 0
# more info here: https://github.com/commaai/openpilot/pull/1867
ret.leftBlindspot = cp_body.vl["BSM_STATUS_LEFT"]['BSM_ALERT'] == 1
ret.rightBlindspot = cp_body.vl["BSM_STATUS_RIGHT"][
'BSM_ALERT'] == 1
return ret
def update(self, sm, pm, CP, VM):
self.atom_timer_cnt += 1
if self.atom_timer_cnt > 1000:
self.atom_timer_cnt = 0
cruiseState = sm['carState'].cruiseState
leftBlindspot = sm['carState'].leftBlindspot
rightBlindspot = sm['carState'].rightBlindspot
lateralsRatom = CP.lateralsRatom
atomTuning = CP.atomTuning
#if atomTuning is None or lateralsRatom is None:
#print('carparams={} steerRatio={} carParams_valid={}'.format(sm.updated['carParams'], sm['carParams'].steerRatio, self.carParams_valid ) )
if not self.carParams_valid and sm[
'carParams'].steerRatio: # sm.updated['carParams']:
self.carParams_valid = True
if self.carParams_valid:
lateralsRatom = sm['carParams'].lateralsRatom
atomTuning = sm['carParams'].atomTuning
v_ego = sm['carState'].vEgo
angle_steers = sm['carState'].steeringAngle
steeringPressed = sm['carState'].steeringPressed
steeringTorque = sm['carState'].steeringTorque
active = sm['controlsState'].active
model_sum = sm['controlsState'].modelSum
v_ego_kph = v_ego * CV.MS_TO_KPH
self.steerRatio = sm['liveParameters'].steerRatio
angle_offset = sm['liveParameters'].angleOffset
angleOffsetAverage = sm['liveParameters'].angleOffsetAverage
stiffnessFactor = sm['liveParameters'].stiffnessFactor
# if (self.atom_timer_cnt % 100) == 0:
# str_log3 = 'angleOffset={:.1f} angleOffsetAverage={:.3f} steerRatio={:.2f} stiffnessFactor={:.3f} '.format( angle_offset, angleOffsetAverage, self.steerRatio, stiffnessFactor )
# self.trLearner.add( 'LearnerParam {} carParams={}'.format( str_log3, self.carParams_valid ) )
if lateralsRatom.learnerParams:
pass
else:
# atom
if self.carParams_valid:
self.steer_rate_cost = sm['carParams'].steerRateCost
self.steerRatio = sm['carParams'].steerRatio
else:
self.steer_rate_cost = CP.steerRateCost
self.steerRatio = CP.steerRatio
#xp = [-5,0,5]
#fp = [0.4, 0.7, 0.4]
#self.steer_rate_cost = interp( angle_steers, xp, fp )
steerRatio = self.atom_tune(v_ego_kph, angle_steers, atomTuning)
self.steerRatio = self.atom_steer(steerRatio, 2, 1)
#actuatorDelay = CP.steerActuatorDelay
steerActuatorDelay = self.atom_actuatorDelay(v_ego_kph, angle_steers,
atomTuning)
# Run MPC
self.angle_steers_des_prev = self.angle_steers_des_mpc
VM.update_params(stiffnessFactor, self.steerRatio)
curvature_factor = VM.curvature_factor(v_ego)
self.LP.parse_model(sm['model'])
# Lane change logic
one_blinker = sm['carState'].leftBlinker != sm['carState'].rightBlinker
below_lane_change_speed = v_ego < LANE_CHANGE_SPEED_MIN
if sm['carState'].leftBlinker:
self.lane_change_direction = LaneChangeDirection.left
elif sm['carState'].rightBlinker:
self.lane_change_direction = LaneChangeDirection.right
if (not active) or (self.lane_change_run_timer >
LANE_CHANGE_TIME_MAX) or (not one_blinker) or (
not self.lane_change_enabled):
self.lane_change_state = LaneChangeState.off
self.lane_change_direction = LaneChangeDirection.none
else:
l_poly = self.LP.l_poly[3]
r_poly = self.LP.r_poly[3]
c_prob = l_poly + r_poly
torque_applied = steeringPressed and \
((steeringTorque > 0 and self.lane_change_direction == LaneChangeDirection.left) or \
(steeringTorque < 0 and self.lane_change_direction == LaneChangeDirection.right))
blindspot_detected = (
(leftBlindspot
and self.lane_change_direction == LaneChangeDirection.left) or
(rightBlindspot
and self.lane_change_direction == LaneChangeDirection.right))
lane_change_prob = self.LP.l_lane_change_prob + self.LP.r_lane_change_prob
# State transitions
# off
if cruiseState.cruiseSwState == Buttons.CANCEL:
self.lane_change_state = LaneChangeState.off
self.lane_change_ll_prob = 1.0
self.lane_change_wait_timer = 0
elif self.lane_change_state == LaneChangeState.off and one_blinker and not self.prev_one_blinker and not below_lane_change_speed:
self.lane_change_state = LaneChangeState.preLaneChange
self.lane_change_ll_prob = 1.0
self.lane_change_wait_timer = 0
# pre
elif self.lane_change_state == LaneChangeState.preLaneChange:
self.lane_change_wait_timer += DT_MDL
if not one_blinker or below_lane_change_speed:
self.lane_change_state = LaneChangeState.off
elif not blindspot_detected and (
torque_applied or (self.lane_change_auto_delay
and self.lane_change_wait_timer >
self.lane_change_auto_delay)):
self.lane_change_state = LaneChangeState.laneChangeStarting
# starting
elif self.lane_change_state == LaneChangeState.laneChangeStarting:
# fade out over .5s
xp = [40, 80]
fp2 = [1, 2]
lane_time = interp(v_ego_kph, xp, fp2)
self.lane_change_ll_prob = max(
self.lane_change_ll_prob - lane_time * DT_MDL, 0.0)
# 98% certainty
if lane_change_prob < 0.02 and self.lane_change_ll_prob < 0.01:
self.lane_change_state = LaneChangeState.laneChangeFinishing
# finishing
elif self.lane_change_state == LaneChangeState.laneChangeFinishing:
# fade in laneline over 1s
self.lane_change_ll_prob = min(
self.lane_change_ll_prob + DT_MDL, 1.0)
if self.lane_change_ll_prob > 0.6 and abs(c_prob) < 0.3:
self.lane_change_state = LaneChangeState.laneChangeDone
# done
elif self.lane_change_state == LaneChangeState.laneChangeDone:
if not one_blinker:
self.lane_change_state = LaneChangeState.off
if self.lane_change_state in [
LaneChangeState.off, LaneChangeState.preLaneChange
]:
self.lane_change_run_timer = 0.0
else:
self.lane_change_run_timer += DT_MDL
self.prev_one_blinker = one_blinker
desire = DESIRES[self.lane_change_direction][self.lane_change_state]
# Turn off lanes during lane change
if desire == log.PathPlan.Desire.laneChangeRight or desire == log.PathPlan.Desire.laneChangeLeft:
self.LP.l_prob *= self.lane_change_ll_prob
self.LP.r_prob *= self.lane_change_ll_prob
self.LP.update_d_poly(v_ego, lateralsRatom.cameraOffset)
# account for actuation delay
self.cur_state = calc_states_after_delay(self.cur_state, v_ego,
angle_steers - angle_offset,
curvature_factor, VM.sR,
steerActuatorDelay)
v_ego_mpc = max(v_ego, 5.0) # avoid mpc roughness due to low speed
self.libmpc.run_mpc(self.cur_state, self.mpc_solution,
list(self.LP.l_poly), list(self.LP.r_poly),
list(self.LP.d_poly), self.LP.l_prob,
self.LP.r_prob, curvature_factor, v_ego_mpc,
self.LP.lane_width)
# reset to current steer angle if not active or overriding
if active:
delta_desired = self.mpc_solution[0].delta[1]
rate_desired = math.degrees(self.mpc_solution[0].rate[0] * VM.sR)
else:
delta_desired = math.radians(angle_steers - angle_offset) / VM.sR
rate_desired = 0.0
self.cur_state[0].delta = delta_desired
self.angle_steers_des_mpc = float(
math.degrees(delta_desired * VM.sR) + angle_offset)
org_angle_steers_des = self.angle_steers_des_mpc
# atom
if self.lane_change_state == LaneChangeState.laneChangeStarting:
xp = [40, 70]
fp2 = [3, 10]
limit_steers = interp(v_ego_kph, xp, fp2)
self.angle_steers_des_mpc = self.limit_ctrl(
org_angle_steers_des, limit_steers, angle_steers)
elif steeringPressed:
delta_steer = org_angle_steers_des - angle_steers
if angle_steers > 10 and steeringTorque > 0:
delta_steer = max(delta_steer, 0)
delta_steer = min(delta_steer, DST_ANGLE_LIMIT)
self.angle_steers_des_mpc = angle_steers + dleta_steer
elif angle_steers < -10 and steeringTorque < 0:
delta_steer = min(delta_steer, 0)
delta_steer = max(delta_steer, -DST_ANGLE_LIMIT)
self.angle_steers_des_mpc = angle_steers + delta_steer
else:
if steeringTorque < 0: # right
if delta_steer > 0:
self.angle_steers_des_mpc = self.limit_ctrl(
org_angle_steers_des, DST_ANGLE_LIMIT,
angle_steers)
elif steeringTorque > 0: # left
if delta_steer < 0:
self.angle_steers_des_mpc = self.limit_ctrl(
org_angle_steers_des, DST_ANGLE_LIMIT,
angle_steers)
elif v_ego_kph < 15: # 30
xp = [3, 10, 15]
fp2 = [3, 5, 7]
limit_steers = interp(v_ego_kph, xp, fp2)
self.angle_steers_des_mpc = self.limit_ctrl(
org_angle_steers_des, limit_steers, angle_steers)
elif v_ego_kph > 60:
pass
elif abs(angle_steers) > 50: # angle steer > 50
# 2.
xp = [-10, -5, 0, 5, 10] # 5 10=>28 15=>35, 30=>52
fp1 = [3, 8, 10, 20, 10] # +
fp2 = [10, 20, 10, 8, 3] # -
limit_steers1 = interp(model_sum, xp, fp1) # +
limit_steers2 = interp(model_sum, xp, fp2) # -
self.angle_steers_des_mpc = self.limit_ctrl1(
org_angle_steers_des, limit_steers1, limit_steers2,
angle_steers)
# 최대 허용 제어 조향각.
delta_steer = self.angle_steers_des_mpc - angle_steers
if delta_steer > DST_ANGLE_LIMIT:
p_angle_steers = angle_steers + DST_ANGLE_LIMIT
self.angle_steers_des_mpc = p_angle_steers
elif delta_steer < -DST_ANGLE_LIMIT:
m_angle_steers = angle_steers - DST_ANGLE_LIMIT
self.angle_steers_des_mpc = m_angle_steers
# Check for infeasable MPC solution
mpc_nans = any(math.isnan(x) for x in self.mpc_solution[0].delta)
t = sec_since_boot()
if mpc_nans:
self.libmpc.init(MPC_COST_LAT.PATH, MPC_COST_LAT.LANE,
MPC_COST_LAT.HEADING, self.steer_rate_cost)
self.cur_state[0].delta = math.radians(angle_steers -
angle_offset) / VM.sR
if t > self.last_cloudlog_t + 5.0:
self.last_cloudlog_t = t
cloudlog.warning("Lateral mpc - nan: True")
#self.trPATH.add( 'mpc_nans ={} libmpc steer_rate_cost={} delta={} angle_steers={}'.format( mpc_nans, self.steer_rate_cost, self.cur_state[0].delta, angle_steers ) )
if self.mpc_solution[
0].cost > 20000. or mpc_nans: # TODO: find a better way to detect when MPC did not converge
self.solution_invalid_cnt += 1
else:
self.solution_invalid_cnt = 0
plan_solution_valid = self.solution_invalid_cnt < 3
plan_send = messaging.new_message('pathPlan')
plan_send.valid = sm.all_alive_and_valid(service_list=[
'carState', 'controlsState', 'liveParameters', 'model'
])
plan_send.pathPlan.laneWidth = float(self.LP.lane_width)
plan_send.pathPlan.dPoly = [float(x) for x in self.LP.d_poly]
plan_send.pathPlan.lPoly = [float(x) for x in self.LP.l_poly]
plan_send.pathPlan.lProb = float(self.LP.l_prob)
plan_send.pathPlan.rPoly = [float(x) for x in self.LP.r_poly]
plan_send.pathPlan.rProb = float(self.LP.r_prob)
plan_send.pathPlan.angleSteers = float(self.angle_steers_des_mpc)
plan_send.pathPlan.rateSteers = float(rate_desired)
plan_send.pathPlan.angleOffset = float(angleOffsetAverage)
plan_send.pathPlan.mpcSolutionValid = bool(plan_solution_valid)
plan_send.pathPlan.paramsValid = bool(sm['liveParameters'].valid)
plan_send.pathPlan.desire = desire
plan_send.pathPlan.laneChangeState = self.lane_change_state
plan_send.pathPlan.laneChangeDirection = self.lane_change_direction
plan_send.pathPlan.steerRatio = self.steerRatio
plan_send.pathPlan.steerActuatorDelay = steerActuatorDelay
pm.send('pathPlan', plan_send)
# if self.solution_invalid_cnt > 0:
# str_log3 = 'v_ego_kph={:.1f} angle_steers_des_mpc={:.1f} angle_steers={:.1f} solution_invalid_cnt={:.0f} mpc_solution={:.1f}/{:.0f}'.format( v_ego_kph, self.angle_steers_des_mpc, angle_steers, self.solution_invalid_cnt, self.mpc_solution[0].cost, mpc_nans )
# self.trpathPlan.add( 'pathPlan {} LOG_MPC={}'.format( str_log3, LOG_MPC ) )
if LOG_MPC:
dat = messaging.new_message('liveMpc')
dat.liveMpc.x = list(self.mpc_solution[0].x)
dat.liveMpc.y = list(self.mpc_solution[0].y)
dat.liveMpc.psi = list(self.mpc_solution[0].psi)
dat.liveMpc.delta = list(self.mpc_solution[0].delta)
dat.liveMpc.cost = self.mpc_solution[0].cost
pm.send('liveMpc', dat)
def RC(self):
return interp(self.speed, self._RC[0], self._RC[1])
def update(self, sendcan, enabled, CS, frame, actuators, \
hud_v_cruise, hud_show_lanes, hud_show_car, chime, chime_cnt):
""" Controls thread """
# Sanity check.
if not self.allow_controls:
return
P = self.params
# Send CAN commands.
can_sends = []
canbus = self.canbus
### STEER ###
if (frame % P.STEER_STEP) == 0:
lkas_enabled = enabled and not CS.steer_not_allowed and CS.v_ego > 3.
if lkas_enabled:
apply_steer = actuators.steer * P.STEER_MAX
apply_steer = apply_std_steer_torque_limits(apply_steer, self.apply_steer_last, CS.steer_torque_driver, P)
else:
apply_steer = 0
self.apply_steer_last = apply_steer
idx = (frame / P.STEER_STEP) % 4
if self.car_fingerprint == CAR.VOLT:
can_sends.append(gmcan.create_steering_control(self.packer_pt,
canbus.powertrain, apply_steer, idx, lkas_enabled))
if self.car_fingerprint == CAR.CADILLAC_CT6:
can_sends += gmcan.create_steering_control_ct6(self.packer_pt,
canbus, apply_steer, CS.v_ego, idx, lkas_enabled)
### GAS/BRAKE ###
if self.car_fingerprint == CAR.VOLT:
# no output if not enabled, but keep sending keepalive messages
# treat pedals as one
final_pedal = actuators.gas - actuators.brake
# *** apply pedal hysteresis ***
final_brake, self.brake_steady = actuator_hystereses(
final_pedal, self.pedal_steady)
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(final_pedal, P.GAS_LOOKUP_BP, P.GAS_LOOKUP_V)))
apply_brake = int(round(interp(final_pedal, P.BRAKE_LOOKUP_BP, P.BRAKE_LOOKUP_V)))
# Gas/regen and brakes - all at 25Hz
if (frame % 4) == 0:
idx = (frame / 4) % 4
at_full_stop = enabled and CS.standstill
near_stop = enabled and (CS.v_ego < 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))
at_full_stop = enabled and CS.standstill
can_sends.append(gmcan.create_gas_regen_command(self.packer_pt, canbus.powertrain, apply_gas, idx, enabled, at_full_stop))
# Send dashboard UI commands (ACC status), 25hz
if (frame % 4) == 0:
can_sends.append(gmcan.create_acc_dashboard_command(self.packer_pt, canbus.powertrain, enabled, hud_v_cruise * CV.MS_TO_KPH, hud_show_car))
# Radar needs to know current speed and yaw rate (50hz),
# and that ADAS is alive (10hz)
time_and_headlights_step = 10
tt = sec_since_boot()
if frame % time_and_headlights_step == 0:
idx = (frame / time_and_headlights_step) % 4
can_sends.append(gmcan.create_adas_time_status(canbus.obstacle, int((tt - self.start_time) * 60), idx))
can_sends.append(gmcan.create_adas_headlights_status(canbus.obstacle))
speed_and_accelerometer_step = 2
if frame % speed_and_accelerometer_step == 0:
idx = (frame / speed_and_accelerometer_step) % 4
can_sends.append(gmcan.create_adas_steering_status(canbus.obstacle, idx))
can_sends.append(gmcan.create_adas_accelerometer_speed_status(canbus.obstacle, CS.v_ego, idx))
if frame % P.ADAS_KEEPALIVE_STEP == 0:
can_sends += gmcan.create_adas_keepalive(canbus.powertrain)
# Show green icon when LKA torque is applied, and
# alarming orange icon when approaching torque limit.
# If not sent again, LKA icon disappears in about 5 seconds.
# Conveniently, sending camera message periodically also works as a keepalive.
lka_active = CS.lkas_status == 1
lka_critical = lka_active and abs(actuators.steer) > 0.9
lka_icon_status = (lka_active, lka_critical)
if frame % P.CAMERA_KEEPALIVE_STEP == 0 \
or lka_icon_status != self.lka_icon_status_last:
can_sends.append(gmcan.create_lka_icon_command(canbus.sw_gmlan, lka_active, lka_critical))
self.lka_icon_status_last = lka_icon_status
# Send chimes
if self.chime != chime:
duration = 0x3c
# There is no 'repeat forever' chime command
# TODO: Manage periodic re-issuing of chime command
# and chime cancellation
if chime_cnt == -1:
chime_cnt = 10
if chime != 0:
can_sends.append(gmcan.create_chime_command(canbus.sw_gmlan, chime, duration, chime_cnt))
# If canceling a repeated chime, cancel command must be
# issued for the same chime type and duration
self.chime = chime
sendcan.send(can_list_to_can_capnp(can_sends, msgtype='sendcan').to_bytes())
def outer_loop_gain(self):
return interp(self.speed, self._outer_loop_gain[0],
self._outer_loop_gain[1])
def update(self, active, v_ego, brake_pressed, v_cruise, v_target_lead,
a_target, jerk_factor, CP):
# actuation limits
gas_max = interp(v_ego, CP.gasMaxBP, CP.gasMaxV)
brake_max = interp(v_ego, CP.brakeMaxBP, CP.brakeMaxV)
overshoot_allowance = 2.0 # overshoot allowed when changing accel sign
output_gb = self.last_output_gb
# limit max target speed based on cruise setting
v_target = min(v_target_lead, v_cruise * CV.KPH_TO_MS)
rate = 100.0
max_speed_delta_up = a_target[1] * 1.0 / rate
max_speed_delta_down = a_target[0] * 1.0 / rate
self.long_control_state = long_control_state_trans(active, self.long_control_state, v_ego,\
v_target, self.v_pid, output_gb, brake_pressed)
v_ego_pid = max(
v_ego, 0.3
) # Without this we get jumps, CAN bus reports 0 when speed < 0.3
# *** long control behavior based on state
if self.long_control_state == LongCtrlState.off:
self.v_pid = v_ego_pid # do nothing
output_gb = 0.
self.pid.reset()
# tracking objects and driving
elif self.long_control_state == LongCtrlState.pid:
#reset v_pid close to v_ego if it was too far and new v_target is closer to v_ego
if ((self.v_pid > v_ego + overshoot_allowance)
and (v_target < self.v_pid)):
self.v_pid = max(v_target, v_ego + overshoot_allowance)
elif ((self.v_pid < v_ego - overshoot_allowance)
and (v_target > self.v_pid)):
self.v_pid = min(v_target, v_ego - overshoot_allowance)
# move v_pid no faster than allowed accel limits
if (v_target > self.v_pid + max_speed_delta_up):
self.v_pid += max_speed_delta_up
elif (v_target < self.v_pid + max_speed_delta_down):
self.v_pid += max_speed_delta_down
else:
self.v_pid = v_target
# to avoid too much wind up on acceleration, limit positive speed error
if CP.enableGas:
max_speed_error = interp(v_ego, _MAX_SPEED_ERROR_BP,
_MAX_SPEED_ERROR_V)
self.v_pid = min(self.v_pid, v_ego + max_speed_error)
self.pid.pos_limit = gas_max
self.pid.neg_limit = -brake_max
output_gb = self.pid.update(self.v_pid,
v_ego_pid,
speed=v_ego_pid,
jerk_factor=jerk_factor)
# intention is to stop, switch to a different brake control until we stop
elif self.long_control_state == LongCtrlState.stopping:
# TODO: use the standstill bit from CAN to detect movements, usually more accurate than looking at v_ego
if v_ego > 0. or output_gb > -brake_stopping_target:
output_gb -= stopping_brake_rate / rate
output_gb = clip(output_gb, -brake_max, gas_max)
self.v_pid = v_ego
self.pid.reset()
# intention is to move again, release brake fast before handling control to PID
elif self.long_control_state == LongCtrlState.starting:
if output_gb < -0.2:
output_gb += starting_brake_rate / rate
self.v_pid = v_ego
self.pid.reset()
self.pid.i = starting_Ui
self.last_output_gb = output_gb
final_gas = clip(output_gb, 0., gas_max)
final_brake = -clip(output_gb, -brake_max, 0.)
return final_gas, final_brake