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

mazda frogpilot

Browse Source
This commit is contained in:
MoreTore
2025-10-17 14:20:09 -05:00
parent ec1c9f3997
commit edfc0dfc70
20 changed files with 959 additions and 170 deletions
Download Patch File Download Diff File Expand all files Collapse all files
common/params.cc
+1
View File
@@ -585,6 +585,7 @@ std::unordered_map<std::string, uint32_t> keys = {
{"NoFSC", PERSISTENT},
{"BlendedACC", PERSISTENT},
{"ManualTransmission", PERSISTENT},
{"RemoteAccess", PERSISTENT},
};
} // namespace
launch_openpilot.sh
+3 -1
View File
@@ -1,3 +1,5 @@
#!/usr/bin/bash
export API_HOST=https://api.konik.ai
export ATHENA_HOST=wss://athena.konik.ai
export MAPS_HOST=https://api.konik.ai/maps
exec ./launch_chffrplus.sh
opendbc/mazda_2019.dbc
+15 -9
View File
@@ -159,10 +159,10 @@ BO_ 129 NEW_MSG_19: 8 XXX
SG_ NEW_SIGNAL_4 : 47|16@0+ (1,0) [0|65535] "" XXX
SG_ NEW_SIGNAL_5 : 56|8@1+ (1,0) [0|15] "" XXX
BO_ 130 STEER_ANGLE: 8 XXX
SG_ STEER_ANGLE : 23|16@0+ (1,0) [0|65535] "" XXX
BO_ 130 STEER: 8 XXX
SG_ STEER_ANGLE : 23|16@0+ (0.05,-1600) [0|65535] "" XXX
SG_ NEW_SIGNAL_2 : 39|8@0+ (1,0) [0|255] "" XXX
SG_ NEW_SIGNAL_1 : 55|16@0- (1,0) [0|65535] "" XXX
SG_ STEER_RATE : 55|16@0- (0.03,0) [0|65535] "" XXX
BO_ 133 NEW_MSG_35: 8 XXX
SG_ NEW_SIGNAL_1 : 7|8@0+ (1,0) [0|255] "" XXX
@@ -185,7 +185,7 @@ BO_ 133 NEW_MSG_35: 8 XXX
SG_ NEW_SIGNAL_7 : 55|8@0+ (1,0) [0|255] "" XXX
SG_ NEW_SIGNAL_8 : 56|8@1+ (1,0) [0|255] "" XXX
BO_ 134 STEER: 8 XXX
BO_ 134 STEER_ANGLE2: 8 XXX
SG_ NEW_SIGNAL_1 : 7|16@0+ (1,0) [0|65535] "" XXX
SG_ STEER_ANGLE : 25|14@0- (0.1,36) [0|1023] "" XXX
@@ -197,6 +197,9 @@ BO_ 136 NEW_MSG_40: 8 XXX
SG_ NEW_SIGNAL_2 : 59|1@0+ (1,0) [0|1] "" XXX
SG_ NEW_SIGNAL_3 : 60|1@0+ (1,0) [0|1] "" XXX
BO_ 143 BRAKE_PEDAL_PARK: 4 XXX
SG_ BRAKE_PRESSED_IN_PARK : 1|1@1+ (1,0) [0|3] "" XXX
BO_ 145 BLINK_INFO: 8 XXX
SG_ RIGHT_BLINK : 12|1@0+ (1,0) [0|3] "" XXX
SG_ LEFT_BLINK : 13|1@0+ (1,0) [0|3] "" XXX
@@ -223,13 +226,13 @@ BO_ 157 CRZ_BTNS: 8 XXX
SG_ CTR : 48|4@1+ (1,0) [0|15] "" XXX
SG_ CHKSUM : 56|8@1+ (1,0) [0|63] "" XXX
BO_ 159 BRAKE_2: 8 XXX
BO_ 159 BRAKE_PEDAL: 8 XXX
SG_ REVERSE : 1|1@0+ (1,0) [0|65535] "" XXX
SG_ NEW_SIGNAL_1 : 2|1@0+ (1,0) [0|1] "" XXX
SG_ NEW_SIGNAL_2 : 4|1@1+ (1,0) [0|3] "" XXX
SG_ NEW_SIGNAL_3 : 40|6@1+ (1,0) [0|15] "" XXX
SG_ BRAKE_NOT_PRESSED : 58|1@0+ (1,0) [0|1] "" XXX
SG_ BRAKE_PEDAL_PRESSED : 59|1@0+ (1,0) [0|1] "" XXX
SG_ BRAKE_PRESSED : 59|1@0+ (1,0) [0|1] "" XXX
BO_ 253 NEW_MSG_34: 8 XXX
SG_ NEW_SIGNAL_1 : 36|5@0+ (1,0) [0|31] "" XXX
@@ -545,7 +548,10 @@ BO_ 1067 NEW_MSG_33: 8 XXX
BO_ 1078 NEW_MSG_44: 8 XXX
SG_ IGNITION : 10|1@1+ (1,0) [0|255] "" XXX
BO_ 1087 BRAKE_PEDAL: 8 XXX
BO_ 1086 NEW_MSG_43E: 8 XXX
SG_ NEW_SIGNAL_1 : 39|16@0+ (1,0) [0|65535] "" XXX
BO_ 1087 BRAKE_PEDAL_SLOW: 8 XXX
SG_ NEW_SIGNAL_3 : 7|16@0+ (1,0) [0|65535] "" XXX
SG_ NEW_SIGNAL_4 : 23|16@0+ (1,0) [0|65535] "" XXX
SG_ NEW_SIGNAL_2 : 32|1@0+ (1,0) [0|1] "" XXX
@@ -629,8 +635,8 @@ BO_ 1868 EPS_FEEDBACK3: 8 XXX
CM_ SG_ 31 GEAR "13-P, 12-R, 11-N, 1-6-D";
VAL_ 31 GEAR 13 "P" 12 "R" 11 "N" 1 "D" 2 "D" 3 "D" 4 "D" 5 "D" 6 "D";
VAL_ 552 GEAR 13 "P" 12 "R" 11 "N" 1 "D" 2 "D" 3 "D" 4 "D" 5 "D" 6 "D";
VAL_ 537 GEAR 0 "P" 1 "D" 2 "R";
VAL_ 552 GEAR 13 "P" 12 "R" 11 "N" 1 "D" 2 "D" 3 "D" 4 "D" 5 "D" 6 "D";
VAL_ 552 GEAR_SHIFT 6 "6th" 5 "5th" 4 "4th" 3 "3rd" 2 "2nd" 1 "1st" 14 "Shift" 13 "Park" 11 "Neutral" 12 "Reverse";
VAL_ 1098 CRZ_STATE 0 "CRUISE_DISABLED" 1 "CRUISE_READY" 2 "CRUISE_ENABLED" 4 "GAS_OVERRIDE";
VAL_ 1098 DISTANCE_SETTING 4 "CLOSE" 3 "MEDIUM_CLOSE" 2 "MEDIUM_FAR" 1 "FAR" 0 "ACC_DISABLED";
VAL_ 1098 DISTANCE_SETTING 4 "CLOSE" 3 "MEDIUM_CLOSE" 2 "MEDIUM_FAR" 1 "FAR" 0 "ACC_DISABLED";
selfdrive/car/fingerprints.py
+3
View File
@@ -255,6 +255,9 @@ MIGRATION = {
"MAZDA 6": MAZDA.MAZDA_6,
"MAZDA CX-9 2021": MAZDA.MAZDA_CX9_2021,
"MAZDA CX-5 2022": MAZDA.MAZDA_CX5_2022,
"MAZDA 3 2019": MAZDA.MAZDA_3_2019,
"MAZDA CX-30": MAZDA.MAZDA_CX_30,
"MAZDA CX-50": MAZDA.MAZDA_CX_50,
"NISSAN X-TRAIL 2017": NISSAN.NISSAN_XTRAIL,
"NISSAN LEAF 2018": NISSAN.NISSAN_LEAF,
"NISSAN LEAF 2018 Instrument Cluster": NISSAN.NISSAN_LEAF_IC,
selfdrive/car/mazda/carcontroller.py
+136 -31
View File
@@ -1,66 +1,171 @@
from cereal import car
from opendbc.can.packer import CANPacker
from openpilot.selfdrive.car import apply_driver_steer_torque_limits
from openpilot.selfdrive.car import apply_driver_steer_torque_limits, apply_ti_steer_torque_limits
from openpilot.selfdrive.car.interfaces import CarControllerBase
from openpilot.selfdrive.car.mazda import mazdacan
from openpilot.selfdrive.car.mazda.values import CarControllerParams, Buttons
from openpilot.selfdrive.car.mazda.values import CarControllerParams, Buttons, MazdaFlags
from openpilot.common.realtime import ControlsTimer as Timer, DT_CTRL
from openpilot.common.filter_simple import FirstOrderFilter
from openpilot.common.params import Params
VisualAlert = car.CarControl.HUDControl.VisualAlert
LongCtrlState = car.CarControl.Actuators.LongControlState
class CarController(CarControllerBase):
def __init__(self, dbc_name, CP, VM):
self.CP = CP
self.apply_steer_last = 0
self.ti_apply_steer_last = 0
self.packer = CANPacker(dbc_name)
self.brake_counter = 0
self.frame = 0
self.ccp = CarControllerParams(CP)
self.hold_timer = Timer(6.0)
self.hold_delay = Timer(.5) # delay before we start holding as to not hit the brakes too hard
self.resume_timer = Timer(0.5)
self.cancel_delay = Timer(0.07) # 70ms delay to try to avoid a race condition with stock system
self.acc_filter = FirstOrderFilter(0.0, .1, DT_CTRL, initialized=False)
self.filtered_acc_last = 0
self.long_active_last = False
self.params = Params()
self.params_memory = Params("/dev/shm/params")
def update(self, CC, CS, now_nanos, frogpilot_toggles):
can_sends = []
apply_steer = 0
ti_apply_steer = 0
if CC.latActive:
# calculate steer and also set limits due to driver torque
new_steer = int(round(CC.actuators.steer * CarControllerParams.STEER_MAX))
new_steer = int(round(CC.actuators.steer * self.ccp.STEER_MAX))
apply_steer = apply_driver_steer_torque_limits(new_steer, self.apply_steer_last,
CS.out.steeringTorque, CarControllerParams)
if CC.cruiseControl.cancel:
# If brake is pressed, let us wait >70ms before trying to disable crz to avoid
# a race condition with the stock system, where the second cancel from openpilot
# will disable the crz 'main on'. crz ctrl msg runs at 50hz. 70ms allows us to
# read 3 messages and most likely sync state before we attempt cancel.
self.brake_counter = self.brake_counter + 1
if self.frame % 10 == 0 and not (CS.out.brakePressed and self.brake_counter < 7):
# Cancel Stock ACC if it's enabled while OP is disengaged
# Send at a rate of 10hz until we sync with stock ACC state
can_sends.append(mazdacan.create_button_cmd(self.packer, self.CP, CS.crz_btns_counter, Buttons.CANCEL))
else:
self.brake_counter = 0
if CC.cruiseControl.resume and self.frame % 5 == 0:
# Mazda Stop and Go requires a RES button (or gas) press if the car stops more than 3 seconds
# Send Resume button when planner wants car to move
can_sends.append(mazdacan.create_button_cmd(self.packer, self.CP, CS.crz_btns_counter, Buttons.RESUME))
CS.out.steeringTorque, self.ccp)
if self.CP.flags & MazdaFlags.TORQUE_INTERCEPTOR:
if CS.ti_lkas_allowed:
ti_new_steer = int(round(CC.actuators.steer * self.ccp.TI_STEER_MAX))
ti_apply_steer = apply_ti_steer_torque_limits(ti_new_steer, self.ti_apply_steer_last,
CS.out.steeringTorque, self.ccp)
self.apply_steer_last = apply_steer
self.ti_apply_steer_last = ti_apply_steer
# send HUD alerts
if self.frame % 50 == 0:
ldw = CC.hudControl.visualAlert == VisualAlert.ldw
steer_required = CC.hudControl.visualAlert == VisualAlert.steerRequired
# TODO: find a way to silence audible warnings so we can add more hud alerts
steer_required = steer_required and CS.lkas_allowed_speed
can_sends.append(mazdacan.create_alert_command(self.packer, CS.cam_laneinfo, ldw, steer_required))
if self.CP.flags & MazdaFlags.GEN1:
if CC.cruiseControl.cancel:
# If brake is pressed, let us wait >70ms before trying to disable crz to avoid
# a race condition with the stock system, where the second cancel from openpilot
# will disable the crz 'main on'. crz ctrl msg runs at 50hz. 70ms allows us to
# read 3 messages and most likely sync state before we attempt cancel.
self.brake_counter = self.brake_counter + 1
if self.frame % 10 == 0 and not (CS.out.brakePressed and self.brake_counter < 7):
# Cancel Stock ACC if it's enabled while OP is disengaged
# Send at a rate of 10hz until we sync with stock ACC state
can_sends.append(mazdacan.create_button_cmd(self.packer, self.CP, CS.crz_btns_counter, Buttons.CANCEL))
else:
self.brake_counter = 0
if CC.cruiseControl.resume and self.frame % 5 == 0:
# Mazda Stop and Go requires a RES button (or gas) press if the car stops more than 3 seconds
# Send Resume button when planner wants car to move
can_sends.append(mazdacan.create_button_cmd(self.packer, self.CP, CS.crz_btns_counter, Buttons.RESUME))
# send HUD alerts
if self.frame % 50 == 0:
ldw = CC.hudControl.visualAlert == VisualAlert.ldw
steer_required = CC.hudControl.visualAlert == VisualAlert.steerRequired
# TODO: find a way to silence audible warnings so we can add more hud alerts
steer_required = steer_required and CS.lkas_allowed_speed
if not self.CP.flags & MazdaFlags.NO_FSC:
can_sends.append(mazdacan.create_alert_command(self.packer, CS.cam_laneinfo, ldw, steer_required))
if self.CP.flags & MazdaFlags.RADAR_INTERCEPTOR:
hold = False
if CS.out.standstill:
hold = self.hold_timer.active()
else:
self.hold_timer.reset()
if CC.longActive:
raw_acc_output = CC.actuators.accel * 1150
raw_acc_output = max(-1000, min(raw_acc_output, 1000))
if self.params.get_bool("BlendedACC"):
if self.params_memory.get_int("CEStatus"):
self.acc_filter.update_alpha(abs(raw_acc_output-self.filtered_acc_last)/1000)
filtered_acc_output = int(self.acc_filter.update(raw_acc_output))
else:
# we want to use the stock value in this case but we need a smooth transition.
self.acc_filter.update_alpha(abs(CS.crz_info["ACCEL_CMD"]-self.filtered_acc_last)/1000)
filtered_acc_output = int(self.acc_filter.update(CS.crz_info["ACCEL_CMD"]))
CS.crz_info["ACCEL_CMD"] = int(filtered_acc_output)
self.filtered_acc_last = filtered_acc_output
else:
acc_output = raw_acc_output
if self.frame % 2 == 0:
can_sends.extend(mazdacan.create_radar_command(self.packer, self.frame, CC.longActive, CS, hold))
else:
raw_acc_output = (CC.actuators.accel * 200) + 2000
if CC.longActive:
if self.params.get_bool("BlendedACC"):
if not self.long_active_last:
# reset the filter when we start ACC
self.acc_filter.initialized = False
if self.params_memory.get_int("CEStatus"):
self.acc_filter.update_alpha(abs(raw_acc_output-self.filtered_acc_last)/1000)
filtered_acc_output = int(self.acc_filter.update(raw_acc_output))
else:
# we want to use the stock value in this case but we need a smooth transition.
self.acc_filter.update_alpha(abs(CS.acc["ACCEL_CMD"]-self.filtered_acc_last)/1000)
filtered_acc_output = int(self.acc_filter.update(CS.acc["ACCEL_CMD"]))
acc_output = filtered_acc_output
self.filtered_acc_last = filtered_acc_output
else:
acc_output = raw_acc_output
if self.params.get_bool("ExperimentalLongitudinalEnabled"):
CS.acc["ACCEL_CMD"] = acc_output
self.long_active_last = CC.longActive
resume = False
hold = False
if Timer.interval(2): # send ACC command at 50hz
"""
Without this hold/resum logic, the car will only stop momentarily.
It will then start creeping forward again. This logic allows the car to
apply the electric brake to hold the car. The hold delay also fixes a
bug with the stock ACC where it sometimes will apply the brakes too early
when coming to a stop.
"""
if CS.out.standstill: # if we're stopped
if not self.hold_delay.active(): # and we have been stopped for more than hold_delay duration. This prevents a hard brake if we aren't fully stopped.
if ((CC.cruiseControl.resume and CC.actuators.longControlState != LongCtrlState.stopping) or
CC.cruiseControl.override or CS.out.gasPressed or
(CC.actuators.longControlState == LongCtrlState.starting) or CS.acc["RESUME"]): # if we are resuming or overriding, we want to release the brake
self.resume_timer.reset() # reset the resume timer so its active
else: # otherwise we're holding
hold = self.hold_timer.active() # hold for 6s. This allows the electric brake to hold the car.
else: # if we're moving
self.hold_timer.reset() # reset the hold timer so its active when we stop
self.hold_delay.reset() # reset the hold delay
resume = self.resume_timer.active() # stay on for 0.5s to release the brake. This allows the car to move.
can_sends.append(mazdacan.create_acc_cmd(self, self.packer, CS.acc, hold, resume))
# send steering command
can_sends.append(mazdacan.create_steering_control(self.packer, self.CP,
can_sends.extend(mazdacan.create_steering_control(self.packer, self.CP,
self.frame, apply_steer, CS.cam_lkas))
new_actuators = CC.actuators.as_builder()
new_actuators.steer = apply_steer / CarControllerParams.STEER_MAX
new_actuators.steer = apply_steer / self.ccp.STEER_MAX
new_actuators.steerOutputCan = apply_steer
self.frame += 1
Timer.tick()
return new_actuators, can_sends
selfdrive/car/mazda/carstate.py
+201 -29
View File
@@ -1,9 +1,11 @@
import copy
from cereal import car, custom
from openpilot.common.conversions import Conversions as CV
from opendbc.can.can_define import CANDefine
from opendbc.can.parser import CANParser
from openpilot.selfdrive.car.interfaces import CarStateBase
from openpilot.selfdrive.car.mazda.values import DBC, LKAS_LIMITS, MazdaFlags
from openpilot.selfdrive.car.mazda.values import DBC, LKAS_LIMITS, MazdaFlags, TI_STATE, CarControllerParams
from openpilot.common.realtime import DT_CTRL
class CarState(CarStateBase):
def __init__(self, CP, FPCP):
@@ -11,17 +13,38 @@ class CarState(CarStateBase):
can_define = CANDefine(DBC[CP.carFingerprint]["pt"])
self.shifter_values = can_define.dv["GEAR"]["GEAR"]
if CP.flags & MazdaFlags.MANUAL_TRANSMISSION:
self.shifter_values = can_define.dv["MANUAL_GEAR"]["GEAR"]
self.crz_btns_counter = 0
self.acc_active_last = False
self.low_speed_alert = False
self.lkas_allowed_speed = False
self.lkas_disabled = False
self.cam_lkas = 0
self.params = CarControllerParams(CP)
self.prev_distance_button = 0
self.distance_button = 0
def update(self, cp, cp_cam, frogpilot_toggles):
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
self.shifting = False
self.torque_converter_lock = True
self._prev_steering_angle = 0.0
self.update = self.update_gen1
if CP.flags & MazdaFlags.GEN1:
self.update = self.update_gen1
if CP.flags & MazdaFlags.GEN2:
self.update = self.update_gen2
def update_gen1(self, cp, cp_cam, cp_body, frogpilot_variables):
ret = car.CarState.new_message()
fp_ret = custom.FrogPilotCarState.new_message()
@@ -51,6 +74,22 @@ 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)
if self.CP.flags & MazdaFlags.TORQUE_INTERCEPTOR:
ret.steeringTorque = cp_body.vl["TI_FEEDBACK"]["TI_TORQUE_SENSOR"]
self.ti_version = cp_body.vl["TI_FEEDBACK"]["VERSION_NUMBER"]
self.ti_state = cp_body.vl["TI_FEEDBACK"]["STATE"] # DISCOVER = 0, OFF = 1, DRIVER_OVER = 2, RUN=3
self.ti_violation = cp_body.vl["TI_FEEDBACK"]["VIOL"] # 0 = no violation
self.ti_error = cp_body.vl["TI_FEEDBACK"]["ERROR"] # 0 = no error
if self.ti_version > 1:
self.ti_ramp_down = (cp_body.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.steeringTorque = cp.vl["STEER_TORQUE"]["STEER_TORQUE_SENSOR"]
ret.steeringPressed = abs(ret.steeringTorque) > LKAS_LIMITS.STEER_THRESHOLD
@@ -85,31 +124,99 @@ class CarState(CarStateBase):
# 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.standstill = cp.vl["PEDALS"]["STANDSTILL"] == 1
ret.cruiseState.standstill = ret.standstill
ret.cruiseState.speed = cp.vl["CRZ_EVENTS"]["CRZ_SPEED"] * CV.KPH_TO_MS
if ret.cruiseState.enabled:
if not self.lkas_allowed_speed and self.acc_active_last:
self.low_speed_alert = True
else:
self.low_speed_alert = False
if self.CP.flags & MazdaFlags.RADAR_INTERCEPTOR:
self.crz_info = copy.copy(cp_cam.vl["CRZ_INFO"])
self.crz_cntr = copy.copy(cp_cam.vl["CRZ_CTRL"])
self.cp_cam = cp_cam
ret.cruiseState.enabled = cp.vl["PEDALS"]["ACC_ACTIVE"] == 1
ret.cruiseState.available = cp.vl["PEDALS"]["CRZ_AVAILABLE"] == 1
elif self.CP.flags & MazdaFlags.NO_MRCC:
ret.cruiseState.enabled = cp.vl["PEDALS"]["ACC_ACTIVE"] == 1
ret.cruiseState.available = cp.vl["PEDALS"]["CRZ_AVAILABLE"] == 1
else:
ret.cruiseState.available = cp.vl["CRZ_CTRL"]["CRZ_AVAILABLE"] == 1
ret.cruiseState.enabled = cp.vl["CRZ_CTRL"]["CRZ_ACTIVE"] == 1
# Check if LKAS is disabled due to lack of driver torque when all other states indicate
# it should be enabled (steer lockout). Don't warn until we actually get lkas active
# and lose it again, i.e, after initial lkas activation
ret.steerFaultTemporary = self.lkas_allowed_speed and lkas_blocked
ret.steerFaultTemporary = self.lkas_allowed_speed and lkas_blocked and not self.ti_lkas_allowed
self.acc_active_last = ret.cruiseState.enabled
self.crz_btns_counter = cp.vl["CRZ_BTNS"]["CTR"]
# camera signals
self.lkas_disabled = cp_cam.vl["CAM_LANEINFO"]["LANE_LINES"] == 0
self.cam_lkas = cp_cam.vl["CAM_LKAS"]
self.cam_laneinfo = cp_cam.vl["CAM_LANEINFO"]
ret.steerFaultPermanent = cp_cam.vl["CAM_LKAS"]["ERR_BIT_1"] == 1
if not self.CP.flags & MazdaFlags.NO_FSC:
self.lkas_disabled = cp_cam.vl["CAM_LANEINFO"]["LANE_LINES"] == 0 if not self.CP.flags & MazdaFlags.TORQUE_INTERCEPTOR else False
self.cam_lkas = cp_cam.vl["CAM_LKAS"]
self.cam_laneinfo = cp_cam.vl["CAM_LANEINFO"]
ret.steerFaultPermanent = cp_cam.vl["CAM_LKAS"]["ERR_BIT_1"] == 1 if not self.CP.flags & MazdaFlags.TORQUE_INTERCEPTOR else False
self.cp_cam = cp_cam
self.cp = cp
# FrogPilot CarState functions
self.lkas_previously_enabled = self.lkas_enabled
self.lkas_enabled = not self.lkas_disabled
return ret, fp_ret
def update_gen2(self, cp, cp_cam, cp_body, frogpilot_variables):
ret = car.CarState.new_message()
fp_ret = custom.FrogPilotCarState.new_message()
ret.wheelSpeeds = self.get_wheel_speeds(
cp_cam.vl["WHEEL_SPEEDS"]["FL"],
cp_cam.vl["WHEEL_SPEEDS"]["FR"],
cp_cam.vl["WHEEL_SPEEDS"]["RL"],
cp_cam.vl["WHEEL_SPEEDS"]["RR"],
)
ret.vEgoRaw = (ret.wheelSpeeds.fl + ret.wheelSpeeds.fr + ret.wheelSpeeds.rl + ret.wheelSpeeds.rr) / 4.
ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw) # Doesn't match cluster speed exactly
ret.leftBlinker, ret.rightBlinker = self.update_blinker_from_lamp(100, cp.vl["BLINK_INFO"]["LEFT_BLINK"] == 1,
cp.vl["BLINK_INFO"]["RIGHT_BLINK"] == 1)
ret.engineRpm = cp_cam.vl["ENGINE_DATA"]["RPM"]
#self.shifting = cp_cam.vl["GEAR"]["SHIFT"]
#self.torque_converter_lock = cp_cam.vl["GEAR"]["TORQUE_CONVERTER_LOCK"]
ret.steeringAngleDeg = cp.vl["STEER"]["STEER_ANGLE"] # updated at 100hz and its high resolution
ret.steeringRateDeg = (ret.steeringAngleDeg - self._prev_steering_angle) / DT_CTRL
self._prev_steering_angle = ret.steeringAngleDeg
#ret.steeringRateDeg = cp.vl["STEER"]["STEER_RATE"] # This signal doesn't seem accurate
ret.steeringTorque = cp_body.vl["EPS_FEEDBACK"]["STEER_TORQUE_SENSOR"]
ret.gas = cp_cam.vl["ENGINE_DATA"]["PEDAL_GAS"]
unit_conversion = CV.MPH_TO_MS if cp.vl["SYSTEM_SETTINGS"]["IMPERIAL_UNIT"] else CV.KPH_TO_MS
ret.steeringPressed = abs(ret.steeringTorque) > self.params.STEER_DRIVER_ALLOWANCE
if self.CP.flags & MazdaFlags.MANUAL_TRANSMISSION:
can_gear = int(cp_cam.vl["MANUAL_GEAR"]["GEAR"])
else:
can_gear = int(cp_cam.vl["GEAR"]["GEAR"])
ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(can_gear, None))
ret.gasPressed = ret.gas > 0
ret.seatbeltUnlatched = False # Cruise will not engage if seatbelt is unlatched (handled by car)
ret.doorOpen = False # Cruise will not engage if door is open (handled by car)
ret.brakePressed = cp.vl["BRAKE_PEDAL"]["BRAKE_PRESSED"] == 1
ret.brake = .1
ret.steerFaultPermanent = False # TODO locate signal. Car shows light on dash if there is a fault
ret.steerFaultTemporary = False # TODO locate signal. Car shows light on dash if there is a fault
ret.standstill = cp_cam.vl["SPEED"]["SPEED"] * unit_conversion < 0.1
ret.cruiseState.speed = cp.vl["CRUZE_STATE"]["CRZ_SPEED"] * unit_conversion
ret.cruiseState.enabled = (cp.vl["CRUZE_STATE"]["CRZ_STATE"] >= 2)
ret.cruiseState.available = (cp.vl["CRUZE_STATE"]["CRZ_STATE"] != 0)
ret.cruiseState.standstill = ret.standstill if not self.CP.openpilotLongitudinalControl else False
self.cp = cp
self.cp_cam = cp_cam
self.acc = copy.copy(cp.vl["ACC"])
# FrogPilot CarState functions
self.lkas_previously_enabled = self.lkas_enabled
@@ -117,23 +224,40 @@ class CarState(CarStateBase):
return ret, fp_ret
@staticmethod
def get_ti_messages(CP):
messages = []
if CP.flags & (MazdaFlags.TORQUE_INTERCEPTOR | MazdaFlags.GEN1):
messages += [
("TI_FEEDBACK", 50),
]
elif CP.flags & MazdaFlags.GEN2:
messages += [
("EPS_FEEDBACK", 50),
("EPS_FEEDBACK2", 50),
("EPS_FEEDBACK3", 50),
]
return messages
@staticmethod
def get_can_parser(CP, FPCP):
messages = [
# sig_address, frequency
("BLINK_INFO", 10),
("STEER", 67),
("STEER_RATE", 83),
("STEER_TORQUE", 83),
("WHEEL_SPEEDS", 100),
]
messages = []
if not (CP.flags & MazdaFlags.GEN2):
messages += [
# sig_address, frequency
("CRZ_BTNS", 10),
("BLINK_INFO", 10),
("STEER", 67),
("STEER_RATE", 83),
("STEER_TORQUE", 83),
("WHEEL_SPEEDS", 100),
]
if CP.flags & MazdaFlags.GEN1:
messages += [
("ENGINE_DATA", 100),
("CRZ_CTRL", 50),
("CRZ_EVENTS", 50),
("CRZ_BTNS", 10),
("PEDALS", 50),
("BRAKE", 50),
("SEATBELT", 10),
@@ -142,6 +266,21 @@ class CarState(CarStateBase):
("BSM", 10),
]
if not (CP.flags & MazdaFlags.RADAR_INTERCEPTOR) and not (CP.flags & MazdaFlags.NO_MRCC):
messages += [
("CRZ_CTRL", 50),
]
if CP.flags & MazdaFlags.GEN2:
messages += [
("BRAKE_PEDAL", 5),
("CRUZE_STATE", 10),
("BLINK_INFO", 10),
("ACC", 50),
("SYSTEM_SETTINGS", 10),
("STEER", 100)
]
return CANParser(DBC[CP.carFingerprint]["pt"], messages, 0)
@staticmethod
@@ -149,10 +288,43 @@ class CarState(CarStateBase):
messages = []
if CP.flags & MazdaFlags.GEN1:
if not CP.flags & MazdaFlags.NO_FSC:
messages += [
# address, frequency
("CAM_LANEINFO", 2),
("CAM_LKAS", 16),
]
if CP.flags & MazdaFlags.RADAR_INTERCEPTOR:
messages += [
("CRZ_INFO", 50),
("CRZ_CTRL", 50),
]
for addr in range(361,367):
msg = f"RADAR_{addr}"
messages += [
(msg,10),
]
if CP.flags & MazdaFlags.GEN2:
messages += [
# sig_address, frequency
("CAM_LANEINFO", 2),
("CAM_LKAS", 16),
("ENGINE_DATA", 100),
("STEER_TORQUE", 100),
("WHEEL_SPEEDS", 100),
("SPEED", 50),
]
if CP.flags & MazdaFlags.MANUAL_TRANSMISSION:
messages += [
("MANUAL_GEAR", 50),
]
else:
messages += [
("GEAR", 40),
]
return CANParser(DBC[CP.carFingerprint]["pt"], messages, 2)
@staticmethod
def get_body_can_parser(CP):
return CANParser(DBC[CP.carFingerprint]["pt"], CarState.get_ti_messages(CP), 1)
selfdrive/car/mazda/fingerprints.py
+90
View File
@@ -262,4 +262,94 @@ FW_VERSIONS = {
b'PXM7-21PS1-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
},
CAR.MAZDA_3_2019: {
(Ecu.eps, 0x730, None): [
b'BDGF-3216X-C\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'BCKA-3216X-E\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'BCKA-3216X-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
#b'BDGF-3216X-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'BCKA-3216X-D\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.engine, 0x7e0, None): [
b'PA2J-188K2-C\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
#b'PX06-188K2-S\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PX06-188K2-N\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PX08-188K2-L\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PX4W-188K2-C\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.fwdRadar, 0x764, None): [
b'BDTS-67XK2-E\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
#b'B0N2-67XK2-A\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'B0N2-67XK2-D\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.abs, 0x760, None): [
b'BFVV-4300F-C\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'BHCB-4300F-\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'BCKA-4300F-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'BCKA-4300F-E\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'BFVV-4300F-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.fwdCamera, 0x706, None): [
b'BDGF-67WK2-K\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'BDGF-67WK2-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
#b'BDGF-67WK2-C\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'DFR5-67WK2-C\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.transmission, 0x7e1, None): [
b'PAM6-21PS1-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
#b'PX01-21PS1-E\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PX03-21PS1-E\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PX4K-21PS1-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
},
CAR.MAZDA_CX_30: {
(Ecu.eps, 0x730, None): [
b'DFR5-3216X-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'BDGF-3216X-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.engine, 0x7e0, None): [
b'PX06-188K2-S\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.fwdRadar, 0x764, None): [
b'BDTS-67XK2-E\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'B0N2-67XK2-A\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.abs, 0x760, None): [
b'DEJW-4300F-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'BCKA-4300F-E\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.fwdCamera, 0x706, None): [
b'BDGF-67WK2-K\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'BDGF-67WK2-C\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.transmission, 0x7e1, None): [
b'PX01-21PS1-E\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
},
CAR.MAZDA_CX_50: {
(Ecu.eps, 0x730, None): [
b'VA40-3216Y-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.engine, 0x7e0, None): [
b'PX06-188K2-N\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PX08-188K2-L\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PX4W-188K2-C\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.fwdRadar, 0x764, None): [
b'VA45-67XK2-\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.abs, 0x760, None): [
b'VA40-4300F-D\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'VA40-4300F-E\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.fwdCamera, 0x706, None): [
b'VA40-67WK2-\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.transmission, 0x7e1, None): [
#b'PX01-21PS1-E\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PX03-21PS1-E\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PX4K-21PS1-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
},
}
selfdrive/car/mazda/interface.py
+111 -13
View File
@@ -1,42 +1,134 @@
#!/usr/bin/env python3
from math import exp, fabs
import numpy as np
from cereal import car, custom
from panda import Panda
from openpilot.common.conversions import Conversions as CV
from openpilot.selfdrive.car.mazda.values import CAR, LKAS_LIMITS
from openpilot.selfdrive.car.mazda.values import CAR, LKAS_LIMITS, MazdaFlags, GEN1, GEN2
from openpilot.selfdrive.car import create_button_events, get_safety_config
from openpilot.selfdrive.car.interfaces import CarInterfaceBase
from openpilot.selfdrive.car.interfaces import CarInterfaceBase, TorqueFromLateralAccelCallbackType, LateralAccelFromTorqueCallbackType
from openpilot.common.params import Params
ButtonType = car.CarState.ButtonEvent.Type
FrogPilotButtonType = custom.FrogPilotCarState.ButtonEvent.Type
EventName = car.CarEvent.EventName
NON_LINEAR_TORQUE_PARAMS = {
CAR.MAZDA_3_2019: (3.8818, 0.6873, 0.0999, 0.3605),
CAR.MAZDA_CX_30: (4.68689, 0.79999, 0.18244, 0.38763),
CAR.MAZDA_CX_50: (4.68689, 0.79999, 0.18244, 0.38763)
}
class CarInterface(CarInterfaceBase):
def get_lataccel_torque_siglin(self) -> float:
def torque_from_lateral_accel_siglin_func(lateral_acceleration: float) -> float:
# The "lat_accel vs torque" relationship is assumed to be the sum of "sigmoid + linear" curves
# An important thing to consider is that the slope at 0 should be > 0 (ideally >1)
# This has big effect on the stability about 0 (noise when going straight)
non_linear_torque_params = NON_LINEAR_TORQUE_PARAMS.get(self.CP.carFingerprint)
assert non_linear_torque_params, "The params are not defined"
a, b, c, _ = non_linear_torque_params
sig_input = a * lateral_acceleration
sig = np.sign(sig_input) * (1 / (1 + exp(-fabs(sig_input))) - 0.5)
steer_torque = (sig * b) + (lateral_acceleration * c)
return float(steer_torque)
lataccel_values = np.arange(-5.0, 5.0, 0.01)
torque_values = [torque_from_lateral_accel_siglin_func(x) for x in lataccel_values]
assert min(torque_values) < -1 and max(torque_values) > 1, "The torque values should cover the range [-1, 1]"
return torque_values, lataccel_values
def torque_from_lateral_accel(self) -> TorqueFromLateralAccelCallbackType:
if self.CP.carFingerprint in NON_LINEAR_TORQUE_PARAMS:
torque_values, lataccel_values = self.get_lataccel_torque_siglin()
def torque_from_lateral_accel_siglin(lateral_acceleration: float, torque_params: car.CarParams.LateralTorqueTuning):
return np.interp(lateral_acceleration, lataccel_values, torque_values)
return torque_from_lateral_accel_siglin
else:
return self.torque_from_lateral_accel_linear
def lateral_accel_from_torque(self) -> LateralAccelFromTorqueCallbackType:
if self.CP.carFingerprint in NON_LINEAR_TORQUE_PARAMS:
torque_values, lataccel_values = self.get_lataccel_torque_siglin()
def lateral_accel_from_torque_siglin(torque: float, torque_params: car.CarParams.LateralTorqueTuning):
return np.interp(torque, torque_values, lataccel_values)
return lateral_accel_from_torque_siglin
else:
return self.lateral_accel_from_torque_linear
@staticmethod
def _get_params(ret, candidate, fingerprint, car_fw, experimental_long, docs, frogpilot_toggles):
ret.carName = "mazda"
ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.mazda)]
ret.radarUnavailable = True
ret.dashcamOnly = False
ret.openpilotLongitudinalControl = True
p = Params()
if p.get_bool("ManualTransmission"):
ret.flags |= MazdaFlags.MANUAL_TRANSMISSION.value
ret.transmissionType = car.CarParams.TransmissionType.manual
else:
ret.transmissionType = car.CarParams.TransmissionType.automatic
ret.dashcamOnly = candidate not in (CAR.MAZDA_CX5_2022, CAR.MAZDA_CX9_2021)
if candidate in GEN1:
ret.safetyConfigs[0].safetyParam |= Panda.FLAG_MAZDA_GEN1
if p.get_bool("TorqueInterceptorEnabled"): # Torque Interceptor Installed
print("Torque Interceptor Installed")
ret.flags |= MazdaFlags.TORQUE_INTERCEPTOR.value
ret.safetyConfigs[0].safetyParam |= Panda.FLAG_MAZDA_TORQUE_INTERCEPTOR
if p.get_bool("RadarInterceptorEnabled"): # Radar Interceptor Installed
ret.flags |= MazdaFlags.RADAR_INTERCEPTOR.value
ret.experimentalLongitudinalAvailable = True
ret.radarUnavailable = False
ret.startingState = True
ret.longitudinalTuning.kpBP = [0., 5., 30.]
ret.longitudinalTuning.kpV = [1.3, 1.0, 0.7]
ret.longitudinalTuning.kiBP = [0., 5., 20., 30.]
ret.longitudinalTuning.kiV = [0.36, 0.23, 0.17, 0.1]
ret.safetyConfigs[0].safetyParam |= Panda.FLAG_MAZDA_RADAR_INTERCEPTOR
if p.get_bool("NoMRCC"): # No Mazda Radar Cruise Control; Missing CRZ_CTRL signal
ret.flags |= MazdaFlags.NO_MRCC.value
ret.safetyConfigs[0].safetyParam |= Panda.FLAG_MAZDA_NO_MRCC
if p.get_bool("NoFSC"): # No Front Sensing Camera
ret.flags |= MazdaFlags.NO_FSC.value
ret.safetyConfigs[0].safetyParam |= Panda.FLAG_MAZDA_NO_FSC
ret.steerActuatorDelay = 0.1
ret.enableBsm = True
if candidate in GEN2:
ret.safetyConfigs[0].safetyParam |= Panda.FLAG_MAZDA_GEN2
ret.experimentalLongitudinalAvailable = True
ret.stopAccel = -.5
ret.vEgoStarting = .2
ret.longitudinalActuatorDelay = 0.35 # gas is 0.25s and brake looks like 0.5
ret.longitudinalTuning.kpBP = [0., 5., 35.]
ret.longitudinalTuning.kpV = [0.0, 0.0, 0.0]
ret.longitudinalTuning.kiBP = [0., 35.]
ret.longitudinalTuning.kiV = [0.1, 0.1]
ret.startingState = True
ret.steerActuatorDelay = 0.335
ret.steerActuatorDelay = 0.1
ret.steerLimitTimer = 0.8
CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning)
if candidate not in (CAR.MAZDA_CX5_2022, ):
if candidate not in (CAR.MAZDA_CX5_2022, CAR.MAZDA_3_2019, CAR.MAZDA_CX_30, CAR.MAZDA_CX_50) and not ret.flags & MazdaFlags.TORQUE_INTERCEPTOR:
ret.minSteerSpeed = LKAS_LIMITS.DISABLE_SPEED * CV.KPH_TO_MS
ret.centerToFront = ret.wheelbase * 0.41
ret.enableBsm = True
return ret
# returns a car.CarState
def _update(self, c, frogpilot_toggles):
ret, fp_ret = self.CS.update(self.cp, self.cp_cam, frogpilot_toggles)
ret, fp_ret = self.CS.update(self.cp, self.cp_cam, self.cp_body, frogpilot_toggles)
# TODO: add button types for inc and dec
ret.buttonEvents = [
*create_button_events(self.CS.distance_button, self.CS.prev_distance_button, {1: ButtonType.gapAdjustCruise}),
@@ -46,10 +138,16 @@ class CarInterface(CarInterfaceBase):
# events
events = self.create_common_events(ret)
if self.CS.lkas_disabled:
events.add(EventName.lkasDisabled)
elif self.CS.low_speed_alert:
events.add(EventName.belowSteerSpeed)
if self.CP.flags & MazdaFlags.GEN1:
if self.CS.lkas_disabled:
events.add(EventName.lkasDisabled)
elif 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)
#if (not self.CS.ti_lkas_allowed) and (self.CP.flags & MazdaFlags.TORQUE_INTERCEPTOR):
# events.add(EventName.steerTempUnavailable) # torqueInterceptorTemporaryWarning
ret.events = events.to_msg()
selfdrive/car/mazda/mazdacan.py
+169 -54
View File
@@ -1,66 +1,105 @@
from openpilot.selfdrive.car.mazda.values import Buttons, MazdaFlags
from openpilot.common.numpy_fast import clip
def create_steering_control(packer, CP, frame, apply_steer, lkas):
tmp = apply_steer + 2048
lo = tmp & 0xFF
hi = tmp >> 8
# copy values from camera
b1 = int(lkas["BIT_1"])
er1 = int(lkas["ERR_BIT_1"])
lnv = 0
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
tmp = steering_angle + 2048
ahi = tmp >> 10
amd = (tmp & 0x3FF) >> 2
amd = (amd >> 4) | (( amd & 0xF) << 4)
alo = (tmp & 0x3) << 2
ctr = frame % 16
# bytes: [ 1 ] [ 2 ] [ 3 ] [ 4 ]
csum = 249 - ctr - hi - lo - (lnv << 3) - er1 - (ldw << 7) - ( er2 << 4) - (b1 << 5)
# bytes [ 5 ] [ 6 ] [ 7 ]
csum = csum - ahi - amd - alo - b2
if ahi == 1:
csum = csum + 15
if csum < 0:
if csum < -256:
csum = csum + 512
else:
csum = csum + 256
csum = csum % 256
values = {}
msgs = []
if CP.flags & MazdaFlags.GEN1:
if not CP.flags & MazdaFlags.NO_FSC:
tmp = apply_steer + 2048
lo = tmp & 0xFF
hi = tmp >> 8
# copy values from camera
b1 = int(lkas["BIT_1"])
er1 = int(lkas["ERR_BIT_1"])
lnv = 0
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
tmp = steering_angle + 2048
ahi = tmp >> 10
amd = (tmp & 0x3FF) >> 2
amd = (amd >> 4) | (( amd & 0xF) << 4)
alo = (tmp & 0x3) << 2
ctr = frame % 16
# bytes: [ 1 ] [ 2 ] [ 3 ] [ 4 ]
csum = 249 - ctr - hi - lo - (lnv << 3) - er1 - (ldw << 7) - ( er2 << 4) - (b1 << 5)
# bytes [ 5 ] [ 6 ] [ 7 ]
csum = csum - ahi - amd - alo - b2
if ahi == 1:
csum = csum + 15
if csum < 0:
if csum < -256:
csum = csum + 512
else:
csum = csum + 256
csum = csum % 256
values = {
"LKAS_REQUEST": apply_steer,
"CTR": ctr,
"ERR_BIT_1": er1,
"LINE_NOT_VISIBLE" : lnv,
"LDW": ldw,
"BIT_1": b1,
"ERR_BIT_2": er2,
"STEERING_ANGLE": steering_angle,
"ANGLE_ENABLED": b2,
"CHKSUM": csum
}
msgs.append(packer.make_can_msg("CAM_LKAS", 0, values))
if CP.flags & MazdaFlags.TORQUE_INTERCEPTOR:
values = {
"LKAS_REQUEST" : apply_steer,
"CHKSUM" : apply_steer,
"KEY" : 3294744160
}
msgs.append(packer.make_can_msg("CAM_LKAS2", 1, values))
elif CP.flags & MazdaFlags.GEN2:
bus = 1
sig_name = "EPS_LKAS"
values = {
"LKAS_REQUEST": apply_steer,
"CTR": ctr,
"ERR_BIT_1": er1,
"LINE_NOT_VISIBLE" : lnv,
"LDW": ldw,
"BIT_1": b1,
"ERR_BIT_2": er2,
"STEERING_ANGLE": steering_angle,
"ANGLE_ENABLED": b2,
"CHKSUM": csum
"STEER_FEEL": 12000,
}
msgs.append(packer.make_can_msg(sig_name, bus, values))
return packer.make_can_msg("CAM_LKAS", 0, values)
return msgs
def create_ti_steering_control(packer, CP, apply_steer):
key = 3294744160
chksum = apply_steer
if CP.flags & MazdaFlags.GEN1:
values = {
"LKAS_REQUEST" : apply_steer,
"CHKSUM" : chksum,
"KEY" : key
}
# TODO
# 1. Add new CAR values for MDARS Mazdas so that we can change the rate of the message. This will take some work.
# 2. Listen for reply's on both CAN buses if not MDARS version of
# Mazda (2021+ or m3 2019+) and warn the user if there is a bad connection
# but do not cause disengagment
# Write to both buses for *future* redundancy, but we only check bus 1 for a response in carstate and safey_mazda.h for now.
# if (frame % 2 == 0):
# commands.append(packer.make_can_msg("CAM_LKAS2", 0, values))
return packer.make_can_msg("CAM_LKAS2", 1, values)
def create_alert_command(packer, cam_msg: dict, ldw: bool, steer_required: bool):
values = {s: cam_msg[s] for s in [
@@ -126,3 +165,79 @@ def create_button_cmd(packer, CP, counter, button):
}
return packer.make_can_msg("CRZ_BTNS", 0, values)
STATIC_DATA_21B = [0x01FFE000, 0x00000000]
STATIC_DATA_361 = [0xFFF7FEFE, 0x1FC]
STATIC_DATA_362 = [0xFFF7FEFE, 0x1FC]
STATIC_DATA_363 = [0xFFF7FEFE, 0x1FC0000]
STATIC_DATA_364 = [0xFFF7FEFE, 0x1FC0000]
STATIC_DATA_365 = [0xFFF7FE7F, 0xFBFF3FC]
STATIC_DATA_366 = [0xFFF7FE7F, 0xFBFF3FC]
static_data_list = [STATIC_DATA_361, STATIC_DATA_362, STATIC_DATA_363, STATIC_DATA_364, STATIC_DATA_365, STATIC_DATA_366]
# GEN1 radar interceptor
def create_radar_command(packer, frame, active, CS, hold):
#accel = 0
ret = []
crz_ctrl = CS.crz_cntr
crz_info = CS.crz_info
# if CC.longActive: # this is set true in longcontrol.py
# accel = CC.actuators.accel * 1150
# accel = accel if accel < 1000 else 1000
# else:
# accel = int(crz_info["ACCEL_CMD"])
crz_info["ACC_ACTIVE"] = active
crz_info["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_info["CRZ_ENDED"] = 0 # this should keep acc on down to 5km/h on my 2018 M3
#crz_info["ACCEL_CMD"] = accel
crz_info["STOPPING_MAYBE"] = hold
crz_info["STOPPING_MAYBE2"] = hold
crz_ctrl["CRZ_ACTIVE"] = active
crz_ctrl["ACC_ACTIVE_2"] = active
crz_ctrl["DISABLE_TIMER_1"] = 0
crz_ctrl["DISABLE_TIMER_2"] = 0
ret.append(packer.make_can_msg("CRZ_INFO", 0, crz_info))
ret.append(packer.make_can_msg("CRZ_CTRL", 0, crz_ctrl))
# convert steering angle to radar units and clip to range
steer_angle = (CS.out.steeringAngleDeg *-17.4) + 2048
if (frame % 10 == 0):
for i, addr in enumerate(range(361,367)):
addr_name = f"RADAR_{addr}"
msg = CS.cp_cam.vl[addr_name]
values = {
"MSGS_1" : static_data_list[i][0],
"MSGS_2" : static_data_list[i][1],
"CTR" : int(msg["CTR"]) #frame % 16
}
if addr == 361:
values.update({
"INVERSE_SPEED" : int(CS.out.vEgo * -4.4),
"BIT" : 1,
})
if addr == 362:
values.update({
"CLIPPED_STEER_ANGLE" : int(clip(steer_angle, 0, 4092)),
})
ret.append(packer.make_can_msg(addr_name, 0, values))
return ret
# GEN2 new mazdas
def create_acc_cmd(self, packer, values, hold, resume):
msg_name = "ACC"
bus = 2
if (values["ACC_ENABLED"]):
values["HOLD"] = hold
values["RESUME"] = resume
else:
pass
return packer.make_can_msg(msg_name, bus, values)
selfdrive/car/mazda/radar_interface.py
+59 -1
View File
@@ -1,5 +1,63 @@
#!/usr/bin/env python3
import math
from cereal import car
from opendbc.can.parser import CANParser
from openpilot.selfdrive.car.interfaces import RadarInterfaceBase
from openpilot.selfdrive.car.mazda.values import DBC, MazdaFlags
def get_radar_can_parser(CP):
if DBC[CP.carFingerprint]['radar'] is None:
return None
if not CP.flags & MazdaFlags.RADAR_INTERCEPTOR:
return None
messages = [(f"RADAR_TRACK_{addr}", 10) for addr in range(361,367)]
return CANParser(DBC[CP.carFingerprint]['radar'], messages, 2)
class RadarInterface(RadarInterfaceBase):
pass
def __init__(self, CP):
super().__init__(CP)
self.updated_messages = set()
self.track_id = 0
self.radar_off_can = CP.radarUnavailable
self.rcp = get_radar_can_parser(CP)
def update(self, can_strings):
if self.radar_off_can or (self.rcp is None):
return super().update(None)
vls = self.rcp.update_strings(can_strings)
self.updated_messages.update(vls)
rr = self._update(self.updated_messages)
self.updated_messages.clear()
return rr
def _update(self, updated_messages):
ret = car.RadarData.new_message()
if self.rcp is None:
return ret
errors = []
if not self.rcp.can_valid:
errors.append("canError")
ret.errors = errors
for addr in range(361,367):
msg = self.rcp.vl[f"RADAR_TRACK_{addr}"]
if addr not in self.pts:
self.pts[addr] = car.RadarData.RadarPoint.new_message()
self.pts[addr].trackId = self.track_id
self.track_id += 1
valid = (msg['DIST_OBJ'] != 4095) and (msg['ANG_OBJ'] != 2046) and (msg['RELV_OBJ'] != -16)
if valid:
azimuth = math.radians(msg['ANG_OBJ']/64)
self.pts[addr].measured = True
self.pts[addr].dRel = msg['DIST_OBJ']/16
self.pts[addr].yRel = -math.sin(azimuth) * msg['DIST_OBJ']/16
self.pts[addr].vRel = msg['RELV_OBJ']/16
self.pts[addr].aRel = float('nan')
self.pts[addr].yvRel = float('nan')
else:
del self.pts[addr]
ret.points = list(self.pts.values())
return ret
selfdrive/car/mazda/values.py
+85 -18
View File
@@ -13,18 +13,38 @@ Ecu = car.CarParams.Ecu
# Steer torque limits
class CarControllerParams:
STEER_MAX = 800 # 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_FACTOR = 1 # from dbc
STEER_ERROR_MAX = 350 # max delta between torque cmd and torque motor
STEER_STEP = 1 # 100 Hz
def __init__(self, CP):
pass
self.STEER_STEP = 1 # 100 Hz
if CP.flags & MazdaFlags.GEN1:
self.STEER_MAX = 600 # theoretical max_steer 2047
self.STEER_DELTA_UP = 10 # torque increase per refresh
self.STEER_DELTA_DOWN = 25 # torque decrease per refresh
self.STEER_DRIVER_ALLOWANCE = 15 # allowed driver torque before start limiting
self.STEER_DRIVER_MULTIPLIER = 40 # weight driver torque
self.STEER_DRIVER_FACTOR = 1 # from dbc
self.STEER_ERROR_MAX = 350 # max delta between torque cmd and torque motor
self.TI_STEER_MAX = 600 # theoretical max_steer 2047
self.TI_STEER_DELTA_UP = 6 # torque increase per refresh
self.TI_STEER_DELTA_DOWN = 15 # torque decrease per refresh
self.TI_STEER_DRIVER_ALLOWANCE = 15 # allowed driver torque before start limiting
self.TI_STEER_DRIVER_MULTIPLIER = 40 # weight driver torque
self.TI_STEER_DRIVER_FACTOR = 1 # from dbc
self.TI_STEER_ERROR_MAX = 350 # max delta between torque cmd and torque motor
if CP.flags & MazdaFlags.GEN2:
self.STEER_MAX = 8000
self.STEER_DELTA_UP = 45 # torque increase per refresh
self.STEER_DELTA_DOWN = 80 # torque decrease per refresh
self.STEER_DRIVER_ALLOWANCE = 1400 # allowed driver torque before start limiting
self.STEER_DRIVER_MULTIPLIER = 5 # weight driver torque
self.STEER_DRIVER_FACTOR = 1 # from dbc
self.STEER_ERROR_MAX = 3500 # max delta between torque cmd and torque motor
class TI_STATE:
DISCOVER = 0
OFF = 1
DRIVER_OVER = 2
RUN = 3
@dataclass
class MazdaCarDocs(CarDocs):
@@ -41,38 +61,69 @@ class MazdaFlags(IntFlag):
# Static flags
# Gen 1 hardware: same CAN messages and same camera
GEN1 = 1
GEN2 = 2
TORQUE_INTERCEPTOR = 4
RADAR_INTERCEPTOR = 8
NO_FSC = 16
NO_MRCC = 32
MANUAL_TRANSMISSION = 64
@dataclass
class MazdaPlatformConfig(PlatformConfig):
dbc_dict: DbcDict = field(default_factory=lambda: dbc_dict('mazda_2017', None))
flags: int = MazdaFlags.GEN1
def init(self):
if self.flags & MazdaFlags.GEN2:
self.dbc_dict = dbc_dict('mazda_2019', None)
elif self.flags & MazdaFlags.GEN1 and self.flags & MazdaFlags.RADAR_INTERCEPTOR:
self.dbc_dict = dbc_dict('mazda_2017', 'mazda_radar')
class CAR(Platforms):
MAZDA_CX5 = MazdaPlatformConfig(
[MazdaCarDocs("Mazda CX-5 2017-21")],
MazdaCarSpecs(mass=3655 * CV.LB_TO_KG, wheelbase=2.7, steerRatio=15.5)
MazdaCarSpecs(mass=3655 * CV.LB_TO_KG, wheelbase=2.7, steerRatio=15.5),
flags=MazdaFlags.GEN1
)
MAZDA_CX9 = MazdaPlatformConfig(
[MazdaCarDocs("Mazda CX-9 2016-20")],
MazdaCarSpecs(mass=4217 * CV.LB_TO_KG, wheelbase=3.1, steerRatio=17.6)
MazdaCarSpecs(mass=4217 * CV.LB_TO_KG, wheelbase=3.1, steerRatio=17.6),
flags=MazdaFlags.GEN1,
)
MAZDA_3 = MazdaPlatformConfig(
[MazdaCarDocs("Mazda 3 2017-18")],
MazdaCarSpecs(mass=2875 * CV.LB_TO_KG, wheelbase=2.7, steerRatio=14.0)
MazdaCarSpecs(mass=2875 * CV.LB_TO_KG, wheelbase=2.7, steerRatio=14.0),
flags=MazdaFlags.GEN1,
)
MAZDA_6 = MazdaPlatformConfig(
[MazdaCarDocs("Mazda 6 2017-20")],
MazdaCarSpecs(mass=3443 * CV.LB_TO_KG, wheelbase=2.83, steerRatio=15.5)
MazdaCarSpecs(mass=3443 * CV.LB_TO_KG, wheelbase=2.83, steerRatio=15.5),
flags=MazdaFlags.GEN1,
)
MAZDA_CX9_2021 = MazdaPlatformConfig(
[MazdaCarDocs("Mazda CX-9 2021-23", video_link="https://youtu.be/dA3duO4a0O4")],
MAZDA_CX9.specs
MAZDA_CX9.specs,
flags=MazdaFlags.GEN1,
)
MAZDA_CX5_2022 = MazdaPlatformConfig(
[MazdaCarDocs("Mazda CX-5 2022-24")],
MAZDA_CX5.specs,
flags=MazdaFlags.GEN1,
)
MAZDA_3_2019 = MazdaPlatformConfig(
[MazdaCarDocs("Mazda 3 2019-24")],
MazdaCarSpecs(mass=3000 * CV.LB_TO_KG, wheelbase=2.725, steerRatio=18.8),
flags=MazdaFlags.GEN2,
)
MAZDA_CX_30 = MazdaPlatformConfig(
[MazdaCarDocs("Mazda CX-30 2019-24")],
MazdaCarSpecs(mass=3375 * CV.LB_TO_KG, wheelbase=2.814, steerRatio=15.5),
flags=MazdaFlags.GEN2,
)
MAZDA_CX_50 = MazdaPlatformConfig(
[MazdaCarDocs("Mazda CX-50 2022-24")],
MazdaCarSpecs(mass=3375 * CV.LB_TO_KG, wheelbase=2.814, steerRatio=15.5),
flags=MazdaFlags.GEN2,
)
@@ -80,7 +131,9 @@ class LKAS_LIMITS:
STEER_THRESHOLD = 15
DISABLE_SPEED = 45 # kph
ENABLE_SPEED = 52 # kph
TI_STEER_THRESHOLD = 6
TI_DISABLE_SPEED = 0 # kph
TI_ENABLE_SPEED = 0 # kph
class Buttons:
NONE = 0
@@ -88,6 +141,7 @@ class Buttons:
SET_MINUS = 2
RESUME = 3
CANCEL = 4
TURN_ON = 5
FW_QUERY_CONFIG = FwQueryConfig(
@@ -98,7 +152,20 @@ FW_QUERY_CONFIG = FwQueryConfig(
[StdQueries.MANUFACTURER_SOFTWARE_VERSION_RESPONSE],
bus=0,
),
Request(
[StdQueries.TESTER_PRESENT_REQUEST, StdQueries.MANUFACTURER_SOFTWARE_VERSION_REQUEST],
[StdQueries.TESTER_PRESENT_RESPONSE, StdQueries.MANUFACTURER_SOFTWARE_VERSION_RESPONSE],
whitelist_ecus=[Ecu.engine],
),
Request(
[StdQueries.TESTER_PRESENT_REQUEST, StdQueries.MANUFACTURER_SOFTWARE_VERSION_REQUEST],
[StdQueries.TESTER_PRESENT_RESPONSE, StdQueries.MANUFACTURER_SOFTWARE_VERSION_RESPONSE],
bus=0,
whitelist_ecus=[Ecu.eps, Ecu.abs, Ecu.fwdRadar, Ecu.fwdCamera, Ecu.shiftByWire],
)
],
)
DBC = CAR.create_dbc_map()
GEN1 = CAR.with_flags(MazdaFlags.GEN1)
GEN2 = CAR.with_flags(MazdaFlags.GEN2)
selfdrive/car/torque_data/override.toml
+4
View File
@@ -79,3 +79,7 @@ legend = ["LAT_ACCEL_FACTOR", "MAX_LAT_ACCEL_MEASURED", "FRICTION"]
# Manually checked
"HONDA_CIVIC_2022" = [2.5, 1.2, 0.15]
"HONDA_HRV_3G" = [2.5, 1.2, 0.2]
"MAZDA_3_2019" = [1.45, 3.0, 0.33]
"MAZDA_CX_30" = [1.45, 3.0, 0.33]
"MAZDA_CX_50" = [1.45, 3.0, 0.33]
selfdrive/controls/lib/longcontrol.py
+12
View File
@@ -4,6 +4,7 @@ from openpilot.common.realtime import DT_CTRL
from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N, apply_deadzone
from openpilot.selfdrive.controls.lib.pid import PIDController
from openpilot.selfdrive.modeld.constants import ModelConstants
from openpilot.common.params import Params
CONTROL_N_T_IDX = ModelConstants.T_IDXS[:CONTROL_N]
@@ -95,6 +96,9 @@ class LongControl:
k_f=CP.longitudinalTuning.kf, rate=1 / DT_CTRL)
self.v_pid = 0.0
self.last_output_accel = 0.0
self.params = Params()
self.params_memory = Params("/dev/shm/params")
self.experimental_mode_last = False
def reset(self):
self.pid.reset()
@@ -107,6 +111,14 @@ class LongControl:
self.long_control_state = long_control_state_trans(self.CP, active, self.long_control_state, CS.vEgo,
should_stop, CS.brakePressed,
CS.cruiseState.standstill, frogpilot_toggles)
if self.params.get_bool("BlendedACC"):
experimental_mode = self.params_memory.get_int("CEStatus") # 0 means expereimental mode is off
if experimental_mode and not self.experimental_mode_last:
self.reset()
self.experimental_mode_last = experimental_mode
if self.long_control_state == LongCtrlState.off:
self.reset()
output_accel = 0.
selfdrive/ui/qt/offroad/settings.cc
+42
View File
@@ -41,6 +41,42 @@ TogglesPanel::TogglesPanel(SettingsWindow *parent) : ListWidget(parent) {
"",
"../assets/img_experimental_white.svg",
},
{
"BlendedACC",
tr("Blended Acc (Experimental)"),
tr("Blend stock MRCC and Experimental Mode longitudinal control."),
"../assets/offroad/icon_openpilot.png",
},
{
"TorqueInterceptorEnabled",
tr("Torque Interceptor Installed"),
tr("Enable the torque interceptor to control the steering wheel."),
"../assets/offroad/icon_openpilot.png",
},
{
"RadarInterceptorEnabled",
tr("Radar Interceptor Installed"),
tr("Enable if you have installed the radar Iterceptor."),
"../assets/offroad/icon_openpilot.png",
},
{
"NoMRCC",
tr("Car Does not have stock MRCC"),
tr("Enable if your car does not have stock MRCC."),
"../assets/offroad/icon_openpilot.png",
},
{
"NoFSC",
tr("Car Does not have stock FSC"),
tr("Enable if your car does not have stock FSC."),
"../assets/offroad/icon_openpilot.png",
},
{
"ManualTransmission",
tr("Manual Transmission"),
tr("Enable if your is a manual."),
"../assets/offroad/icon_openpilot.png",
},
{
"DisengageOnAccelerator",
tr("Disengage on Accelerator Pedal"),
@@ -59,6 +95,12 @@ TogglesPanel::TogglesPanel(SettingsWindow *parent) : ListWidget(parent) {
tr("Upload data from the driver facing camera and help improve the driver monitoring algorithm."),
"../assets/offroad/icon_monitoring.png",
},
{
"RecordRoad",
tr("Record and Upload Road Cameras"),
tr("Upload data from the road cameras."),
"../assets/offroad/icon_monitoring.png",
},
{
"IsMetric",
tr("Use Metric System"),
system/athena/athenad.py
+9 -6
View File
@@ -145,6 +145,9 @@ def handle_long_poll(ws: WebSocket, exit_event: threading.Event | None) -> None:
threading.Thread(target=ws_recv, args=(ws, end_event), name='ws_recv'),
threading.Thread(target=ws_send, args=(ws, end_event), name='ws_send'),
threading.Thread(target=upload_handler, args=(end_event,), name='upload_handler'),
threading.Thread(target=upload_handler, args=(end_event,), name='upload_handler2'),
threading.Thread(target=upload_handler, args=(end_event,), name='upload_handler3'),
threading.Thread(target=upload_handler, args=(end_event,), name='upload_handler4'),
threading.Thread(target=log_handler, args=(end_event,), name='log_handler'),
threading.Thread(target=stat_handler, args=(end_event,), name='stat_handler'),
] + [
@@ -259,13 +262,13 @@ def upload_handler(end_event: threading.Event) -> None:
sz = -1
cloudlog.event("athena.upload_handler.upload_start", fn=fn, sz=sz, network_type=network_type, metered=metered, retry_count=item.retry_count)
response = _do_upload(item, partial(cb, sm, item, tid, end_event))
if response.status_code not in (200, 201, 401, 403, 412):
cloudlog.event("athena.upload_handler.retry", status_code=response.status_code, fn=fn, sz=sz, network_type=network_type, metered=metered)
retry_upload(tid, end_event)
else:
cloudlog.event("athena.upload_handler.success", fn=fn, sz=sz, network_type=network_type, metered=metered)
with _do_upload(item, partial(cb, sm, item, tid, end_event)) as response:
if response.status_code not in (200, 201, 401, 403, 412):
cloudlog.event("athena.upload_handler.retry", status_code=response.status_code, fn=fn, sz=sz, network_type=network_type, metered=metered)
retry_upload(tid, end_event)
else:
cloudlog.event("athena.upload_handler.success", fn=fn, sz=sz, network_type=network_type, metered=metered)
UploadQueueCache.cache(upload_queue)
except (requests.exceptions.Timeout, requests.exceptions.ConnectionError, requests.exceptions.SSLError):
system/athena/registration.py
Regular → Executable
+1 -2
View File
@@ -10,8 +10,7 @@ from datetime import datetime, timedelta
from openpilot.common.api import api_get
from openpilot.common.params import Params
from openpilot.common.spinner import Spinner
from openpilot.selfdrive.controls.lib.alertmanager import set_offroad_alert
from openpilot.system.hardware import HARDWARE, PC
from openpilot.system.hardware import HARDWARE
from openpilot.system.hardware.hw import Paths
from openpilot.common.swaglog import cloudlog
system/loggerd/loggerd.h
+2
View File
@@ -59,12 +59,14 @@ public:
const EncoderInfo main_road_encoder_info = {
.publish_name = "roadEncodeData",
.filename = "fcamera.hevc",
.record = Params().getBool("RecordRoad"),
INIT_ENCODE_FUNCTIONS(RoadEncode),
};
const EncoderInfo main_wide_road_encoder_info = {
.publish_name = "wideRoadEncodeData",
.filename = "ecamera.hevc",
.record = Params().getBool("RecordRoad"),
INIT_ENCODE_FUNCTIONS(WideRoadEncode),
};
system/manager/manager.py
+12 -2
View File
@@ -46,6 +46,9 @@ def manager_init() -> None:
("LanguageSetting", "main_en"),
("OpenpilotEnabledToggle", "1"),
("LongitudinalPersonality", str(log.LongitudinalPersonality.standard)),
("BlendedACC", "0"),
("RecordRoad", "1"),
("RemoteAccess", "0"),
]
if not PC:
default_params.append(("LastUpdateTime", datetime.datetime.utcnow().isoformat().encode('utf8')))
@@ -152,7 +155,10 @@ def manager_thread() -> None:
pm = messaging.PubMaster(['managerState'])
write_onroad_params(False, params)
ensure_running(managed_processes.values(), False, params=params, CP=sm['carParams'], not_run=ignore, classic_model=False, tinygrad_model=False, frogpilot_toggles=get_frogpilot_toggles())
ensure_running(
managed_processes.values(), False, params=params,CP=sm['carParams'], not_run=ignore,
classic_model=False, tinygrad_model=False, frogpilot_toggles=get_frogpilot_toggles()
)
started_prev = False
@@ -187,7 +193,11 @@ def manager_thread() -> None:
started_prev = started
ensure_running(managed_processes.values(), started, params=params, CP=sm['carParams'], not_run=ignore, classic_model=classic_model, tinygrad_model=tinygrad_model, frogpilot_toggles=frogpilot_toggles)
ensure_running(
managed_processes.values(), started, params=params, CP=sm['carParams'],
not_run=ignore, classic_model=classic_model, tinygrad_model=tinygrad_model,
frogpilot_toggles=frogpilot_toggles
)
running = ' '.join("{}{}\u001b[0m".format("\u001b[32m" if p.proc.is_alive() else "\u001b[31m", p.name)
for p in managed_processes.values() if p.proc)
tools/cabana/videowidget.cc
+3 -3
View File
@@ -300,9 +300,9 @@ void Slider::paintEvent(QPaintEvent *ev) {
};
const auto replay = qobject_cast<ReplayStream *>(can)->getReplay();
for (auto [begin, end, type] : replay->getTimeline()) {
fillRange(begin, end, timeline_colors[(int)type]);
}
// for (auto [begin, end, type] : replay->getTimeline()) {
// fillRange(begin, end, timeline_colors[(int)type]);
// }
QColor empty_color = palette().color(QPalette::Window);
empty_color.setAlpha(160);
tools/lib/auth.py
+1 -1
View File
@@ -66,7 +66,7 @@ def auth_redirect_link(method):
}[method]
params = {
'redirect_uri': f"https://api.comma.ai/v2/auth/{provider_id}/redirect/",
'redirect_uri': f"https://api.konik.ai/v2/auth/{provider_id}/redirect/",
'state': f'service,localhost:{PORT}',
}