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