From 8b6e56b6eac7a9a52ac1f6c78100b24ec718e4a7 Mon Sep 17 00:00:00 2001 From: Ryley Date: Thu, 21 Apr 2022 20:38:13 -0500 Subject: [PATCH] added radar intereceptor panda infrastructure. not able to detect yet --- cereal/car.capnp | 1 + cereal/log.capnp | 1 + panda/board/main.c | 2 ++ panda/board/safety.h | 3 ++- panda/board/safety/safety_mazda.h | 31 +++++++++++++++++++++++++------ panda/board/safety_declarations.h | 1 + panda/python/__init__.py | 5 +++-- selfdrive/boardd/boardd.cc | 1 + selfdrive/boardd/panda.h | 1 + selfdrive/car/car_helpers.py | 7 +++++++ selfdrive/car/interfaces.py | 3 ++- selfdrive/car/mazda/interface.py | 5 +++++ 12 files changed, 51 insertions(+), 10 deletions(-) diff --git a/cereal/car.capnp b/cereal/car.capnp index e6121ca76..499307429 100644 --- a/cereal/car.capnp +++ b/cereal/car.capnp @@ -385,6 +385,7 @@ struct CarParams { flags @64 :UInt32; # flags for car specific quirks #enable torque interceptor enableTorqueInterceptor @65 :Bool; + radarInterceptMode @66 :Bool; minEnableSpeed @7 :Float32; minSteerSpeed @8 :Float32; diff --git a/cereal/log.capnp b/cereal/log.capnp index f56171eb4..6b5dddfa7 100644 --- a/cereal/log.capnp +++ b/cereal/log.capnp @@ -396,6 +396,7 @@ struct PandaState @0xa7649e2575e4591e { harnessStatus @21 :HarnessStatus; heartbeatLost @22 :Bool; torqueInterceptorDetected @23 :Bool; + radarInterceptMode @24 :Bool; enum FaultStatus { none @0; diff --git a/panda/board/main.c b/panda/board/main.c index 074a77f2d..adb85f19b 100644 --- a/panda/board/main.c +++ b/panda/board/main.c @@ -49,6 +49,7 @@ struct __attribute__((packed)) health_t { uint8_t power_save_enabled_pkt; uint8_t heartbeat_lost_pkt; uint8_t torque_interceptor_detected_pkt; + uint8_t radar_intercept_mode_pkt; }; @@ -174,6 +175,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->radar_intercept_mode_pkt = radar_intercept_mode; 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 92147a1de..d81437bc3 100644 --- a/panda/board/safety.h +++ b/panda/board/safety.h @@ -279,7 +279,8 @@ int set_safety_hooks(uint16_t mode, int16_t param) { desired_angle_last = 0; ts_last = 0; torque_interceptor_detected = false; - + radar_intercept_mode = false; + torque_meas.max = 0; torque_meas.max = 0; torque_driver.min = 0; diff --git a/panda/board/safety/safety_mazda.h b/panda/board/safety/safety_mazda.h index e3d05df72..0756dd36e 100644 --- a/panda/board/safety/safety_mazda.h +++ b/panda/board/safety/safety_mazda.h @@ -2,12 +2,23 @@ #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 +#define MAZDA_CRZ_EVENTS 0x21F + +// Radar +#define MAZDA_CRZ_CTRL 0x21C +#define MAZDA_CRZ_INFO 0x21B +#define MAZDA_RADAR_361 0x361 +#define MAZDA_RADAR_362 0x362 +#define MAZDA_RADAR_363 0x363 +#define MAZDA_RADAR_364 0x364 +#define MAZDA_RADAR_365 0x365 +#define MAZDA_RADAR_366 0x366 +#define MAZDA_RADAR_499 0x499 // CAN bus numbers #define MAZDA_MAIN 0 @@ -26,10 +37,13 @@ #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_LKAS2, 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}, + {MAZDA_CRZ_CTRL, 0, 8}, {MAZDA_CRZ_INFO, 0, 8}, {MAZDA_RADAR_361, 0, 8}, {MAZDA_RADAR_362, 0, 8}, + {MAZDA_RADAR_363, 0, 8}, {MAZDA_RADAR_364, 0, 8}, {MAZDA_RADAR_365, 0, 8}, {MAZDA_RADAR_366, 0, 8}, + {MAZDA_RADAR_499, 0, 8}}; AddrCheckStruct mazda_addr_checks[] = { - {.msg = {{MAZDA_CRZ_CTRL, 0, 8, .expected_timestep = 20000U}, { 0 }, { 0 }}}, + {.msg = {{MAZDA_CRZ_EVENTS, 0, 8, .expected_timestep = 20000U}, { 0 }, { 0 }}}, {.msg = {{MAZDA_CRZ_BTNS, 0, 8, .expected_timestep = 100000U}, { 0 }, { 0 }}}, {.msg = {{MAZDA_STEER_TORQUE, 0, 8, .expected_timestep = 12000U}, { 0 }, { 0 }}}, {.msg = {{MAZDA_ENGINE_DATA, 0, 8, .expected_timestep = 10000U}, { 0 }, { 0 }}}, @@ -54,6 +68,8 @@ static int mazda_rx_hook(CANPacket_t *to_push) { torque_interceptor_detected = 1; valid &= addr_safety_check(to_push, &mazda_ti_rx_checks, NULL, NULL, NULL); } + // TODO: Detect if RI wiring mode is present + radar_intercept_mode = true; if (valid && (GET_BUS(to_push) == MAZDA_MAIN)) { int addr = GET_ADDR(to_push); @@ -77,8 +93,8 @@ static int mazda_rx_hook(CANPacket_t *to_push) { } // enter controls on rising edge of ACC, exit controls on ACC off - if (addr == MAZDA_CRZ_CTRL) { - bool cruise_engaged = GET_BYTE(to_push, 0) & 0x8U; + if (addr == MAZDA_CRZ_EVENTS) { + bool cruise_engaged = GET_BYTE(to_push, 2) & 0x1U; // cruise active car moving. if (cruise_engaged) { if (!cruise_engaged_prev) { controls_allowed = 1; @@ -178,11 +194,14 @@ static int mazda_tx_hook(CANPacket_t *to_send) { static int mazda_fwd_hook(int bus, CANPacket_t *to_fwd) { int bus_fwd = -1; int addr = GET_ADDR(to_fwd); + bool block = false; if (bus == MAZDA_MAIN) { bus_fwd = MAZDA_CAM; } else if (bus == MAZDA_CAM) { - bool block = (addr == MAZDA_LKAS) || (addr == MAZDA_LKAS_HUD); + block |= (addr == MAZDA_LKAS); + block |= (addr == MAZDA_LKAS_HUD); + if (!block) { bus_fwd = MAZDA_MAIN; } diff --git a/panda/board/safety_declarations.h b/panda/board/safety_declarations.h index 1a0871561..7447d2a50 100644 --- a/panda/board/safety_declarations.h +++ b/panda/board/safety_declarations.h @@ -113,6 +113,7 @@ 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; +bool radar_intercept_mode = 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 28dbc30be..1b9839f6c 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, 45) - a = struct.unpack(" &pandas, bool s ps.setHeartbeatLost((bool)(pandaState.heartbeat_lost)); ps.setHarnessStatus(cereal::PandaState::HarnessStatus(pandaState.car_harness_status)); ps.setTorqueInterceptorDetected(pandaState.torque_interceptor_detected); + ps.setRadarInterceptMode(pandaState.radar_intercept_mode); // Convert faults bitset to capnp list diff --git a/selfdrive/boardd/panda.h b/selfdrive/boardd/panda.h index f8dd37e25..70e41e9ff 100644 --- a/selfdrive/boardd/panda.h +++ b/selfdrive/boardd/panda.h @@ -46,6 +46,7 @@ struct __attribute__((packed)) health_t { uint8_t power_save_enabled; uint8_t heartbeat_lost; uint8_t torque_interceptor_detected; + uint8_t radar_intercept_mode; }; struct __attribute__((packed)) can_header { diff --git a/selfdrive/car/car_helpers.py b/selfdrive/car/car_helpers.py index f1e0caa96..a06d48705 100644 --- a/selfdrive/car/car_helpers.py +++ b/selfdrive/car/car_helpers.py @@ -199,3 +199,10 @@ def get_ti(): return car_params +def enable_mazda_long(): + print("enable_mazda_long, entering get_params") + CarInterface = global_ti.saved_CarInterface + car_params = CarInterface.get_params("mazda_long", global_ti.saved_finger) + + return car_params + diff --git a/selfdrive/car/interfaces.py b/selfdrive/car/interfaces.py index d354d8de8..04b51f821 100644 --- a/selfdrive/car/interfaces.py +++ b/selfdrive/car/interfaces.py @@ -99,8 +99,9 @@ class CarInterfaceBase(): ret.longitudinalActuatorDelayLowerBound = 0.15 ret.longitudinalActuatorDelayUpperBound = 0.15 - # No Torque Interceptor by default + # No Intercept by default ret.enableTorqueInterceptor = False + ret.radarInterceptMode = False return ret diff --git a/selfdrive/car/mazda/interface.py b/selfdrive/car/mazda/interface.py index 207759e66..fadcd2629 100755 --- a/selfdrive/car/mazda/interface.py +++ b/selfdrive/car/mazda/interface.py @@ -5,6 +5,7 @@ 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 +from selfdrive import global_ri as RI ButtonType = car.CarState.ButtonEvent.Type EventName = car.CarEvent.EventName @@ -150,6 +151,10 @@ class CarInterface(CarInterfaceBase): if self.CP.enableTorqueInterceptor and not TI.enabled: TI.enabled = True self.cp = self.CS.get_can_parser(self.CP) + if self.CP.radarInterceptMode and not RI.enabled: + RI.enabled = True + self.cp = self.CS.get_can_parser(self.CP) + self.cp_cam = self.CS.get_cam_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