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

enable mazda long control

Browse Source
This commit is contained in:
Ryley
2022-04-21 21:10:21 -05:00
parent 8b6e56b6ea
commit 05bbd08ed3
5 changed files with 144 additions and 58 deletions
Download Patch File Download Diff File Expand all files Collapse all files
panda/board/safety/safety_mazda.h
+8
View File
@@ -201,6 +201,14 @@ static int mazda_fwd_hook(int bus, CANPacket_t *to_fwd) {
} else if (bus == MAZDA_CAM) {
block |= (addr == MAZDA_LKAS);
block |= (addr == MAZDA_LKAS_HUD);
block |= (addr == MAZDA_CRZ_INFO);
block |= (addr == MAZDA_CRZ_CTRL);
block |= (addr == MAZDA_RADAR_361);
block |= (addr == MAZDA_RADAR_362);
block |= (addr == MAZDA_RADAR_363);
block |= (addr == MAZDA_RADAR_364);
block |= (addr == MAZDA_RADAR_365);
block |= (addr == MAZDA_RADAR_366);
if (!block) {
bus_fwd = MAZDA_MAIN;
selfdrive/car/mazda/carcontroller.py
+4
View File
@@ -74,4 +74,8 @@ class CarController():
can_sends.append(mazdacan.create_steering_control(self.packer, CS.CP.carFingerprint,
frame, apply_steer, CS.cam_lkas))
"""ACC RADAR COMMAND"""
if frame % 2 == 0:
can_sends.extend(mazdacan.create_radar_command(self.packer, CS.CP.carFingerprint, frame, c, CS))
return can_sends
selfdrive/car/mazda/carstate.py
+82 -10
View File
@@ -36,8 +36,8 @@ class CarState(CarStateBase):
ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw)
# Match panda speed reading
speed_kph = cp.vl["ENGINE_DATA"]["SPEED"]
ret.standstill = speed_kph < .1
self.speed_kph = cp.vl["ENGINE_DATA"]["SPEED"]
ret.standstill = self.speed_kph < .1
can_gear = int(cp.vl["GEAR"]["GEAR"])
ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(can_gear, None))
@@ -86,15 +86,15 @@ class CarState(CarStateBase):
# LKAS is enabled at 52kph going up and disabled at 45kph going down
# wait for LKAS_BLOCK signal to clear when going up since it lags behind the speed sometimes
if speed_kph > LKAS_LIMITS.ENABLE_SPEED:
if self.speed_kph > LKAS_LIMITS.ENABLE_SPEED:
self.lkas_allowed_speed = True
elif speed_kph < LKAS_LIMITS.DISABLE_SPEED:
elif self.speed_kph < LKAS_LIMITS.DISABLE_SPEED:
self.lkas_allowed_speed = False
# TODO: the signal used for available seems to be the adaptive cruise signal, instead of the main on
# it should be used for carState.cruiseState.nonAdaptive instead
ret.cruiseState.available = cp.vl["CRZ_CTRL"]["CRZ_AVAILABLE"] == 1
ret.cruiseState.enabled = cp.vl["CRZ_CTRL"]["CRZ_ACTIVE"] == 1
ret.cruiseState.available = cp_cam.vl["CRZ_CTRL"]["CRZ_AVAILABLE"] == 1
ret.cruiseState.enabled = cp.vl["CRZ_EVENTS"]["CRUISE_ACTIVE_CAR_MOVING"] == 1
ret.cruiseState.speed = cp.vl["CRZ_EVENTS"]["CRZ_SPEED"] * CV.KPH_TO_MS
# On if no driver torque the last 5 seconds
@@ -110,6 +110,9 @@ class CarState(CarStateBase):
self.crz_btns_counter = cp.vl["CRZ_BTNS"]["CTR"]
ret.steerError = cp_cam.vl["CAM_LKAS"]["ERR_BIT_1"] == 1
self.cp_cam = cp_cam
self.cp = cp
return ret
@staticmethod
@@ -142,8 +145,7 @@ class CarState(CarStateBase):
("LKAS_BLOCK", "STEER_RATE", 0),
("LKAS_TRACK_STATE", "STEER_RATE", 0),
("HANDS_OFF_5_SECONDS", "STEER_RATE", 0),
("CRZ_ACTIVE", "CRZ_CTRL", 0),
("CRZ_AVAILABLE", "CRZ_CTRL", 0),
("CRUISE_ACTIVE_CAR_MOVING", "CRZ_EVENTS", 0),
("CRZ_SPEED", "CRZ_EVENTS", 0),
("STANDSTILL", "PEDALS", 0),
("BRAKE_ON", "PEDALS", 0),
@@ -163,7 +165,7 @@ class CarState(CarStateBase):
checks += [
("ENGINE_DATA", 100),
("CRZ_CTRL", 50),
("CRZ_EVENTS", 50),
("CRZ_BTNS", 10),
("PEDALS", 50),
@@ -219,11 +221,81 @@ class CarState(CarStateBase):
("S1", "CAM_LANEINFO", 0),
("S1_HBEAM", "CAM_LANEINFO", 0),
]
checks += [
# sig_address, frequency
("CAM_LANEINFO", 2),
("CAM_LKAS", 16),
]
signals += [
("CRZ_ACTIVE", "CRZ_CTRL", 0),
("CRZ_AVAILABLE", "CRZ_CTRL", 0),
("DISTANCE_SETTING", "CRZ_CTRL", 0),
("ACC_ACTIVE_2", "CRZ_CTRL", 0),
("DISABLE_TIMER_1", "CRZ_CTRL", 0),
("DISABLE_TIMER_2", "CRZ_CTRL", 0),
("NEW_SIGNAL_1", "CRZ_CTRL", 0),
("NEW_SIGNAL_2", "CRZ_CTRL", 0),
("NEW_SIGNAL_3", "CRZ_CTRL", 0),
("NEW_SIGNAL_4", "CRZ_CTRL", 0),
("NEW_SIGNAL_5", "CRZ_CTRL", 0),
("NEW_SIGNAL_6", "CRZ_CTRL", 0),
]
signals += [
("STATUS", "CRZ_INFO", 0),
("STATIC_1", "CRZ_INFO", 0),
("ACCEL_CMD", "CRZ_INFO", 0),
("CRZ_ENDED", "CRZ_INFO", 0),
("ACC_SET_ALLOWED", "CRZ_INFO", 0),
("ACC_ACTIVE", "CRZ_INFO", 0),
("MYSTERY_BIT", "CRZ_INFO", 0),
("CTR1", "CRZ_INFO", 0),
("CHECKSUM", "CRZ_INFO", 0),
]
signals += [
("DISTANCE_LEAD", "RADAR_361", 0),
("RELATIVE_VEL_LEAD", "RADAR_361", 0),
("STATIC_1", "RADAR_361", 0),
("DISTANCE_RELATED", "RADAR_361", 0),
("STATIC_2", "RADAR_361", 0),
("SPEED_INVERSE", "RADAR_361", 0),
("IS_MOVING", "RADAR_361", 0),
("CTR", "RADAR_361", 0),
]
signals += [
("STEER_ANGLE", "RADAR_362", 0),
("STATIC_1", "RADAR_362", 0),
("STATIC_2", "RADAR_362", 0),
("STATIC_3", "RADAR_362", 0),
("CTR", "RADAR_362", 0),
]
signals += [
("STATIC_1", "RADAR_363", 0),
("STATIC_2", "RADAR_363", 0),
]
signals += [
("STATIC_1", "RADAR_364", 0),
("STATIC_2", "RADAR_364", 0),
]
signals += [
("STATIC_1", "RADAR_365", 0),
("STATIC_2", "RADAR_365", 0),
]
signals += [
("STATIC_1", "RADAR_366", 0),
("STATIC_2", "RADAR_366", 0),
]
checks += [
("CRZ_CTRL", 50), # Not blocked in panda 0x21BC
("CRZ_INFO", 50), # Blocked in panda 0x21B
("RADAR_361", 10), # 0x361
("RADAR_362", 10), # 0x362
("RADAR_363", 10), # Likely containes radar tracks 0x363
("RADAR_364", 10), # Likely containes radar tracks 0x364
("RADAR_365", 10), # 0x365
("RADAR_366", 10), # 0x366
#("RADAR_499_STATIC", 10), # 0x499
]
return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, 2)
selfdrive/car/mazda/interface.py
+5 -3
View File
@@ -28,9 +28,11 @@ class CarInterface(CarInterfaceBase):
ret.dashcamOnly = False # candidate not in [CAR.CX9_2021]
#ret.enableTorqueInterceptor = 0x24A in fingerprint[0]
if ret.enableTorqueInterceptor:
print("Recieving torque interceptor signal.")
ret.openpilotLongitudinalControl = True
ret.longitudinalTuning.kpBP = [0., 10.]
ret.longitudinalTuning.kpV = [.5, 0.3,]
ret.longitudinalTuning.kiBP = [0.]
ret.longitudinalTuning.kiV = [0.06]
ret.steerActuatorDelay = 0.1
ret.steerRateCost = 1.0
selfdrive/car/mazda/mazdacan.py
+45 -45
View File
@@ -132,14 +132,14 @@ def create_button_cmd(packer, car_fingerprint, counter, button):
return packer.make_can_msg("CRZ_BTNS", 0, values)
def create_radar_command(packer, car_fingerprint, frame, actuators, enabled, cp_cam, cp, cs):
def create_radar_command(packer, car_fingerprint, frame, c, CS):
RI.active = True
accel = 0
radar_accel = int(cp_cam.vl["CRZ_INFO"]["ACCEL_CMD"]) # get stock accel command. dbc offset should be applied already.
radar_accel = int(CS.cp_cam.vl["CRZ_INFO"]["ACCEL_CMD"]) # get stock accel command. dbc offset should be applied already.
ret = []
# request low speed mode transition
if cs.speed < 30: #kmh
if CS.speed_kph < 30:
if not RI.low_speed_mode:
RI.reset = True # request reset of PID loop
RI.radar_accel = radar_accel # save radar accel value to have a smooth transition into low speed mode
@@ -152,36 +152,36 @@ def create_radar_command(packer, car_fingerprint, frame, actuators, enabled, cp_
# after we have transitioned to low speed mode, we use the vision only accel command
if RI.low_speed_mode: # this is set true in longcontrol.py
accel = actuators.accel * 2000
accel = c.actuators.accel * 2000
clip(accel, -4000, 1000)
else:
accel = radar_accel
if car_fingerprint in GEN1:
values_21B = {
"ACC_ACTIVE" : int(enabled),
"ACC_SET_ALLOWED" : int(bool(int(cp.vl["GEAR"]["GEAR"]) & 4)), # we can set ACC_SET_ALLOWED bit when in drive. Allows crz to be set from 1kmh.
"ACC_ACTIVE" : int(c.enabled),
"ACC_SET_ALLOWED" : int(bool(int(CS.cp.vl["GEAR"]["GEAR"]) & 4)), # we can set ACC_SET_ALLOWED bit when in drive. Allows crz to be set from 1kmh.
"CRZ_ENDED" : 0, # this should keep acc on down to 5km/h on my 2018 M3
"ACCEL_CMD" : accel,
"STATIC_1" : int(cp_cam.vl["CRZ_INFO"]["STATIC_1"]), #0x7FF,
"STATUS" : int(cp_cam.vl["CRZ_INFO"]["STATUS"]), #1
"MYSTERY_BIT" : int(cp_cam.vl["CRZ_INFO"]["MYSTERY_BIT"]),
"CTR1" : int(cp_cam.vl["CRZ_INFO"]["CTR1"])
"STATIC_1" : int(CS.cp_cam.vl["CRZ_INFO"]["STATIC_1"]), #0x7FF,
"STATUS" : int(CS.cp_cam.vl["CRZ_INFO"]["STATUS"]), #1
"MYSTERY_BIT" : int(CS.cp_cam.vl["CRZ_INFO"]["MYSTERY_BIT"]),
"CTR1" : int(CS.cp_cam.vl["CRZ_INFO"]["CTR1"])
}
values_21C = {
"CRZ_ACTIVE" : int(enabled),
"CRZ_AVAILABLE" : int(cp_cam.vl["CRZ_CTRL"]["CRZ_AVAILABLE"]),
"DISTANCE_SETTING" : int(cp_cam.vl["CRZ_CTRL"]["DISTANCE_SETTING"]),
"ACC_ACTIVE_2" : int(enabled),
"CRZ_ACTIVE" : int(c.enabled),
"CRZ_AVAILABLE" : int(CS.cp_cam.vl["CRZ_CTRL"]["CRZ_AVAILABLE"]),
"DISTANCE_SETTING" : int(CS.cp_cam.vl["CRZ_CTRL"]["DISTANCE_SETTING"]),
"ACC_ACTIVE_2" : int(c.enabled),
"DISABLE_TIMER_1" : 0,
"DISABLE_TIMER_2" : 0,
"NEW_SIGNAL_1" : int(cp_cam.vl["CRZ_CTRL"]["NEW_SIGNAL_1"]),
"NEW_SIGNAL_2" : int(cp_cam.vl["CRZ_CTRL"]["NEW_SIGNAL_2"]),
"NEW_SIGNAL_3" : int(cp_cam.vl["CRZ_CTRL"]["NEW_SIGNAL_3"]),
"NEW_SIGNAL_4" : int(cp_cam.vl["CRZ_CTRL"]["NEW_SIGNAL_4"]),
"NEW_SIGNAL_5" : int(cp_cam.vl["CRZ_CTRL"]["NEW_SIGNAL_5"]),
"NEW_SIGNAL_6" : int(cp_cam.vl["CRZ_CTRL"]["NEW_SIGNAL_6"]),
"NEW_SIGNAL_1" : int(CS.cp_cam.vl["CRZ_CTRL"]["NEW_SIGNAL_1"]),
"NEW_SIGNAL_2" : int(CS.cp_cam.vl["CRZ_CTRL"]["NEW_SIGNAL_2"]),
"NEW_SIGNAL_3" : int(CS.cp_cam.vl["CRZ_CTRL"]["NEW_SIGNAL_3"]),
"NEW_SIGNAL_4" : int(CS.cp_cam.vl["CRZ_CTRL"]["NEW_SIGNAL_4"]),
"NEW_SIGNAL_5" : int(CS.cp_cam.vl["CRZ_CTRL"]["NEW_SIGNAL_5"]),
"NEW_SIGNAL_6" : int(CS.cp_cam.vl["CRZ_CTRL"]["NEW_SIGNAL_6"]),
}
ret.append(packer.make_can_msg("CRZ_INFO", 0, values_21B))
@@ -189,41 +189,41 @@ def create_radar_command(packer, car_fingerprint, frame, actuators, enabled, cp_
if (frame % 10 == 0):
values_361 = {
"CTR" : int(cp_cam.vl["RADAR_361"]["CTR"]),
"SPEED_INVERSE" : int(cp_cam.vl["RADAR_361"]["SPEED_INVERSE"]),
"IS_MOVING" : int(cp_cam.vl["RADAR_361"]["IS_MOVING"]),
"DISTANCE_LEAD" : int(cp_cam.vl["RADAR_361"]["DISTANCE_LEAD"]),
"DISTANCE_RELATED" : int(cp_cam.vl["RADAR_361"]["DISTANCE_RELATED"]),
"RELATIVE_VEL_LEAD" : int(cp_cam.vl["RADAR_361"]["RELATIVE_VEL_LEAD"]),
"STATIC_1" : int(cp_cam.vl["RADAR_361"]["STATIC_1"]),
"STATIC_2" : int(cp_cam.vl["RADAR_361"]["STATIC_2"])
"CTR" : int(CS.cp_cam.vl["RADAR_361"]["CTR"]),
"SPEED_INVERSE" : int(CS.cp_cam.vl["RADAR_361"]["SPEED_INVERSE"]),
"IS_MOVING" : int(CS.cp_cam.vl["RADAR_361"]["IS_MOVING"]),
"DISTANCE_LEAD" : int(CS.cp_cam.vl["RADAR_361"]["DISTANCE_LEAD"]),
"DISTANCE_RELATED" : int(CS.cp_cam.vl["RADAR_361"]["DISTANCE_RELATED"]),
"RELATIVE_VEL_LEAD" : int(CS.cp_cam.vl["RADAR_361"]["RELATIVE_VEL_LEAD"]),
"STATIC_1" : int(CS.cp_cam.vl["RADAR_361"]["STATIC_1"]),
"STATIC_2" : int(CS.cp_cam.vl["RADAR_361"]["STATIC_2"])
}
values_362 = {
"CTR" : int(cp_cam.vl["RADAR_361"]["CTR"]),
"STEER_ANGLE" : int(cp_cam.vl["RADAR_362"]["STEER_ANGLE"]),
"STATIC_1" : int(cp_cam.vl["RADAR_362"]["STATIC_1"]),
"STATIC_2" : int(cp_cam.vl["RADAR_362"]["STATIC_2"]),
"STATIC_3" : int(cp_cam.vl["RADAR_362"]["STATIC_3"])
"CTR" : int(CS.cp_cam.vl["RADAR_361"]["CTR"]),
"STEER_ANGLE" : int(CS.cp_cam.vl["RADAR_362"]["STEER_ANGLE"]),
"STATIC_1" : int(CS.cp_cam.vl["RADAR_362"]["STATIC_1"]),
"STATIC_2" : int(CS.cp_cam.vl["RADAR_362"]["STATIC_2"]),
"STATIC_3" : int(CS.cp_cam.vl["RADAR_362"]["STATIC_3"])
}
values_363 = {
"CTR" : int(cp_cam.vl["RADAR_361"]["CTR"]),
"STATIC_1" : int(cp_cam.vl["RADAR_363"]["STATIC_1"]),
"STATIC_2" : int(cp_cam.vl["RADAR_363"]["STATIC_2"])
"CTR" : int(CS.cp_cam.vl["RADAR_361"]["CTR"]),
"STATIC_1" : int(CS.cp_cam.vl["RADAR_363"]["STATIC_1"]),
"STATIC_2" : int(CS.cp_cam.vl["RADAR_363"]["STATIC_2"])
}
values_364 = {
"CTR" : int(cp_cam.vl["RADAR_361"]["CTR"]),
"STATIC_1" : int(cp_cam.vl["RADAR_364"]["STATIC_1"]),
"STATIC_2" : int(cp_cam.vl["RADAR_364"]["STATIC_2"])
"CTR" : int(CS.cp_cam.vl["RADAR_361"]["CTR"]),
"STATIC_1" : int(CS.cp_cam.vl["RADAR_364"]["STATIC_1"]),
"STATIC_2" : int(CS.cp_cam.vl["RADAR_364"]["STATIC_2"])
}
values_365 = {
"CTR" : int(cp_cam.vl["RADAR_361"]["CTR"]),
"STATIC_1" : int(cp_cam.vl["RADAR_365"]["STATIC_1"]),
"STATIC_2" : int(cp_cam.vl["RADAR_365"]["STATIC_2"])
"CTR" : int(CS.cp_cam.vl["RADAR_361"]["CTR"]),
"STATIC_1" : int(CS.cp_cam.vl["RADAR_365"]["STATIC_1"]),
"STATIC_2" : int(CS.cp_cam.vl["RADAR_365"]["STATIC_2"])
}
values_366 = {
"CTR" : int(cp_cam.vl["RADAR_361"]["CTR"]),
"STATIC_1" : int(cp_cam.vl["RADAR_366"]["STATIC_1"]),
"STATIC_2" : int(cp_cam.vl["RADAR_366"]["STATIC_2"])
"CTR" : int(CS.cp_cam.vl["RADAR_361"]["CTR"]),
"STATIC_1" : int(CS.cp_cam.vl["RADAR_366"]["STATIC_1"]),
"STATIC_2" : int(CS.cp_cam.vl["RADAR_366"]["STATIC_2"])
}
ret.append(packer.make_can_msg("RADAR_361", 0, values_361))