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