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

cruize speed

Browse Source
This commit is contained in:
Jafar Al-Gharaibeh
2021-10-02 15:35:51 -05:00
parent a965de3c96
commit 0fd5588c89
21 changed files with 330 additions and 60 deletions
Download Patch File Download Diff File Expand all files Collapse all files
cereal/car.capnp
+2
View File
@@ -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;
cereal/log.capnp
+1
View File
@@ -395,6 +395,7 @@ struct PandaState @0xa7649e2575e4591e {
faults @18 :List(FaultType);
harnessStatus @21 :HarnessStatus;
heartbeatLost @22 :Bool;
torqueInterceptorDetected @23 :Bool;
enum FaultStatus {
none @0;
opendbc/mazda_2017.dbc
+15
View File
@@ -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
panda/board/main.c
+2
View File
@@ -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;
panda/board/safety.h
+1
View File
@@ -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;
panda/board/safety/safety_mazda.h
+29 -6
View File
@@ -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;
}
panda/board/safety_declarations.h
+1
View File
@@ -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
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, 44)
a = struct.unpack("<IIIIIIIIBBBBBBBHBBB", dat)
dat = self._handle.controlRead(Panda.REQUEST_IN, 0xd2, 0, 0, 45)
a = struct.unpack("<IIIIIIIIBBBBBBBHBBBB", dat)
return {
"uptime": a[0],
"voltage": a[1],
@@ -456,6 +456,7 @@ class Panda(object):
"fault_status": a[16],
"power_save_enabled": a[17],
"heartbeat_lost": a[18],
"torque_interceptor_detected": a[19],
}
# ******************* control *******************
selfdrive/boardd/boardd.cc
+2
View File
@@ -357,6 +357,8 @@ bool send_panda_states(PubMaster *pm, const std::vector<Panda *> &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<sizeof(pandaState.faults) * 8> fault_bits(pandaState.faults);
selfdrive/boardd/panda.h
+1
View File
@@ -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 {
selfdrive/car/__init__.py
+19
View File
@@ -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):
selfdrive/car/car_helpers.py
+19 -1
View File
@@ -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
selfdrive/car/interfaces.py
+7
View File
@@ -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
selfdrive/car/mazda/__init__.py
+1
View File
@@ -0,0 +1 @@
selfdrive/car/mazda/carcontroller.py
+22 -4
View File
@@ -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
selfdrive/car/mazda/carstate.py
+40 -6
View File
@@ -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
selfdrive/car/mazda/interface.py
+103 -30
View File
@@ -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()
selfdrive/car/mazda/mazdacan.py
+14 -2
View File
@@ -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)
selfdrive/car/mazda/values.py
+25 -8
View File
@@ -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: {
selfdrive/controls/controlsd.py
+12 -1
View File
@@ -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"]:
selfdrive/global_ti.py
+11
View File
@@ -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