mawei/MoreTore
1
0
Fork 0
mirror of https://github.com/MoreTore/openpilot.git synced 2026-08-05 08:16:06 +08:00
Code Issues Packages Projects Releases Wiki Activity

added radar intereceptor panda infrastructure. not able to detect yet

Browse Source
This commit is contained in:
Ryley
2022-04-21 20:38:13 -05:00
parent 2bf9f77354
commit 8b6e56b6ea
12 changed files with 51 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files
cereal/car.capnp
+1
View File
@@ -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;
cereal/log.capnp
+1
View File
@@ -396,6 +396,7 @@ struct PandaState @0xa7649e2575e4591e {
harnessStatus @21 :HarnessStatus;
heartbeatLost @22 :Bool;
torqueInterceptorDetected @23 :Bool;
radarInterceptMode @24 :Bool;
enum FaultStatus {
none @0;
panda/board/main.c
+2
View File
@@ -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;
panda/board/safety.h
+2 -1
View File
@@ -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;
panda/board/safety/safety_mazda.h
+25 -6
View File
@@ -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;
}
panda/board/safety_declarations.h
+1
View File
@@ -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
panda/python/__init__.py
+3 -2
View File
@@ -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("<IIIIIIIIBBBBBBBHBBBB", dat)
dat = self._handle.controlRead(Panda.REQUEST_IN, 0xd2, 0, 0, 46)
a = struct.unpack("<IIIIIIIIBBBBBBBHBBBBB", dat)
return {
"uptime": a[0],
"voltage": a[1],
@@ -457,6 +457,7 @@ class Panda(object):
"power_save_enabled": a[17],
"heartbeat_lost": a[18],
"torque_interceptor_detected": a[19],
"radar_intercept_mode": a[20],
}
# ******************* control *******************
selfdrive/boardd/boardd.cc
+1
View File
@@ -358,6 +358,7 @@ bool send_panda_states(PubMaster *pm, const std::vector<Panda *> &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
selfdrive/boardd/panda.h
+1
View File
@@ -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 {
selfdrive/car/car_helpers.py
+7
View File
@@ -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
selfdrive/car/interfaces.py
+2 -1
View File
@@ -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
selfdrive/car/mazda/interface.py
+5
View File
@@ -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