diff --git a/panda/board/safety/safety_toyota.h b/panda/board/safety/safety_toyota.h index 844e387fef7716..73dada00f440d7 100644 --- a/panda/board/safety/safety_toyota.h +++ b/panda/board/safety/safety_toyota.h @@ -4,17 +4,17 @@ const int TOYOTA_MAX_TORQUE = 1500; // max torque cmd allowed ever // rate based torque limit + stay within actually applied // packet is sent at 100hz, so this limit is 1000/sec const int TOYOTA_MAX_RATE_UP = 10; // ramp up slow -const int TOYOTA_MAX_RATE_DOWN = 25; // ramp down fast +const int TOYOTA_MAX_RATE_DOWN = 44; // ramp down fast const int TOYOTA_MAX_TORQUE_ERROR = 350; // max torque cmd in excess of torque motor // real time torque limit to prevent controls spamming // the real time limit is 1500/sec -const int TOYOTA_MAX_RT_DELTA = 375; // max delta torque allowed for real time checks +const int TOYOTA_MAX_RT_DELTA = 475; // max delta torque allowed for real time checks const uint32_t TOYOTA_RT_INTERVAL = 250000; // 250ms between real time checks // longitudinal limits -const int TOYOTA_MAX_ACCEL = 1500; // 1.5 m/s2 -const int TOYOTA_MIN_ACCEL = -3000; // 3.0 m/s2 +const int TOYOTA_MAX_ACCEL = 4000; // 4.0 m/s2 +const int TOYOTA_MIN_ACCEL = -8000; // 8.0 m/s2 const int TOYOTA_GAS_INTERCEPTOR_THRESHOLD = 475; // ratio between offset and gain from dbc file @@ -37,6 +37,8 @@ int toyota_desired_torque_last = 0; // last desired steer torque int toyota_rt_torque_last = 0; // last desired torque for real time check uint32_t toyota_ts_last = 0; int toyota_cruise_engaged_last = 0; // cruise state +int ego_speed_toyota = 0; // speed +int toyota_gas_pressed = 0; int toyota_gas_prev = 0; struct sample_t toyota_torque_meas; // last 3 motor torques produced by the eps @@ -63,7 +65,12 @@ static int toyota_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { if (valid) { int bus = GET_BUS(to_push); int addr = GET_ADDR(to_push); - + // sample speed + if (addr == 0xb4) { + // Middle bytes needed + ego_speed_toyota = (GET_BYTE(to_push, 5) << 8) | GET_BYTE(to_push, 6); + ego_speed_toyota = to_signed(ego_speed_toyota, 16); + } // get eps motor torque (0.66 factor in dbc) if (addr == 0x260) { int torque_meas_new = (GET_BYTE(to_push, 5) << 8) | GET_BYTE(to_push, 6); @@ -81,9 +88,9 @@ static int toyota_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { } // enter controls on rising edge of ACC, exit controls on ACC off - if (addr == 0x1D2) { - // 5th bit is CRUISE_ACTIVE - int cruise_engaged = GET_BYTE(to_push, 0) & 0x20; + if (addr == 0x1D3) { + // 5th bit is MAIN_ON + int cruise_engaged = GET_BYTE(to_push, 1) >> 7; if (!cruise_engaged) { controls_allowed = 0; } @@ -99,7 +106,7 @@ static int toyota_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { int gas_interceptor = GET_INTERCEPTOR(to_push); if ((gas_interceptor > TOYOTA_GAS_INTERCEPTOR_THRESHOLD) && (gas_interceptor_prev <= TOYOTA_GAS_INTERCEPTOR_THRESHOLD)) { - controls_allowed = 0; + toyota_gas_pressed = 1; } gas_interceptor_prev = gas_interceptor; } @@ -108,7 +115,7 @@ static int toyota_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { if (addr == 0x2C1) { int gas = GET_BYTE(to_push, 6) & 0xFF; if ((gas > 0) && (toyota_gas_prev == 0) && !gas_interceptor_detected) { - controls_allowed = 0; + toyota_gas_pressed = 1; } toyota_gas_prev = gas; } @@ -142,7 +149,7 @@ static int toyota_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) { if (addr == 0x200) { if (!controls_allowed) { if (GET_BYTE(to_send, 0) || GET_BYTE(to_send, 1)) { - tx = 0; + toyota_gas_pressed = 1; } } } @@ -194,8 +201,12 @@ static int toyota_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) { } // no torque if controls is not allowed - if (!controls_allowed && (desired_torque != 0)) { + if (!controls_allowed) { + if (ego_speed_toyota > 4500){ + violation |= max_limit_check(desired_torque, 805, -805); + } else { violation = 1; + } } // reset to 0 if either controls is not allowed or there's a violation diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py index 61516b638aa7da..ee52c0821abf07 100644 --- a/selfdrive/car/toyota/carcontroller.py +++ b/selfdrive/car/toyota/carcontroller.py @@ -11,8 +11,8 @@ # Accel limits ACCEL_HYST_GAP = 0.02 # don't change accel command for small oscilalitons within this value -ACCEL_MAX = 1.5 # 1.5 m/s2 -ACCEL_MIN = -3.0 # 3 m/s2 +ACCEL_MAX = 3.5 # 1.5 m/s2 +ACCEL_MIN = -3.5 # 3 m/s2 ACCEL_SCALE = max(ACCEL_MAX, -ACCEL_MIN) @@ -126,6 +126,18 @@ def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd, hud_alert, apply_accel, self.accel_steady = accel_hysteresis(apply_accel, self.accel_steady, enabled) apply_accel = clip(apply_accel * ACCEL_SCALE, ACCEL_MIN, ACCEL_MAX) + if CS.CP.enableGasInterceptor: + if CS.pedal_gas > 15.0: + apply_accel = max(apply_accel, 0.06) + if CS.brake_pressed: + apply_gas = 0.0 + apply_accel = min(apply_accel, 0.00) + else: + if CS.pedal_gas > 0.0: + apply_accel = max(apply_accel, 0.0) + if CS.brake_pressed and CS.v_ego > 1: + apply_accel = min(apply_accel, 0.0) + # steer torque new_steer = int(round(actuators.steer * SteerLimitParams.STEER_MAX)) diff --git a/selfdrive/car/toyota/carstate.py b/selfdrive/car/toyota/carstate.py index fd86fe527ae654..989570abbe94cf 100644 --- a/selfdrive/car/toyota/carstate.py +++ b/selfdrive/car/toyota/carstate.py @@ -121,6 +121,8 @@ def __init__(self, CP): self.right_blinker_on = 0 self.angle_offset = 0. self.init_angle_offset = False + self.v_cruise_pcmlast = 41.0 + self.setspeedoffset = 34.0 if not travis: self.arne_pm = messaging_arne.PubMaster(['liveTrafficData', 'arne182Status']) @@ -211,6 +213,41 @@ def update(self, cp, cp_cam): else: self.v_cruise_pcm = cp.vl["PCM_CRUISE_2"]['SET_SPEED'] self.low_speed_lockout = cp.vl["PCM_CRUISE_2"]['LOW_SPEED_LOCKOUT'] == 2 + if self.CP.carFingerprint in TSS2_CAR: + minimum_set_speed = 27.0 + else: + minimum_set_speed = 41.0 + if cp.vl["PCM_CRUISE"]['CRUISE_STATE'] and not self.pcm_acc_status: + if self.v_ego < 11.38: + self.setspeedoffset = max(min(int(minimum_set_speed-self.v_ego*3.6),(minimum_set_speed-7.0)),0.0) + self.v_cruise_pcmlast = self.v_cruise_pcm + else: + self.setspeedoffset = 0 + self.v_cruise_pcmlast = self.v_cruise_pcm + if self.v_cruise_pcm < self.v_cruise_pcmlast: + if self.setspeedcounter > 0 and self.v_cruise_pcm > minimum_set_speed: + self.setspeedoffset = self.setspeedoffset + 4 + else: + if math.floor((int((-self.v_cruise_pcm)*(minimum_set_speed-7.0)/(169.0-minimum_set_speed) + 169.0*(minimum_set_speed-7.0)/(169.0-minimum_set_speed))-self.setspeedoffset)/(self.v_cruise_pcm-(minimum_set_speed-1.0))) > 0: + self.setspeedoffset = self.setspeedoffset + math.floor((int((-self.v_cruise_pcm)*(minimum_set_speed-7.0)/(169.0-minimum_set_speed) + 169*(minimum_set_speed-7.0)/(169.0-minimum_set_speed))-self.setspeedoffset)/(self.v_cruise_pcm-(minimum_set_speed-1.0))) + self.setspeedcounter = 50 + if self.v_cruise_pcmlast < self.v_cruise_pcm: + if self.setspeedcounter > 0 and (self.setspeedoffset - 4) > 0: + self.setspeedoffset = self.setspeedoffset - 4 + else: + self.setspeedoffset = self.setspeedoffset + math.floor((int((-self.v_cruise_pcm)*(minimum_set_speed-7.0)/(169.0-minimum_set_speed) + 169*(minimum_set_speed-7.0)/(169.0-minimum_set_speed))-self.setspeedoffset)/(170-self.v_cruise_pcm)) + self.setspeedcounter = 50 + if self.setspeedcounter > 0: + self.setspeedcounter = self.setspeedcounter - 1 + self.v_cruise_pcmlast = self.v_cruise_pcm + if int(self.v_cruise_pcm) - self.setspeedoffset < 7: + self.setspeedoffset = int(self.v_cruise_pcm) - 7 + if int(self.v_cruise_pcm) - self.setspeedoffset > 169: + self.setspeedoffset = int(self.v_cruise_pcm) - 169 + + + self.v_cruise_pcm = min(max(7, int(self.v_cruise_pcm) - self.setspeedoffset),169) + self.pcm_acc_status = cp.vl["PCM_CRUISE"]['CRUISE_STATE'] self.pcm_acc_active = bool(cp.vl["PCM_CRUISE"]['CRUISE_ACTIVE']) self.brake_lights = bool(cp.vl["ESP_CONTROL"]['BRAKE_LIGHTS_ACC'] or self.brake_pressed) diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py index 83f499f4509f78..b4ecf585b8acef 100755 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -16,7 +16,7 @@ class CarInterface(CarInterfaceBase): def __init__(self, CP, CarController): self.CP = CP self.VM = VehicleModel(CP) - + self.waiting = False self.frame = 0 self.gas_pressed_prev = False self.brake_pressed_prev = False @@ -34,7 +34,7 @@ def __init__(self, CP, CarController): @staticmethod def compute_gb(accel, speed): - return float(accel) / 3.0 + return float(accel) / 4.0 @staticmethod def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False, car_fw=[]): @@ -94,7 +94,7 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False, ret.lateralTuning.init('lqr') ret.lateralTuning.lqr.scale = 1500.0 - ret.lateralTuning.lqr.ki = 0.05 + ret.lateralTuning.lqr.ki = 0.06 ret.lateralTuning.lqr.a = [0., 1., -0.22619643, 1.21822268] ret.lateralTuning.lqr.b = [-1.92006585e-04, 3.95603032e-05] @@ -242,8 +242,8 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False, # steer, gas, brake limitations VS speed ret.steerMaxBP = [16. * CV.KPH_TO_MS, 45. * CV.KPH_TO_MS] # breakpoints at 1 and 40 kph ret.steerMaxV = [1., 1.] # 2/3rd torque allowed above 45 kph - ret.brakeMaxBP = [0.] - ret.brakeMaxV = [1.] + ret.brakeMaxBP = [0., 35., 55.] + ret.brakeMaxV = [1.0, 0.8, 0.7] ret.enableCamera = is_ecu_disconnected(fingerprint[0], FINGERPRINTS, ECU_FINGERPRINT, candidate, ECU.CAM) or has_relay # In TSS2 cars the camera does long control @@ -271,15 +271,15 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False, ret.startAccel = 0.0 if ret.enableGasInterceptor: - ret.gasMaxBP = [0., 9., 35] + ret.gasMaxBP = [0., 9., 55] ret.gasMaxV = [0.2, 0.5, 0.7] - ret.longitudinalTuning.kpV = [1.2, 0.8, 0.5] - ret.longitudinalTuning.kiV = [0.18, 0.12] + ret.longitudinalTuning.kpV = [0.50, 0.4, 0.3] # braking tune + ret.longitudinalTuning.kiV = [0.135, 0.1] else: - ret.gasMaxBP = [0.] - ret.gasMaxV = [0.5] - ret.longitudinalTuning.kpV = [3.6, 2.4, 1.5] - ret.longitudinalTuning.kiV = [0.54, 0.36] + ret.gasMaxBP = [0., 9., 55] + ret.gasMaxV = [0.2, 0.5, 0.7] + ret.longitudinalTuning.kpV = [0.325, 0.325, 0.325] # braking tune from rav4h + ret.longitudinalTuning.kiV = [0.1, 0.10] return ret @@ -333,7 +333,18 @@ def update(self, c, can_strings): ret.steeringRateLimited = self.CC.steer_rate_limited if self.CC is not None else False # cruise state - ret.cruiseState.enabled = self.CS.pcm_acc_active + if not self.cruise_enabled_prev: + self.waiting = False + ret.cruiseState.enabled = self.CS.pcm_acc_active + else: + if self.keep_openpilot_engaged: + ret.cruiseState.enabled = bool(self.CS.main_on) + if not self.CS.pcm_acc_active: + #eventsArne182.append(create_event_arne('longControlDisabled', [ET.WARNING])) + ret.brakePressed = True + self.waiting = False + if self.CS.v_ego < 1 or not self.keep_openpilot_engaged: + ret.cruiseState.enabled = self.CS.pcm_acc_active ret.cruiseState.speed = self.CS.v_cruise_pcm * CV.KPH_TO_MS ret.cruiseState.available = bool(self.CS.main_on) ret.cruiseState.speedOffset = 0. @@ -404,7 +415,13 @@ def update(self, c, can_strings): events.append(create_event('pcmEnable', [ET.ENABLE])) elif not ret.cruiseState.enabled: events.append(create_event('pcmDisable', [ET.USER_DISABLE])) - + if not self.waiting and ret.vEgo < 0.3 and not ret.gasPressed and self.CP.carFingerprint == CAR.RAV4H: + self.waiting = True + if self.waiting: + if ret.gasPressed: + self.waiting = False + #else: + # eventsArne182.append(create_event_arne('waitingMode', [ET.WARNING])) # disable on pedals rising edge or when brake is pressed and speed isn't zero if (ret.gasPressed and not self.gas_pressed_prev) or \ (ret.brakePressed and (not self.brake_pressed_prev or ret.vEgo > 0.001)): diff --git a/selfdrive/controls/lib/pathplanner.py b/selfdrive/controls/lib/pathplanner.py index 8a6e5286f37c46..ac07c53c983a24 100644 --- a/selfdrive/controls/lib/pathplanner.py +++ b/selfdrive/controls/lib/pathplanner.py @@ -1,4 +1,3 @@ -import os import math from common.realtime import sec_since_boot, DT_MDL from selfdrive.swaglog import cloudlog @@ -12,9 +11,8 @@ LaneChangeState = log.PathPlan.LaneChangeState LaneChangeDirection = log.PathPlan.LaneChangeDirection -LOG_MPC = os.environ.get('LOG_MPC', False) -LANE_CHANGE_SPEED_MIN = 45 * CV.MPH_TO_MS +LANE_CHANGE_SPEED_MIN = 20 * CV.MPH_TO_MS LANE_CHANGE_TIME_MAX = 10. DESIRES = { @@ -219,12 +217,12 @@ def update(self, sm, pm, CP, VM): pm.send('pathPlan', plan_send) - if LOG_MPC: - dat = messaging.new_message() - dat.init('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) + + dat = messaging.new_message() + dat.init('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) diff --git a/selfdrive/ui/paint.cc b/selfdrive/ui/paint.cc index f3a3df9978e852..74d88f636345ca 100644 --- a/selfdrive/ui/paint.cc +++ b/selfdrive/ui/paint.cc @@ -666,13 +666,13 @@ static void ui_draw_vision_event(UIState *s) { nvgSave(s->vg); nvgTranslate(s->vg,bg_wheel_x,(bg_wheel_y + (bdr_s*1.5))); nvgRotate(s->vg,-img_rotation); - nvgBeginPath(s->vg); - NVGpaint imgPaint = nvgImagePattern(s->vg, img_wheel_x-bg_wheel_x, img_wheel_y-(bg_wheel_y + (bdr_s*1.5)), - img_wheel_size, img_wheel_size, 0, s->img_wheel, img_wheel_alpha); - nvgRect(s->vg, img_wheel_x-bg_wheel_x, img_wheel_y-(bg_wheel_y + (bdr_s*1.5)), img_wheel_size, img_wheel_size); - nvgFillPaint(s->vg, imgPaint); + nvgBeginPath(s->vg); + NVGpaint imgPaint = nvgImagePattern(s->vg, img_wheel_x-bg_wheel_x, img_wheel_y-(bg_wheel_y + (bdr_s*1.5)), + img_wheel_size, img_wheel_size, 0, s->img_wheel, img_wheel_alpha); + nvgRect(s->vg, img_wheel_x-bg_wheel_x, img_wheel_y-(bg_wheel_y + (bdr_s*1.5)), img_wheel_size, img_wheel_size); + nvgFillPaint(s->vg, imgPaint); nvgFill(s->vg); - nvgRestore(s->vg); + nvgRestore(s->vg); } } diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc index b1824c84e2d045..dcee8f6328e2a3 100644 --- a/selfdrive/ui/ui.cc +++ b/selfdrive/ui/ui.cc @@ -275,6 +275,7 @@ void handle_message(UIState *s, Message * msg) { s->scene.v_cruise_update_ts = eventd.logMonoTime; } s->scene.v_cruise = datad.vCruise; + s->scene.angleSteers = datad.angleSteers; s->scene.v_ego = datad.vEgo; s->scene.curvature = datad.curvature; s->scene.engaged = datad.enabled; diff --git a/selfdrive/ui/ui.hpp b/selfdrive/ui/ui.hpp index 1dcf546bbcfcb3..c24db61035e900 100644 --- a/selfdrive/ui/ui.hpp +++ b/selfdrive/ui/ui.hpp @@ -85,7 +85,9 @@ typedef struct UIScene { uint64_t v_cruise_update_ts; float v_ego; bool decel_for_model; - + + float angleSteers; + float speedlimit; float speedlimitaheaddistance; bool speedlimitahead_valid;