From 0594f00d61c1bf1be95ec6a8d8521d575f8b497e Mon Sep 17 00:00:00 2001 From: Jafar Al-Gharaibeh Date: Thu, 28 May 2020 00:13:14 -0500 Subject: [PATCH] Improve acc stock and go by removing the 3 seconds limit Signed-off-by: Jafar Al-Gharaibeh --- selfdrive/car/mazda/carcontroller.py | 18 +++++++++++++----- selfdrive/car/mazda/carstate.py | 13 ++++++------- selfdrive/car/mazda/interface.py | 11 +++++++---- selfdrive/car/mazda/mazdacan.py | 23 +++++++++++++++++------ selfdrive/car/mazda/values.py | 7 +++++++ 5 files changed, 50 insertions(+), 22 deletions(-) diff --git a/selfdrive/car/mazda/carcontroller.py b/selfdrive/car/mazda/carcontroller.py index 2270e83fe1..6649e76295 100644 --- a/selfdrive/car/mazda/carcontroller.py +++ b/selfdrive/car/mazda/carcontroller.py @@ -1,5 +1,5 @@ from selfdrive.car.mazda import mazdacan -from selfdrive.car.mazda.values import SteerLimitParams +from selfdrive.car.mazda.values import SteerLimitParams, Buttons from opendbc.can.packer import CANPacker from selfdrive.car import apply_std_steer_torque_limits @@ -9,7 +9,8 @@ class CarController(): self.packer = CANPacker(dbc_name) self.steer_rate_limited = False - def update(self, enabled, CS, frame, actuators): + def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd, visual_alert, + left_lane, right_lane, left_lane_depart, right_lane_depart): """ Controls thread """ can_sends = [] @@ -22,11 +23,18 @@ class CarController(): apply_steer = apply_std_steer_torque_limits(new_steer, self.apply_steer_last, CS.out.steeringTorque, SteerLimitParams) self.steer_rate_limited = new_steer != apply_steer + + if CS.out.standstill and frame % 50 == 0: + # Mazda Stop and Go requires a RES button (or gas) press if the car stops more than 3 seconds + # Send Resume button at 2hz if we're engaged at standstill to support full stop and go! + # TODO: improve the resume trigger logic by looking at actual radar data + can_sends.append(mazdacan.create_cancel_acc(self.packer, CS.CP.carFingerprint, Buttons.RESUME)) else: apply_steer = 0 - self.steer_rate_limited = False - if CS.acc_active and frame % 10 == 0: - can_sends.append(mazdacan.create_cancel_acc(self.packer, CS.CP.carFingerprint)) + if CS.out.cruiseState.enabled and frame % 10 == 0: + # Cancel Stock ACC if it's engaged with OP disengaged + # Match stock message rate which is sent at 10hz + can_sends.append(mazdacan.create_cancel_acc(self.packer, CS.CP.carFingerprint, Buttons.CANCEL)) self.apply_steer_last = apply_steer diff --git a/selfdrive/car/mazda/carstate.py b/selfdrive/car/mazda/carstate.py index 25d2bc0f53..16e54e3f29 100644 --- a/selfdrive/car/mazda/carstate.py +++ b/selfdrive/car/mazda/carstate.py @@ -75,12 +75,11 @@ class CarState(CarStateBase): cp.vl["CRZ_BTNS"]['SET_M']]): self.cruise_speed = ret.vEgoRaw - ret.cruiseState.available = cp.vl["CRZ_CTRL"]['CRZ_ACTIVE'] == 1 - self.acc_active = ret.cruiseState.available - ret.cruiseState.enabled = self.acc_active + ret.cruiseState.available = True + ret.cruiseState.enabled = cp.vl["CRZ_CTRL"]['CRZ_ACTIVE'] == 1 ret.cruiseState.speed = self.cruise_speed - if self.acc_active: + if ret.cruiseState.enabled: if not self.lkas_on: if not self.acc_active_last: self.low_speed_lockout = True @@ -92,7 +91,7 @@ class CarState(CarStateBase): ret.steerWarning = handsoff - self.acc_active_last = self.acc_active + self.acc_active_last = ret.cruiseState.enabled self.cam_lkas = cp_cam.vl["CAM_LKAS"] ret.steerError = cp_cam.vl["CAM_LKAS"]['ERR_BIT_1'] == 1 @@ -170,14 +169,14 @@ class CarState(CarStateBase): if CP.carFingerprint == CAR.CX5: signals += [ # sig_name, sig_address, default - ("LKAS_REQUEST", "CAM_LKAS", 2048), + ("LKAS_REQUEST", "CAM_LKAS", 0), ("CTR", "CAM_LKAS", 0), ("ERR_BIT_1", "CAM_LKAS", 0), ("LINE_NOT_VISIBLE", "CAM_LKAS", 0), ("LDW", "CAM_LKAS", 0), ("BIT_1", "CAM_LKAS", 1), ("ERR_BIT_2", "CAM_LKAS", 0), - ("STEERING_ANGLE", "CAM_LKAS", 2048), + ("STEERING_ANGLE", "CAM_LKAS", 0), ("ANGLE_ENABLED", "CAM_LKAS", 0), ("CHKSUM", "CAM_LKAS", 0), ] diff --git a/selfdrive/car/mazda/interface.py b/selfdrive/car/mazda/interface.py index a15c5e19ef..82c7005a1f 100755 --- a/selfdrive/car/mazda/interface.py +++ b/selfdrive/car/mazda/interface.py @@ -1,7 +1,7 @@ #!/usr/bin/env python3 from cereal import car from selfdrive.config import Conversions as CV -from selfdrive.car.mazda.values import CAR, FINGERPRINTS, ECU_FINGERPRINT, Ecu +from selfdrive.car.mazda.values import CAR, LKAS_LIMITS, FINGERPRINTS, ECU_FINGERPRINT, Ecu from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint, is_ecu_disconnected from selfdrive.car.interfaces import CarInterfaceBase @@ -40,8 +40,8 @@ class CarInterface(CarInterfaceBase): ret.lateralTuning.pid.kf = 0.00006 - # No steer below 45kph - ret.minSteerSpeed = 45 * CV.KPH_TO_MS + # No steer below disable speed + ret.minSteerSpeed = LKAS_LIMITS.DISABLE_SPEED * CV.KPH_TO_MS ret.centerToFront = ret.wheelbase * 0.41 @@ -86,6 +86,9 @@ class CarInterface(CarInterfaceBase): return self.CS.out def apply(self, c): - can_sends = self.CC.update(c.enabled, self.CS, self.frame, c.actuators) + can_sends = self.CC.update(c.enabled, self.CS, self.frame, c.actuators, + c.cruiseControl.cancel, c.hudControl.visualAlert, + c.hudControl.leftLaneVisible, c.hudControl.rightLaneVisible, + c.hudControl.leftLaneDepart, c.hudControl.rightLaneDepart) self.frame += 1 return can_sends diff --git a/selfdrive/car/mazda/mazdacan.py b/selfdrive/car/mazda/mazdacan.py index a768f9f15f..9baaeb00a5 100644 --- a/selfdrive/car/mazda/mazdacan.py +++ b/selfdrive/car/mazda/mazdacan.py @@ -1,4 +1,4 @@ -from selfdrive.car.mazda.values import CAR +from selfdrive.car.mazda.values import CAR, Buttons def create_steering_control(packer, car_fingerprint, frame, apply_steer, lkas): @@ -57,17 +57,28 @@ def create_steering_control(packer, car_fingerprint, frame, apply_steer, lkas): return packer.make_can_msg("CAM_LKAS", 0, values) -def create_cancel_acc(packer, car_fingerprint): +def create_cancel_acc(packer, car_fingerprint, button): + + if button == Buttons.CANCEL: + can = 1 + res = 0 + elif button == Buttons.RESUME: + can = 0 + res = 1 + else: + can = 0 + res = 0 + if car_fingerprint == CAR.CX5: values = { - "CAN_OFF" : 1, - "CAN_OFF_INV" : 0, + "CAN_OFF" : can, + "CAN_OFF_INV" : (can + 1) % 2, "SET_P" : 0, "SET_P_INV" : 1, - "RES" : 0, - "RES_INV" : 1, + "RES" : res, + "RES_INV" : (res + 1) % 2, "SET_M" : 0, "SET_M_INV" : 1, diff --git a/selfdrive/car/mazda/values.py b/selfdrive/car/mazda/values.py index 46aec77488..e69cb33138 100644 --- a/selfdrive/car/mazda/values.py +++ b/selfdrive/car/mazda/values.py @@ -22,6 +22,13 @@ class LKAS_LIMITS: DISABLE_SPEED = 45 #kph ENABLE_SPEED = 52 #kph +class Buttons: + NONE = 0 + SET_PLUS = 1 + SET_MINUS = 2 + RESUME = 3 + CANCEL = 4 + FINGERPRINTS = { CAR.CX5: [ # CX-5 2017 GT