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

Mazda CarController

Browse Source
This commit is contained in:
MoreTore
2025-04-26 12:37:04 -05:00
parent 0edd0864b0
commit 387013ac25
1 changed files with 127 additions and 31 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/car/mazda/carcontroller.py
+127 -31
View File
@@ -1,66 +1,162 @@
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.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 * 240) + 2000
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.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") and CC.longActive:
CS.acc["ACCEL_CMD"] = acc_output
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 or CC.cruiseControl.override or CS.out.gasPressed or
(CC.actuators.longControlState == LongCtrlState.starting) or CS.acc["RESUME"]): # and we want to resume
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