diff --git a/cereal/car.capnp b/cereal/car.capnp index 5c8c339b7..e6121ca76 100644 --- a/cereal/car.capnp +++ b/cereal/car.capnp @@ -383,6 +383,8 @@ struct CarParams { enableApgs @6 :Bool; # advanced parking guidance system enableBsm @56 :Bool; # blind spot monitoring flags @64 :UInt32; # flags for car specific quirks + #enable torque interceptor + enableTorqueInterceptor @65 :Bool; minEnableSpeed @7 :Float32; minSteerSpeed @8 :Float32; diff --git a/cereal/log.capnp b/cereal/log.capnp index 473a9f6b8..f56171eb4 100644 --- a/cereal/log.capnp +++ b/cereal/log.capnp @@ -395,6 +395,7 @@ struct PandaState @0xa7649e2575e4591e { faults @18 :List(FaultType); harnessStatus @21 :HarnessStatus; heartbeatLost @22 :Bool; + torqueInterceptorDetected @23 :Bool; enum FaultStatus { none @0; diff --git a/opendbc/mazda_2017.dbc b/opendbc/mazda_2017.dbc index fdd6d2824..b05fef5f5 100644 --- a/opendbc/mazda_2017.dbc +++ b/opendbc/mazda_2017.dbc @@ -167,6 +167,21 @@ BO_ 581 CAM_IDK3: 8 XXX SG_ S8 : 48|8@1+ (1,0) [0|255] "" XXX SG_ S9 : 56|8@1+ (1,0) [0|255] "" XXX +BO_ 585 CAM_LKAS2: 8 XXX + SG_ LKAS_REQUEST : 3|12@0+ (1,-2048) [0|2048] "" XXX + SG_ CHKSUM : 19|12@0+ (1,-2048) [0|2048] "" XXX + SG_ KEY : 39|32@0+ (1,0) [3294744159|3294744161] "" XXX + +BO_ 586 TI_FEEDBACK: 8 XXX + SG_ TI_TORQUE_SENSOR : 7|8@0+ (1,-127) [-85|85] "" XXX + SG_ CHKSUM : 15|8@0+ (1,-127) [-127|128] "" XXX + SG_ VERSION_NUMBER : 23|8@0+ (1,0) [0|255] "" XXX + SG_ STATE : 31|8@0+ (1,0) [0|3] "" XXX + SG_ VIOL : 39|8@0+ (1,0) [0|255] "" XXX + SG_ ERROR : 47|8@0+ (1,0) [0|255] "" XXX + SG_ RAMP_DOWN : 55|8@0+ (1,0) [0|1] "" XXX + SG_ SPARE : 63|8@0+ (1,0) [0|255] "" XXX + BO_ 863 CAM_TRAFFIC_SIGNS: 8 XXX SG_ NEW_SIGNAL_3 : 55|1@0+ (1,0) [0|127] "" XXX SG_ FORWARD_COLLISION : 40|8@1+ (1,0) [0|7] "" XXX diff --git a/panda/board/main.c b/panda/board/main.c index ac7a75a5e..074a77f2d 100644 --- a/panda/board/main.c +++ b/panda/board/main.c @@ -48,6 +48,7 @@ struct __attribute__((packed)) health_t { uint8_t fault_status_pkt; uint8_t power_save_enabled_pkt; uint8_t heartbeat_lost_pkt; + uint8_t torque_interceptor_detected_pkt; }; @@ -172,6 +173,7 @@ int get_health_pkt(void *dat) { health->controls_allowed_pkt = controls_allowed; health->gas_interceptor_detected_pkt = gas_interceptor_detected; + health->torque_interceptor_detected_pkt = torque_interceptor_detected; health->can_rx_errs_pkt = can_rx_errs; health->can_send_errs_pkt = can_send_errs; health->can_fwd_errs_pkt = can_fwd_errs; diff --git a/panda/board/safety.h b/panda/board/safety.h index 1f1bcba72..92147a1de 100644 --- a/panda/board/safety.h +++ b/panda/board/safety.h @@ -278,6 +278,7 @@ int set_safety_hooks(uint16_t mode, int16_t param) { ts_angle_last = 0; desired_angle_last = 0; ts_last = 0; + torque_interceptor_detected = false; torque_meas.max = 0; torque_meas.max = 0; diff --git a/panda/board/safety/safety_mazda.h b/panda/board/safety/safety_mazda.h index 48632f830..e3d05df72 100644 --- a/panda/board/safety/safety_mazda.h +++ b/panda/board/safety/safety_mazda.h @@ -1,8 +1,10 @@ // CAN msgs we care about #define MAZDA_LKAS 0x243 +#define MAZDA_LKAS2 0x249 #define MAZDA_LKAS_HUD 0x440 #define MAZDA_CRZ_CTRL 0x21c #define MAZDA_CRZ_BTNS 0x09d +#define TI_STEER_TORQUE 0x24A #define MAZDA_STEER_TORQUE 0x240 #define MAZDA_ENGINE_DATA 0x202 #define MAZDA_PEDALS 0x165 @@ -24,7 +26,7 @@ #define MAZDA_DRIVER_TORQUE_FACTOR 1 #define MAZDA_MAX_TORQUE_ERROR 350 -const CanMsg MAZDA_TX_MSGS[] = {{MAZDA_LKAS, 0, 8}, {MAZDA_CRZ_BTNS, 0, 8}, {MAZDA_LKAS_HUD, 0, 8}}; +const CanMsg MAZDA_TX_MSGS[] = {{MAZDA_LKAS, 0, 8}, {MAZDA_CRZ_BTNS, 0, 8}, {MAZDA_LKAS2, 0, 8}, {MAZDA_LKAS_HUD, 0, 8}}; AddrCheckStruct mazda_addr_checks[] = { {.msg = {{MAZDA_CRZ_CTRL, 0, 8, .expected_timestep = 20000U}, { 0 }, { 0 }}}, @@ -36,10 +38,24 @@ AddrCheckStruct mazda_addr_checks[] = { #define MAZDA_ADDR_CHECKS_LEN (sizeof(mazda_addr_checks) / sizeof(mazda_addr_checks[0])) addr_checks mazda_rx_checks = {mazda_addr_checks, MAZDA_ADDR_CHECKS_LEN}; +AddrCheckStruct mazda_ti_addr_checks[] = { + {.msg = {{TI_STEER_TORQUE, 0, 8, .expected_timestep = 12000U}}}, + // TI_STEER_TORQUE expected_timestep should be the same as the tx rate of MAZDA_LKAS2 +}; +#define MAZDA_TI_ADDR_CHECKS_LEN (sizeof(mazda_ti_addr_checks) / sizeof(mazda_ti_addr_checks[0])) +addr_checks mazda_ti_rx_checks = {mazda_ti_addr_checks, MAZDA_TI_ADDR_CHECKS_LEN}; + // track msgs coming from OP so that we know what CAM msgs to drop and what to forward static int mazda_rx_hook(CANPacket_t *to_push) { bool valid = addr_safety_check(to_push, &mazda_rx_checks, NULL, NULL, NULL); - if (valid && ((int)GET_BUS(to_push) == MAZDA_MAIN)) { + + if (((GET_ADDR(to_push) == TI_STEER_TORQUE)) && + ((GET_BYTE(to_push, 0) == GET_BYTE(to_push, 1)))) { + torque_interceptor_detected = 1; + valid &= addr_safety_check(to_push, &mazda_ti_rx_checks, NULL, NULL, NULL); + } + + if (valid && (GET_BUS(to_push) == MAZDA_MAIN)) { int addr = GET_ADDR(to_push); if (addr == MAZDA_ENGINE_DATA) { @@ -48,10 +64,16 @@ static int mazda_rx_hook(CANPacket_t *to_push) { vehicle_moving = speed > 10; // moving when speed > 0.1 kph } - if (addr == MAZDA_STEER_TORQUE) { - int torque_driver_new = GET_BYTE(to_push, 0) - 127U; - // update array of samples - update_sample(&torque_driver, torque_driver_new); + if (torque_interceptor_detected) { + if (addr == TI_STEER_TORQUE) { + int torque_driver_new = GET_BYTE(to_push, 0) - 126; + update_sample(&torque_driver, torque_driver_new); + } + }else{ + if (addr == MAZDA_STEER_TORQUE) { + int torque_driver_new = GET_BYTE(to_push, 0) - 127; + update_sample(&torque_driver, torque_driver_new); + } } // enter controls on rising edge of ACC, exit controls on ACC off @@ -175,6 +197,7 @@ static const addr_checks* mazda_init(int16_t param) { UNUSED(param); controls_allowed = false; relay_malfunction_reset(); + torque_interceptor_detected = 0; return &mazda_rx_checks; } diff --git a/panda/board/safety_declarations.h b/panda/board/safety_declarations.h index 11438a486..1a0871561 100644 --- a/panda/board/safety_declarations.h +++ b/panda/board/safety_declarations.h @@ -112,6 +112,7 @@ bool cruise_engaged_prev = false; float vehicle_speed = 0; bool vehicle_moving = false; bool acc_main_on = false; // referred to as "ACC off" in ISO 15622:2018 +bool torque_interceptor_detected = false; // for safety modes with torque steering control int desired_torque_last = 0; // last desired steer torque diff --git a/panda/python/__init__.py b/panda/python/__init__.py index 87c811fea..28dbc30be 100644 --- a/panda/python/__init__.py +++ b/panda/python/__init__.py @@ -434,8 +434,8 @@ class Panda(object): @ensure_health_packet_version def health(self): - dat = self._handle.controlRead(Panda.REQUEST_IN, 0xd2, 0, 0, 44) - a = struct.unpack(" &pandas, bool s ps.setPowerSaveEnabled((bool)(pandaState.power_save_enabled)); ps.setHeartbeatLost((bool)(pandaState.heartbeat_lost)); ps.setHarnessStatus(cereal::PandaState::HarnessStatus(pandaState.car_harness_status)); + ps.setTorqueInterceptorDetected(pandaState.torque_interceptor_detected); + // Convert faults bitset to capnp list std::bitset fault_bits(pandaState.faults); diff --git a/selfdrive/boardd/panda.h b/selfdrive/boardd/panda.h index fe6918948..f8dd37e25 100644 --- a/selfdrive/boardd/panda.h +++ b/selfdrive/boardd/panda.h @@ -45,6 +45,7 @@ struct __attribute__((packed)) health_t { uint8_t fault_status; uint8_t power_save_enabled; uint8_t heartbeat_lost; + uint8_t torque_interceptor_detected; }; struct __attribute__((packed)) can_header { diff --git a/selfdrive/car/__init__.py b/selfdrive/car/__init__.py index 408e0d075..c3280e886 100644 --- a/selfdrive/car/__init__.py +++ b/selfdrive/car/__init__.py @@ -44,6 +44,25 @@ def scale_tire_stiffness(mass, wheelbase, center_to_front, tire_stiffness_factor def dbc_dict(pt_dbc, radar_dbc, chassis_dbc=None, body_dbc=None): return {'pt': pt_dbc, 'radar': radar_dbc, 'chassis': chassis_dbc, 'body': body_dbc} +#alternate settings when using torque interceptor. May or may not be useful to some users/branches. +def apply_ti_steer_torque_limits(apply_torque, apply_torque_last, driver_torque, LIMITS): + + # limits due to driver torque + driver_max_torque = LIMITS.TI_STEER_MAX + (LIMITS.TI_STEER_DRIVER_ALLOWANCE + driver_torque * LIMITS.TI_STEER_DRIVER_FACTOR) * LIMITS.TI_STEER_DRIVER_MULTIPLIER + driver_min_torque = -LIMITS.TI_STEER_MAX + (-LIMITS.TI_STEER_DRIVER_ALLOWANCE + driver_torque * LIMITS.TI_STEER_DRIVER_FACTOR) * LIMITS.TI_STEER_DRIVER_MULTIPLIER + max_steer_allowed = max(min(LIMITS.TI_STEER_MAX, driver_max_torque), 0) + min_steer_allowed = min(max(-LIMITS.TI_STEER_MAX, driver_min_torque), 0) + apply_torque = clip(apply_torque, min_steer_allowed, max_steer_allowed) + + # slow rate if steer torque increases in magnitude + if apply_torque_last > 0: + apply_torque = clip(apply_torque, max(apply_torque_last - LIMITS.TI_STEER_DELTA_DOWN, -LIMITS.TI_STEER_DELTA_UP), + apply_torque_last + LIMITS.TI_STEER_DELTA_UP) + else: + apply_torque = clip(apply_torque, apply_torque_last - LIMITS.TI_STEER_DELTA_UP, + min(apply_torque_last + LIMITS.TI_STEER_DELTA_DOWN, LIMITS.TI_STEER_DELTA_UP)) + + return int(round(float(apply_torque))) def apply_std_steer_torque_limits(apply_torque, apply_torque_last, driver_torque, LIMITS): diff --git a/selfdrive/car/car_helpers.py b/selfdrive/car/car_helpers.py index 786b00fa2..f1e0caa96 100644 --- a/selfdrive/car/car_helpers.py +++ b/selfdrive/car/car_helpers.py @@ -9,8 +9,12 @@ from selfdrive.swaglog import cloudlog import cereal.messaging as messaging from selfdrive.car import gen_empty_fingerprint +from selfdrive import global_ti + from cereal import car +from cereal import log EventName = car.CarEvent.EventName +DynamicParam = log.PandaState def get_startup_event(car_recognized, controller_available, fw_seen): @@ -163,17 +167,23 @@ def fingerprint(logcan, sendcan): cloudlog.event("fingerprinted", car_fingerprint=car_fingerprint, source=source, fuzzy=not exact_match, fw_count=len(car_fw)) + + global_ti.saved_candidate = car_fingerprint + global_ti.saved_finger = finger + return car_fingerprint, finger, vin, car_fw, source, exact_match def get_car(logcan, sendcan): candidate, fingerprints, vin, car_fw, source, exact_match = fingerprint(logcan, sendcan) - if candidate is None: cloudlog.warning("car doesn't match any fingerprints: %r", fingerprints) candidate = "mock" CarInterface, CarController, CarState = interfaces[candidate] + + global_ti.saved_CarInterface = CarInterface + car_params = CarInterface.get_params(candidate, fingerprints, car_fw) car_params.carVin = vin car_params.carFw = car_fw @@ -181,3 +191,11 @@ def get_car(logcan, sendcan): car_params.fuzzyFingerprint = not exact_match return CarInterface(car_params, CarController, CarState), car_params + +def get_ti(): + print("get_ti, entering get_params") + CarInterface = global_ti.saved_CarInterface + car_params = CarInterface.get_params(global_ti.saved_candidate, global_ti.saved_finger) + + return car_params + diff --git a/selfdrive/car/interfaces.py b/selfdrive/car/interfaces.py index bef3456a7..d354d8de8 100644 --- a/selfdrive/car/interfaces.py +++ b/selfdrive/car/interfaces.py @@ -98,8 +98,14 @@ class CarInterfaceBase(): ret.longitudinalTuning.kiV = [1.] ret.longitudinalActuatorDelayLowerBound = 0.15 ret.longitudinalActuatorDelayUpperBound = 0.15 + + # No Torque Interceptor by default + ret.enableTorqueInterceptor = False + return ret + + # returns a car.CarState, pass in car.CarControl def update(self, c, can_strings): raise NotImplementedError @@ -194,6 +200,7 @@ class CarStateBase: self.right_blinker_cnt = 0 self.left_blinker_prev = False self.right_blinker_prev = False + #self.tiAllowed = car.CarState. # Q = np.matrix([[10.0, 0.0], [0.0, 100.0]]) # R = 1e3 diff --git a/selfdrive/car/mazda/__init__.py b/selfdrive/car/mazda/__init__.py index e69de29bb..8b1378917 100644 --- a/selfdrive/car/mazda/__init__.py +++ b/selfdrive/car/mazda/__init__.py @@ -0,0 +1 @@ + diff --git a/selfdrive/car/mazda/carcontroller.py b/selfdrive/car/mazda/carcontroller.py index 06c5eb094..7b722f044 100644 --- a/selfdrive/car/mazda/carcontroller.py +++ b/selfdrive/car/mazda/carcontroller.py @@ -2,13 +2,14 @@ from cereal import car from opendbc.can.packer import CANPacker from selfdrive.car.mazda import mazdacan from selfdrive.car.mazda.values import CarControllerParams, Buttons -from selfdrive.car import apply_std_steer_torque_limits +from selfdrive.car import apply_std_steer_torque_limits, apply_ti_steer_torque_limits VisualAlert = car.CarControl.HUDControl.VisualAlert class CarController(): def __init__(self, dbc_name, CP, VM): self.apply_steer_last = 0 + self.ti_apply_steer_last = 0 self.packer = CANPacker(dbc_name) self.steer_rate_limited = False self.brake_counter = 0 @@ -17,14 +18,24 @@ class CarController(): can_sends = [] apply_steer = 0 + ti_apply_steer = 0 self.steer_rate_limited = False if c.enabled: # calculate steer and also set limits due to driver torque + if CS.CP.enableTorqueInterceptor: + if CS.ti_lkas_allowed: + ti_new_steer = int(round(c.actuators.steer * CarControllerParams.TI_STEER_MAX)) + ti_apply_steer = apply_ti_steer_torque_limits(ti_new_steer, self.ti_apply_steer_last, + CS.out.steeringTorque, CarControllerParams) + else: + ti_apply_steer = ti_new_steer = 0 + new_steer = int(round(c.actuators.steer * CarControllerParams.STEER_MAX)) apply_steer = apply_std_steer_torque_limits(new_steer, self.apply_steer_last, - CS.out.steeringTorque, CarControllerParams) - self.steer_rate_limited = new_steer != apply_steer + CS.out.steeringTorque, CarControllerParams) + + self.steer_rate_limited = (ti_new_steer != ti_apply_steer) or (new_steer != apply_steer) if CS.out.standstill and frame % 5 == 0: # Mazda Stop and Go requires a RES button (or gas) press if the car stops more than 3 seconds @@ -46,6 +57,7 @@ class CarController(): self.brake_counter = 0 self.apply_steer_last = apply_steer + self.ti_apply_steer_last = ti_apply_steer # send HUD alerts if frame % 50 == 0: @@ -56,6 +68,12 @@ class CarController(): can_sends.append(mazdacan.create_alert_command(self.packer, CS.cam_laneinfo, ldw, steer_required)) # send steering command + + #The ti cannot be detected unless OP sends a can message to it becasue the ti only transmits when it + #sees the signature key in the designated address range. + can_sends.append(mazdacan.create_ti_steering_control(self.packer, CS.CP.carFingerprint,ti_apply_steer)) + # always send to the stock system can_sends.append(mazdacan.create_steering_control(self.packer, CS.CP.carFingerprint, - frame, apply_steer, CS.cam_lkas)) + frame, apply_steer, CS.cam_lkas)) + return can_sends diff --git a/selfdrive/car/mazda/carstate.py b/selfdrive/car/mazda/carstate.py index 7ebf60c96..524441fe8 100644 --- a/selfdrive/car/mazda/carstate.py +++ b/selfdrive/car/mazda/carstate.py @@ -3,7 +3,7 @@ from selfdrive.config import Conversions as CV from opendbc.can.can_define import CANDefine from opendbc.can.parser import CANParser from selfdrive.car.interfaces import CarStateBase -from selfdrive.car.mazda.values import DBC, LKAS_LIMITS, GEN1 +from selfdrive.car.mazda.values import DBC, LKAS_LIMITS, GEN1, TI_STATE class CarState(CarStateBase): def __init__(self, CP): @@ -17,6 +17,13 @@ class CarState(CarStateBase): self.low_speed_alert = False self.lkas_allowed_speed = False + self.ti_ramp_down = False + self.ti_version = 1 + self.ti_state = TI_STATE.RUN + self.ti_violation = 0 + self.ti_error = 0 + self.ti_lkas_allowed = False + def update(self, cp, cp_cam): ret = car.CarState.new_message() @@ -42,9 +49,23 @@ class CarState(CarStateBase): ret.leftBlinker, ret.rightBlinker = self.update_blinker_from_lamp(40, cp.vl["BLINK_INFO"]["LEFT_BLINK"] == 1, cp.vl["BLINK_INFO"]["RIGHT_BLINK"] == 1) - ret.steeringAngleDeg = cp.vl["STEER"]["STEER_ANGLE"] - ret.steeringTorque = cp.vl["STEER_TORQUE"]["STEER_TORQUE_SENSOR"] - ret.steeringPressed = abs(ret.steeringTorque) > LKAS_LIMITS.STEER_THRESHOLD + if self.CP.enableTorqueInterceptor: + ret.steeringTorque = cp.vl["TI_FEEDBACK"]["TI_TORQUE_SENSOR"] + + self.ti_version = cp.vl["TI_FEEDBACK"]["VERSION_NUMBER"] + self.ti_state = cp.vl["TI_FEEDBACK"]["STATE"] # DISCOVER = 0, OFF = 1, DRIVER_OVER = 2, RUN=3 + self.ti_violation = cp.vl["TI_FEEDBACK"]["VIOL"] # 0 = no violation + self.ti_error = cp.vl["TI_FEEDBACK"]["ERROR"] # 0 = no error + if self.ti_version > 1: + self.ti_ramp_down = (cp.vl["TI_FEEDBACK"]["RAMP_DOWN"] == 1) + + ret.steeringPressed = abs(ret.steeringTorque) > LKAS_LIMITS.TI_STEER_THRESHOLD + self.ti_lkas_allowed = not self.ti_ramp_down and self.ti_state == TI_STATE.RUN + else: + ret.steeringTorque = cp.vl["STEER_TORQUE"]["STEER_TORQUE_SENSOR"] + ret.steeringPressed = abs(ret.steeringTorque) > LKAS_LIMITS.STEER_THRESHOLD + + ret.steeringAngleDeg = cp.vl["STEER"]["STEER_ANGLE"] ret.steeringTorqueEps = cp.vl["STEER_TORQUE"]["STEER_TORQUE_MOTOR"] ret.steeringRateDeg = cp.vl["STEER_RATE"]["STEER_ANGLE_RATE"] @@ -115,7 +136,6 @@ class CarState(CarStateBase): ("RL", "WHEEL_SPEEDS", 0), ("RR", "WHEEL_SPEEDS", 0), ] - checks = [ # sig_address, frequency ("BLINK_INFO", 10), @@ -124,7 +144,6 @@ class CarState(CarStateBase): ("STEER_TORQUE", 83), ("WHEEL_SPEEDS", 100), ] - if CP.carFingerprint in GEN1: signals += [ ("LKAS_BLOCK", "STEER_RATE", 0), @@ -161,7 +180,22 @@ class CarState(CarStateBase): ("GEAR", 20), ("BSM", 10), ] + # get real driver torque if we are using a torque interceptor + if CP.enableTorqueInterceptor: + signals += [ + ("TI_TORQUE_SENSOR", "TI_FEEDBACK", 0), + ("CHKSUM", "TI_FEEDBACK", 0), + ("VERSION_NUMBER", "TI_FEEDBACK", 0), + ("STATE", "TI_FEEDBACK", 0), + ("VIOL", "TI_FEEDBACK", 0), + ("ERROR", "TI_FEEDBACK", 0), + ("RAMP_DOWN", "TI_FEEDBACK", 0), + ] + checks += [ + ("TI_FEEDBACK", 100), + ] + return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, 0) @staticmethod diff --git a/selfdrive/car/mazda/interface.py b/selfdrive/car/mazda/interface.py index 8364bf000..995ce3e6a 100755 --- a/selfdrive/car/mazda/interface.py +++ b/selfdrive/car/mazda/interface.py @@ -4,6 +4,7 @@ from selfdrive.config import Conversions as CV from selfdrive.car.mazda.values import CAR, LKAS_LIMITS from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint, get_safety_config from selfdrive.car.interfaces import CarInterfaceBase +from selfdrive import global_ti as TI ButtonType = car.CarState.ButtonEvent.Type EventName = car.CarEvent.EventName @@ -16,47 +17,114 @@ class CarInterface(CarInterfaceBase): @staticmethod def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None): + print("in get_params, entering get_std_params") ret = CarInterfaceBase.get_std_params(candidate, fingerprint) ret.carName = "mazda" + ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.mazda)] ret.radarOffCan = True - ret.dashcamOnly = candidate not in [CAR.CX9_2021] + ret.dashcamOnly = False # candidate not in [CAR.CX9_2021] + + #ret.enableTorqueInterceptor = 0x24A in fingerprint[0] + if ret.enableTorqueInterceptor: + print("Recieving torque interceptor signal.") ret.steerActuatorDelay = 0.1 ret.steerRateCost = 1.0 ret.steerLimitTimer = 0.8 tire_stiffness_factor = 0.70 # not optimized yet - if candidate == CAR.CX5: - ret.mass = 3655 * CV.LB_TO_KG + STD_CARGO_KG - ret.wheelbase = 2.7 - ret.steerRatio = 15.5 - ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] - ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.19], [0.019]] - ret.lateralTuning.pid.kf = 0.00006 - elif candidate in [CAR.CX9, CAR.CX9_2021]: - ret.mass = 4217 * CV.LB_TO_KG + STD_CARGO_KG - ret.wheelbase = 3.1 - ret.steerRatio = 17.6 - ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] - ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.19], [0.019]] - ret.lateralTuning.pid.kf = 0.00006 - elif candidate == CAR.MAZDA3: - ret.mass = 2875 * CV.LB_TO_KG + STD_CARGO_KG - ret.wheelbase = 2.7 - ret.steerRatio = 14.0 - ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] - ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.19], [0.019]] - ret.lateralTuning.pid.kf = 0.00006 - elif candidate == CAR.MAZDA6: - ret.mass = 3443 * CV.LB_TO_KG + STD_CARGO_KG - ret.wheelbase = 2.83 - ret.steerRatio = 15.5 - ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] - ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.19], [0.019]] - ret.lateralTuning.pid.kf = 0.00006 + if ret.enableTorqueInterceptor: + print("Adjusting PID parameters for TI") + if candidate == CAR.CX5: + ret.mass = 3655 * CV.LB_TO_KG + STD_CARGO_KG + ret.wheelbase = 2.7 + ret.steerRatio = 15.5 + ret.lateralTuning.pid.kiBP = [5.0, 25.0] + ret.lateralTuning.pid.kpBP = [5.0, 25.0] + ret.lateralTuning.pid.kpV = [0.25,0.28] + ret.lateralTuning.pid.kiV = [0.01,0.025] + ret.lateralTuning.pid.kf = 0.00008 + + ret.lateralTuning.init('indi') + ret.lateralTuning.indi.innerLoopGainBP = [5.0, 35] + ret.lateralTuning.indi.innerLoopGainV = [4.5, 6.0] + ret.lateralTuning.indi.outerLoopGainBP = [5, 35] + ret.lateralTuning.indi.outerLoopGainV = [3.0, 6] + ret.lateralTuning.indi.timeConstantBP = [2, 35] + ret.lateralTuning.indi.timeConstantV = [0.2, 1.5] + ret.lateralTuning.indi.actuatorEffectivenessBP = [0, 25] + ret.lateralTuning.indi.actuatorEffectivenessV = [2.0, 1] + + elif candidate in [CAR.CX9, CAR.CX9_2021]: + ret.mass = 4217 * CV.LB_TO_KG + STD_CARGO_KG + ret.wheelbase = 3.1 + ret.steerRatio = 17.6 + ret.lateralTuning.pid.kiBP = [8.0, 30.0] + ret.lateralTuning.pid.kpBP = [8.0, 30.0] + ret.lateralTuning.pid.kpV = [0.10,0.22] + ret.lateralTuning.pid.kiV = [0.01,0.019] + ret.lateralTuning.pid.kf = 0.00006 + elif candidate == CAR.MAZDA3: + ret.mass = 2875 * CV.LB_TO_KG + STD_CARGO_KG + ret.wheelbase = 2.7 + ret.steerRatio = 14.0 + + ret.lateralTuning.pid.kiBP = [5.0, 25.0] + ret.lateralTuning.pid.kpBP = [5.0, 25.0] + ret.lateralTuning.pid.kpV = [0.25,0.28] + ret.lateralTuning.pid.kiV = [0.01,0.025] + ret.lateralTuning.pid.kf = 0.00008 + + ret.lateralTuning.init('indi') + ret.lateralTuning.indi.innerLoopGainBP = [5.0, 35] + ret.lateralTuning.indi.innerLoopGainV = [4.5, 6.0] + ret.lateralTuning.indi.outerLoopGainBP = [5, 35] + ret.lateralTuning.indi.outerLoopGainV = [3.0, 6] + ret.lateralTuning.indi.timeConstantBP = [2, 35] + ret.lateralTuning.indi.timeConstantV = [0.2, 1.5] + ret.lateralTuning.indi.actuatorEffectivenessBP = [0, 25] + ret.lateralTuning.indi.actuatorEffectivenessV = [2.0, 1] + elif candidate == CAR.MAZDA6: + ret.mass = 3443 * CV.LB_TO_KG + STD_CARGO_KG + ret.wheelbase = 2.83 + ret.steerRatio = 15.5 + ret.lateralTuning.pid.kiBP = [8.0, 30.0] + ret.lateralTuning.pid.kpBP = [8.0, 30.0] + ret.lateralTuning.pid.kpV = [0.10,0.22] + ret.lateralTuning.pid.kiV = [0.01,0.019] + ret.lateralTuning.pid.kf = 0.00006 + else: + if candidate == CAR.CX5: + ret.mass = 3655 * CV.LB_TO_KG + STD_CARGO_KG + ret.wheelbase = 2.7 + ret.steerRatio = 15.5 + ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] + ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.19], [0.019]] + ret.lateralTuning.pid.kf = 0.00006 + elif candidate in [CAR.CX9, CAR.CX9_2021]: + ret.mass = 4217 * CV.LB_TO_KG + STD_CARGO_KG + ret.wheelbase = 3.1 + ret.steerRatio = 17.6 + ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] + ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.19], [0.019]] + ret.lateralTuning.pid.kf = 0.00006 + elif candidate == CAR.MAZDA3: + ret.mass = 2875 * CV.LB_TO_KG + STD_CARGO_KG + ret.wheelbase = 2.7 + ret.steerRatio = 14.0 + ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] + ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.19], [0.019]] + ret.lateralTuning.pid.kf = 0.00006 + elif candidate == CAR.MAZDA6: + ret.mass = 3443 * CV.LB_TO_KG + STD_CARGO_KG + ret.wheelbase = 2.83 + ret.steerRatio = 15.5 + ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] + ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.19], [0.019]] + ret.lateralTuning.pid.kf = 0.00006 # No steer below disable speed ret.minSteerSpeed = LKAS_LIMITS.DISABLE_SPEED * CV.KPH_TO_MS @@ -79,7 +147,9 @@ class CarInterface(CarInterfaceBase): self.cp.update_strings(can_strings) self.cp_cam.update_strings(can_strings) - + if self.CP.enableTorqueInterceptor and not TI.enabled: + TI.enabled = True + self.cp = self.CS.get_can_parser(self.CP) ret = self.CS.update(self.cp, self.cp_cam) ret.canValid = self.cp.can_valid and self.cp_cam.can_valid @@ -89,6 +159,9 @@ class CarInterface(CarInterfaceBase): if self.CS.low_speed_alert: events.add(EventName.belowSteerSpeed) + if not self.CS.acc_active_last and not self.CS.ti_lkas_allowed: + events.add(EventName.steerTempUnavailable) + ret.events = events.to_msg() self.CS.out = ret.as_reader() diff --git a/selfdrive/car/mazda/mazdacan.py b/selfdrive/car/mazda/mazdacan.py index e2ee93e02..cfec71620 100644 --- a/selfdrive/car/mazda/mazdacan.py +++ b/selfdrive/car/mazda/mazdacan.py @@ -17,8 +17,6 @@ def create_steering_control(packer, car_fingerprint, frame, apply_steer, lkas): ldw = 0 er2 = int(lkas["ERR_BIT_2"]) - # Some older models do have these, newer models don't. - # Either way, they all work just fine if set to zero. steering_angle = 0 b2 = 0 @@ -62,6 +60,20 @@ def create_steering_control(packer, car_fingerprint, frame, apply_steer, lkas): return packer.make_can_msg("CAM_LKAS", 0, values) +def create_ti_steering_control(packer, car_fingerprint, apply_steer): + + key = 3294744160 + chksum = apply_steer + + if car_fingerprint in GEN1: + values = { + "LKAS_REQUEST" : apply_steer, + "CHKSUM" : chksum, + "KEY" : key + } + + return packer.make_can_msg("CAM_LKAS2", 0, values) + def create_alert_command(packer, cam_msg: dict, ldw: bool, steer_required: bool): values = copy.copy(cam_msg) diff --git a/selfdrive/car/mazda/values.py b/selfdrive/car/mazda/values.py index 58e1dd703..08c958c9c 100644 --- a/selfdrive/car/mazda/values.py +++ b/selfdrive/car/mazda/values.py @@ -4,18 +4,31 @@ from selfdrive.car import dbc_dict from cereal import car Ecu = car.CarParams.Ecu - # Steer torque limits - class CarControllerParams: - STEER_MAX = 800 # theoretical max_steer 2047 + STEER_MAX = 600 # theoretical max_steer 2047 STEER_DELTA_UP = 10 # torque increase per refresh STEER_DELTA_DOWN = 25 # torque decrease per refresh - STEER_DRIVER_ALLOWANCE = 15 # allowed driver torque before start limiting - STEER_DRIVER_MULTIPLIER = 1 # weight driver torque + STEER_DRIVER_ALLOWANCE = 5 # allowed driver torque before start limiting + STEER_DRIVER_MULTIPLIER = 40 # weight driver torque STEER_DRIVER_FACTOR = 1 # from dbc STEER_ERROR_MAX = 350 # max delta between torque cmd and torque motor + TI_STEER_MAX = 600 # theoretical max_steer 2047 + TI_STEER_DELTA_UP = 6 # torque increase per refresh + TI_STEER_DELTA_DOWN = 15 # torque decrease per refresh + TI_STEER_DRIVER_ALLOWANCE = 5 # allowed driver torque before start limiting + TI_STEER_DRIVER_MULTIPLIER = 40 # weight driver torque + TI_STEER_DRIVER_FACTOR = 1 # from dbc + TI_STEER_ERROR_MAX = 350 # max delta between torque cmd and torque motor + + +class TI_STATE: + DISCOVER = 0 + OFF = 1 + DRIVER_OVER = 2 + RUN = 3 + class CAR: CX5 = "MAZDA CX-5" CX9 = "MAZDA CX-9" @@ -25,8 +38,12 @@ class CAR: class LKAS_LIMITS: STEER_THRESHOLD = 15 - DISABLE_SPEED = 45 # kph - ENABLE_SPEED = 52 # kph + DISABLE_SPEED = 0 # kph + ENABLE_SPEED = 0 # kph + TI_STEER_THRESHOLD = 15 + TI_DISABLE_SPEED = 0 # kph + TI_ENABLE_SPEED = 0 # kph + class Buttons: NONE = 0 @@ -34,7 +51,7 @@ class Buttons: SET_MINUS = 2 RESUME = 3 CANCEL = 4 - + TURN_ON = 5 FW_VERSIONS = { CAR.CX5: { diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 19fe328db..c25f5a870 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -12,7 +12,7 @@ import cereal.messaging as messaging from selfdrive.config import Conversions as CV from selfdrive.swaglog import cloudlog from selfdrive.boardd.boardd import can_list_to_can_capnp -from selfdrive.car.car_helpers import get_car, get_startup_event, get_one_can +from selfdrive.car.car_helpers import get_car, get_startup_event, get_one_can, get_ti from selfdrive.controls.lib.lane_planner import CAMERA_OFFSET from selfdrive.controls.lib.drive_helpers import update_v_cruise, initialize_v_cruise from selfdrive.controls.lib.drive_helpers import get_lag_adjusted_curvature @@ -94,6 +94,8 @@ class Controls: print("Waiting for CAN messages...") get_one_can(self.can_sock) + self.ti_ready = False + self.CI, self.CP = get_car(self.can_sock, self.pm.sock['sendcan']) # read params @@ -237,6 +239,8 @@ class Controls: else: self.events.add(EventName.calibrationInvalid) + + # Handle lane change if self.sm['lateralPlan'].laneChangeState == LaneChangeState.preLaneChange: direction = self.sm['lateralPlan'].laneChangeDirection @@ -267,7 +271,14 @@ class Controls: if log.PandaState.FaultType.relayMalfunction in pandaState.faults: self.events.add(EventName.relayMalfunction) + if pandaState.torqueInterceptorDetected and not self.ti_ready: + self.ti_ready = True + self.CP.enableTorqueInterceptor = True + #Update CP based on torque_interceptor_ready + self.CP = get_ti() + # Check for HW or system issues + if len(self.sm['radarState'].radarErrors): self.events.add(EventName.radarFault) elif not self.sm.valid["pandaStates"]: diff --git a/selfdrive/global_ti.py b/selfdrive/global_ti.py new file mode 100644 index 000000000..269db824a --- /dev/null +++ b/selfdrive/global_ti.py @@ -0,0 +1,11 @@ +#!/usr/bin/env python3 +from selfdrive.car import gen_empty_fingerprint + + +global saved_candidate +saved_candidate = {} +global saved_finger +saved_finger = gen_empty_fingerprint() +global saved_CarInterface +global enabled +enabled = False