def get_apply_accel(self, CS, sm, accel, stopping):
gas_factor = ntune_scc_get("sccGasFactor")
brake_factor = ntune_scc_get("sccBrakeFactor")
#lead = self.get_lead(sm)
#if lead is not None:
# if not lead.radar:
# brake_factor *= 0.975
if accel > 0:
accel *= gas_factor
else:
accel *= brake_factor
return accel
def cal_curve_speed(self, sm, v_ego, frame):
if frame % 20 == 0:
md = sm['modelV2']
if len(md.position.x) == TRAJECTORY_SIZE and len(
md.position.y) == TRAJECTORY_SIZE:
x = md.position.x
y = md.position.y
dy = np.gradient(y, x)
d2y = np.gradient(dy, x)
curv = d2y / (1 + dy**2)**1.5
start = int(
interp(v_ego, [10., 27.], [10, TRAJECTORY_SIZE - 10]))
curv = curv[start:min(start + 10, TRAJECTORY_SIZE)]
a_y_max = 2.975 - v_ego * 0.0375 # ~1.85 @ 75mph, ~2.6 @ 25mph
v_curvature = np.sqrt(a_y_max /
np.clip(np.abs(curv), 1e-4, None))
model_speed = np.mean(v_curvature) * 0.85 * ntune_scc_get(
"sccCurvatureFactor")
if model_speed < v_ego:
self.curve_speed_ms = float(
max(model_speed, MIN_CURVE_SPEED))
else:
self.curve_speed_ms = 255.
if np.isnan(self.curve_speed_ms):
self.curve_speed_ms = 255.
else:
self.curve_speed_ms = 255.
def get_apply_accel(self, CS, sm, accel, stopping):
gas_factor = ntune_scc_get("sccGasFactor")
brake_factor = ntune_scc_get("sccBrakeFactor")
#lead = self.get_lead(sm)
#if lead is not None:
# if not lead.radar:
# brake_factor *= 0.975
if accel > 0:
accel *= gas_factor
else:
accel *= brake_factor
start_boost = interp(CS.out.vEgo, [0.0, CREEP_SPEED, 2 * CREEP_SPEED],
[0.6, 0.6, 0.0])
is_accelerating = interp(accel, [0.0, 0.2], [0.0, 1.0])
boost = start_boost * is_accelerating
return accel + boost
def cal_curve_speed(self, sm, v_ego, frame):
lateralPlan = sm['lateralPlan']
if len(lateralPlan.curvatures) == CONTROL_N:
curv = (lateralPlan.curvatures[-1] + lateralPlan.curvatures[-2]) / 2.
a_y_max = 2.975 - v_ego * 0.0375 # ~1.85 @ 75mph, ~2.6 @ 25mph
v_curvature = sqrt(a_y_max / max(abs(curv), 1e-4))
model_speed = v_curvature * 0.85 * ntune_scc_get("sccCurvatureFactor")
if model_speed < v_ego:
self.curve_speed_ms = float(max(model_speed, MIN_CURVE_SPEED))
else:
self.curve_speed_ms = 255.
if np.isnan(self.curve_speed_ms):
self.curve_speed_ms = 255.
else:
self.curve_speed_ms = 255.
def publish_logs(self, CS, start_time, actuators, lac_log):
"""Send actuators and hud commands to the car, send controlsstate and MPC logging"""
CC = car.CarControl.new_message()
CC.enabled = self.enabled
CC.active = self.active
CC.actuators = actuators
orientation_value = self.sm['liveLocationKalman'].orientationNED.value
if len(orientation_value) > 2:
CC.roll = orientation_value[0]
CC.pitch = orientation_value[1]
CC.cruiseControl.cancel = self.CP.pcmCruise and not self.enabled and CS.cruiseState.enabled
if self.joystick_mode and self.sm.rcv_frame[
'testJoystick'] > 0 and self.sm['testJoystick'].buttons[0]:
CC.cruiseControl.cancel = True
hudControl = CC.hudControl
hudControl.setSpeed = float(self.v_cruise_kph * CV.KPH_TO_MS)
hudControl.speedVisible = self.enabled
hudControl.lanesVisible = self.enabled
hudControl.leadVisible = self.sm['longitudinalPlan'].hasLead
right_lane_visible = self.sm['lateralPlan'].rProb > 0.5
left_lane_visible = self.sm['lateralPlan'].lProb > 0.5
if self.sm.frame % 100 == 0:
self.right_lane_visible = right_lane_visible
self.left_lane_visible = left_lane_visible
CC.hudControl.rightLaneVisible = self.right_lane_visible
CC.hudControl.leftLaneVisible = self.left_lane_visible
recent_blinker = (self.sm.frame - self.last_blinker_frame
) * DT_CTRL < 5.0 # 5s blinker cooldown
ldw_allowed = self.is_ldw_enabled and CS.vEgo > LDW_MIN_SPEED and not recent_blinker \
and not self.active and self.sm['liveCalibration'].calStatus == Calibration.CALIBRATED
model_v2 = self.sm['modelV2']
desire_prediction = model_v2.meta.desirePrediction
if len(desire_prediction) and ldw_allowed:
right_lane_visible = self.sm['lateralPlan'].rProb > 0.5
left_lane_visible = self.sm['lateralPlan'].lProb > 0.5
l_lane_change_prob = desire_prediction[Desire.laneChangeLeft - 1]
r_lane_change_prob = desire_prediction[Desire.laneChangeRight - 1]
lane_lines = model_v2.laneLines
cameraOffset = ntune_common_get(
"cameraOffset"
) + 0.08 if self.wide_camera else ntune_common_get("cameraOffset")
l_lane_close = left_lane_visible and (lane_lines[1].y[0] >
-(1.08 + cameraOffset))
r_lane_close = right_lane_visible and (lane_lines[2].y[0] <
(1.08 - cameraOffset))
hudControl.leftLaneDepart = bool(
l_lane_change_prob > LANE_DEPARTURE_THRESHOLD and l_lane_close)
hudControl.rightLaneDepart = bool(
r_lane_change_prob > LANE_DEPARTURE_THRESHOLD and r_lane_close)
if hudControl.rightLaneDepart or hudControl.leftLaneDepart:
self.events.add(EventName.ldw)
clear_event_types = set()
if ET.WARNING not in self.current_alert_types:
clear_event_types.add(ET.WARNING)
if self.enabled:
clear_event_types.add(ET.NO_ENTRY)
alerts = self.events.create_alerts(
self.current_alert_types,
[self.CP, self.sm, self.is_metric, self.soft_disable_timer])
self.AM.add_many(self.sm.frame, alerts)
current_alert = self.AM.process_alerts(self.sm.frame,
clear_event_types)
if current_alert:
hudControl.visualAlert = current_alert.visual_alert
if not self.read_only and self.initialized:
# send car controls over can
self.last_actuators, can_sends = self.CI.apply(CC, self)
self.pm.send(
'sendcan',
can_list_to_can_capnp(can_sends,
msgtype='sendcan',
valid=CS.canValid))
CC.actuatorsOutput = self.last_actuators
force_decel = (self.sm['driverMonitoringState'].awarenessStatus < 0.) or \
(self.state == State.softDisabling)
# Curvature & Steering angle
params = self.sm['liveParameters']
steer_angle_without_offset = math.radians(CS.steeringAngleDeg -
params.angleOffsetDeg)
curvature = -self.VM.calc_curvature(steer_angle_without_offset,
CS.vEgo, params.roll)
# controlsState
dat = messaging.new_message('controlsState')
dat.valid = CS.canValid
controlsState = dat.controlsState
if current_alert:
controlsState.alertText1 = current_alert.alert_text_1
controlsState.alertText2 = current_alert.alert_text_2
controlsState.alertSize = current_alert.alert_size
controlsState.alertStatus = current_alert.alert_status
controlsState.alertBlinkingRate = current_alert.alert_rate
controlsState.alertType = current_alert.alert_type
controlsState.alertSound = current_alert.audible_alert
controlsState.canMonoTimes = list(CS.canMonoTimes)
controlsState.longitudinalPlanMonoTime = self.sm.logMonoTime[
'longitudinalPlan']
controlsState.lateralPlanMonoTime = self.sm.logMonoTime['lateralPlan']
controlsState.enabled = self.enabled
controlsState.active = self.active
controlsState.curvature = curvature
controlsState.state = self.state
controlsState.engageable = not self.events.any(ET.NO_ENTRY)
controlsState.longControlState = self.LoC.long_control_state
controlsState.vPid = float(self.LoC.v_pid)
controlsState.vCruise = float(
self.applyMaxSpeed if self.CP.
openpilotLongitudinalControl else self.v_cruise_kph)
controlsState.upAccelCmd = float(self.LoC.pid.p)
controlsState.uiAccelCmd = float(self.LoC.pid.i)
controlsState.ufAccelCmd = float(self.LoC.pid.f)
controlsState.cumLagMs = -self.rk.remaining * 1000.
controlsState.startMonoTime = int(start_time * 1e9)
controlsState.forceDecel = bool(force_decel)
controlsState.canErrorCounter = self.can_rcv_error_counter
controlsState.angleSteers = steer_angle_without_offset * CV.RAD_TO_DEG
controlsState.applyAccel = self.apply_accel
controlsState.aReqValue = self.aReqValue
controlsState.aReqValueMin = self.aReqValueMin
controlsState.aReqValueMax = self.aReqValueMax
controlsState.sccStockCamAct = self.sccStockCamAct
controlsState.sccStockCamStatus = self.sccStockCamStatus
controlsState.steerRatio = self.VM.sR
controlsState.steerRateCost = ntune_common_get('steerRateCost')
controlsState.steerActuatorDelay = ntune_common_get(
'steerActuatorDelay')
controlsState.sccGasFactor = ntune_scc_get('sccGasFactor')
controlsState.sccBrakeFactor = ntune_scc_get('sccBrakeFactor')
controlsState.sccCurvatureFactor = ntune_scc_get('sccCurvatureFactor')
lat_tuning = self.CP.lateralTuning.which()
if self.joystick_mode:
controlsState.lateralControlState.debugState = lac_log
elif self.CP.steerControlType == car.CarParams.SteerControlType.angle:
controlsState.lateralControlState.angleState = lac_log
elif lat_tuning == 'pid':
controlsState.lateralControlState.pidState = lac_log
elif lat_tuning == 'lqr':
controlsState.lateralControlState.lqrState = lac_log
elif lat_tuning == 'indi':
controlsState.lateralControlState.indiState = lac_log
self.pm.send('controlsState', dat)
# carState
car_events = self.events.to_msg()
cs_send = messaging.new_message('carState')
cs_send.valid = CS.canValid
cs_send.carState = CS
cs_send.carState.events = car_events
self.pm.send('carState', cs_send)
# carEvents - logged every second or on change
if (self.sm.frame % int(1. / DT_CTRL)
== 0) or (self.events.names != self.events_prev):
ce_send = messaging.new_message('carEvents', len(self.events))
ce_send.carEvents = car_events
self.pm.send('carEvents', ce_send)
self.events_prev = self.events.names.copy()
# carParams - logged every 50 seconds (> 1 per segment)
if (self.sm.frame % int(50. / DT_CTRL) == 0):
cp_send = messaging.new_message('carParams')
cp_send.carParams = self.CP
self.pm.send('carParams', cp_send)
# carControl
cc_send = messaging.new_message('carControl')
cc_send.valid = CS.canValid
cc_send.carControl = CC
self.pm.send('carControl', cc_send)
# copy CarControl to pass to CarInterface on the next iteration
self.CC = CC
def update(self, active, CS, CP, long_plan, accel_limits):
"""Update longitudinal control. This updates the state machine and runs a PID loop"""
# Interp control trajectory
# TODO estimate car specific lag, use .15s for now
if len(long_plan.speeds) == CONTROL_N:
longitudinalActuatorDelay = ntune_scc_get("longitudinalActuatorDelay")
v_target = interp(longitudinalActuatorDelay, T_IDXS[:CONTROL_N], long_plan.speeds)
v_target_future = long_plan.speeds[-1]
a_target = 2 * (v_target - long_plan.speeds[0])/longitudinalActuatorDelay - long_plan.accels[0]
else:
v_target = 0.0
v_target_future = 0.0
a_target = 0.0
# TODO: This check is not complete and needs to be enforced by MPC
a_target = clip(a_target, ACCEL_MIN_ISO, ACCEL_MAX_ISO)
self.pid.neg_limit = accel_limits[0]
self.pid.pos_limit = accel_limits[1]
# Update state machine
output_accel = self.last_output_accel
self.long_control_state = long_control_state_trans(active, self.long_control_state, CS.vEgo,
v_target_future, self.v_pid, output_accel,
CS.brakePressed, CS.cruiseState.standstill, CP.minSpeedCan)
v_ego_pid = max(CS.vEgo, CP.minSpeedCan) # Without this we get jumps, CAN bus reports 0 when speed < 0.3
if self.long_control_state == LongCtrlState.off or CS.gasPressed:
self.reset(v_ego_pid)
output_accel = 0.
# tracking objects and driving
elif self.long_control_state == LongCtrlState.pid:
self.v_pid = v_target
# 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)
freeze_integrator = prevent_overshoot
output_accel = self.pid.update(self.v_pid, v_ego_pid, speed=v_ego_pid, deadzone=deadzone, feedforward=a_target, freeze_integrator=freeze_integrator)
if prevent_overshoot:
output_accel = min(output_accel, 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_accel > -DECEL_STOPPING_TARGET:
output_accel -= CP.stoppingDecelRate / RATE
output_accel = clip(output_accel, accel_limits[0], accel_limits[1])
self.reset(CS.vEgo)
# Intention is to move again, release brake fast before handing control to PID
elif self.long_control_state == LongCtrlState.starting:
if output_accel < -DECEL_THRESHOLD_TO_PID:
output_accel += CP.startingAccelRate / RATE
self.reset(CS.vEgo)
self.last_output_accel = output_accel
final_accel = clip(output_accel, accel_limits[0], accel_limits[1])
return final_accel, v_target, a_target
def update(self, active, CS, CP, long_plan, accel_limits, radarState):
"""Update longitudinal control. This updates the state machine and runs a PID loop"""
# Interp control trajectory
# TODO estimate car specific lag, use .15s for now
speeds = long_plan.speeds
if len(speeds) == CONTROL_N:
longitudinalActuatorDelayLowerBound = ntune_scc_get(
'longitudinalActuatorDelayLowerBound')
longitudinalActuatorDelayUpperBound = ntune_scc_get(
'longitudinalActuatorDelayUpperBound')
v_target_lower = interp(longitudinalActuatorDelayLowerBound,
T_IDXS[:CONTROL_N], speeds)
a_target_lower = 2 * (
v_target_lower - speeds[0]
) / longitudinalActuatorDelayLowerBound - long_plan.accels[0]
v_target_upper = interp(longitudinalActuatorDelayUpperBound,
T_IDXS[:CONTROL_N], speeds)
a_target_upper = 2 * (
v_target_upper - speeds[0]
) / longitudinalActuatorDelayUpperBound - long_plan.accels[0]
a_target = min(a_target_lower, a_target_upper)
v_target = speeds[0]
v_target_future = speeds[-1]
else:
v_target = 0.0
v_target_future = 0.0
a_target = 0.0
if a_target > 0.:
a_target *= interp(CS.vEgo, [0., 3.], [1.2, 1.])
# TODO: This check is not complete and needs to be enforced by MPC
a_target = clip(a_target, ACCEL_MIN_ISO, ACCEL_MAX_ISO)
self.pid.neg_limit = accel_limits[0]
self.pid.pos_limit = accel_limits[1]
# Update state machine
output_accel = self.last_output_accel
self.long_control_state = long_control_state_trans(
CP, active, self.long_control_state, CS.vEgo, v_target_future,
self.v_pid, CS.brakePressed, CS.cruiseState.standstill, radarState)
if self.long_control_state == LongCtrlState.off or CS.gasPressed:
self.reset(CS.vEgo)
output_accel = 0.
# tracking objects and driving
elif self.long_control_state == LongCtrlState.pid:
self.v_pid = v_target
# 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 and v_target_future < self.v_pid
deadzone = interp(CS.vEgo, CP.longitudinalTuning.deadzoneBP,
CP.longitudinalTuning.deadzoneV)
freeze_integrator = prevent_overshoot
output_accel = self.pid.update(self.v_pid,
CS.vEgo,
speed=CS.vEgo,
deadzone=deadzone,
feedforward=a_target,
freeze_integrator=freeze_integrator)
if prevent_overshoot:
output_accel = min(output_accel, 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_accel > CP.stopAccel:
output_accel -= CP.stoppingDecelRate * DT_CTRL * \
interp(output_accel, [CP.stopAccel, CP.stopAccel/2., CP.stopAccel/4., 0.], [0.3, 0.65, 1., 5.])
output_accel = clip(output_accel, accel_limits[0], accel_limits[1])
self.reset(CS.vEgo)
self.last_output_accel = output_accel
final_accel = clip(output_accel, accel_limits[0], accel_limits[1])
return final_accel
