mirror of
https://github.com/sunnypilot/sunnypilot.git
synced 2026-07-18 08:22:05 +08:00
Jason Wen
d35cdb510d
Sync: `commaai/opendbc:master` into `sunnypilot/opendbc:master` Sync: `commaai/panda:master` into `sunnypilot/panda:master`
253 lines
10 KiB
Python
253 lines
10 KiB
Python
from cereal import car
|
|
import cereal.messaging as messaging
|
|
from openpilot.common.conversions import Conversions as CV
|
|
from openpilot.common.params import Params
|
|
from openpilot.common.realtime import DT_CTRL
|
|
from opendbc.can.packer import CANPacker
|
|
from openpilot.selfdrive.car import apply_driver_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.controls.lib.drive_helpers import MAZDA_V_CRUISE_MIN
|
|
|
|
VisualAlert = car.CarControl.HUDControl.VisualAlert
|
|
ButtonType = car.CarState.ButtonEvent.Type
|
|
|
|
|
|
class CarController(CarControllerBase):
|
|
def __init__(self, dbc_name, CP, VM):
|
|
super().__init__(dbc_name, CP, VM)
|
|
self.apply_steer_last = 0
|
|
self.packer = CANPacker(dbc_name)
|
|
self.brake_counter = 0
|
|
|
|
self.sm = messaging.SubMaster(['longitudinalPlanSP'])
|
|
self.param_s = Params()
|
|
self.last_speed_limit_sign_tap_prev = False
|
|
self.speed_limit = 0.
|
|
self.speed_limit_offset = 0
|
|
self.timer = 0
|
|
self.final_speed_kph = 0
|
|
self.init_speed = 0
|
|
self.current_speed = 0
|
|
self.v_set_dis = 0
|
|
self.v_cruise_min = 0
|
|
self.button_type = 0
|
|
self.button_select = 0
|
|
self.button_count = 0
|
|
self.target_speed = 0
|
|
self.t_interval = 7
|
|
self.slc_active_stock = False
|
|
self.sl_force_active_timer = 0
|
|
self.v_tsc_state = 0
|
|
self.slc_state = 0
|
|
self.m_tsc_state = 0
|
|
self.cruise_button = None
|
|
self.speed_diff = 0
|
|
self.v_tsc = 0
|
|
self.m_tsc = 0
|
|
self.steady_speed = 0
|
|
|
|
def update(self, CC, CS, now_nanos):
|
|
if not self.CP.pcmCruiseSpeed:
|
|
self.sm.update(0)
|
|
|
|
if self.sm.updated['longitudinalPlanSP']:
|
|
self.v_tsc_state = self.sm['longitudinalPlanSP'].visionTurnControllerState
|
|
self.slc_state = self.sm['longitudinalPlanSP'].speedLimitControlState
|
|
self.m_tsc_state = self.sm['longitudinalPlanSP'].turnSpeedControlState
|
|
self.speed_limit = self.sm['longitudinalPlanSP'].speedLimit
|
|
self.speed_limit_offset = self.sm['longitudinalPlanSP'].speedLimitOffset
|
|
self.v_tsc = self.sm['longitudinalPlanSP'].visionTurnSpeed
|
|
self.m_tsc = self.sm['longitudinalPlanSP'].turnSpeed
|
|
|
|
self.v_cruise_min = MAZDA_V_CRUISE_MIN[CS.params_list.is_metric] * (CV.KPH_TO_MPH if not CS.params_list.is_metric else 1)
|
|
|
|
can_sends = []
|
|
|
|
if not self.CP.pcmCruiseSpeed:
|
|
if not self.last_speed_limit_sign_tap_prev and CS.params_list.last_speed_limit_sign_tap:
|
|
self.sl_force_active_timer = self.frame
|
|
self.param_s.put_bool_nonblocking("LastSpeedLimitSignTap", False)
|
|
self.last_speed_limit_sign_tap_prev = CS.params_list.last_speed_limit_sign_tap
|
|
|
|
sl_force_active = CS.params_list.speed_limit_control_enabled and (self.frame < (self.sl_force_active_timer * DT_CTRL + 2.0))
|
|
sl_inactive = not sl_force_active and (not CS.params_list.speed_limit_control_enabled or (True if self.slc_state == 0 else False))
|
|
sl_temp_inactive = not sl_force_active and (CS.params_list.speed_limit_control_enabled and (True if self.slc_state == 1 else False))
|
|
slc_active = not sl_inactive and not sl_temp_inactive
|
|
|
|
self.slc_active_stock = slc_active
|
|
|
|
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))
|
|
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))
|
|
elif CS.out.cruiseState.enabled and not self.CP.pcmCruiseSpeed:
|
|
self.cruise_button = self.get_cruise_buttons(CS, CC.vCruise)
|
|
if self.cruise_button is not None:
|
|
if self.frame % 10 == 0:
|
|
can_sends.append(mazdacan.create_button_cmd(self.packer, self.CP, self.frame // 10, self.cruise_button))
|
|
|
|
self.apply_steer_last = 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))
|
|
|
|
# send steering command
|
|
can_sends.append(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.steerOutputCan = apply_steer
|
|
|
|
self.frame += 1
|
|
return new_actuators, can_sends
|
|
|
|
# multikyd methods, sunnyhaibin logic
|
|
def get_cruise_buttons_status(self, CS):
|
|
if not CS.out.cruiseState.enabled:
|
|
for be in CS.out.buttonEvents:
|
|
if be.type in (ButtonType.accelCruise, ButtonType.resumeCruise,
|
|
ButtonType.decelCruise, ButtonType.setCruise) and be.pressed:
|
|
self.timer = 40
|
|
elif self.timer:
|
|
self.timer -= 1
|
|
else:
|
|
return 1
|
|
return 0
|
|
|
|
def get_target_speed(self, v_cruise_kph_prev):
|
|
v_cruise_kph = v_cruise_kph_prev
|
|
if self.slc_state > 1:
|
|
v_cruise_kph = (self.speed_limit + self.speed_limit_offset) * CV.MS_TO_KPH
|
|
if not self.slc_active_stock:
|
|
v_cruise_kph = v_cruise_kph_prev
|
|
return v_cruise_kph
|
|
|
|
def get_button_type(self, button_type):
|
|
self.type_status = "type_" + str(button_type)
|
|
self.button_picker = getattr(self, self.type_status, lambda: "default")
|
|
return self.button_picker()
|
|
|
|
def reset_button(self):
|
|
if self.button_type != 3:
|
|
self.button_type = 0
|
|
|
|
def type_default(self):
|
|
self.button_type = 0
|
|
return None
|
|
|
|
def type_0(self):
|
|
self.button_count = 0
|
|
self.target_speed = self.init_speed
|
|
self.speed_diff = self.target_speed - self.v_set_dis
|
|
if self.target_speed > self.v_set_dis:
|
|
return Buttons.SET_PLUS
|
|
self.button_type = 1
|
|
elif self.target_speed < self.v_set_dis and self.v_set_dis > self.v_cruise_min:
|
|
return Buttons.SET_MINUS
|
|
self.button_type = 2
|
|
return None
|
|
|
|
def type_1(self):
|
|
cruise_button = Buttons.SET_PLUS
|
|
self.button_count += 1
|
|
if self.target_speed <= self.v_set_dis:
|
|
self.button_count = 0
|
|
self.button_type = 3
|
|
elif self.button_count > 5:
|
|
self.button_count = 0
|
|
self.button_type = 3
|
|
return cruise_button
|
|
|
|
def type_2(self):
|
|
cruise_button = Buttons.SET_MINUS
|
|
self.button_count += 1
|
|
if self.target_speed >= self.v_set_dis or self.v_set_dis <= self.v_cruise_min:
|
|
self.button_count = 0
|
|
self.button_type = 3
|
|
elif self.button_count > 5:
|
|
self.button_count = 0
|
|
self.button_type = 3
|
|
return cruise_button
|
|
|
|
def type_3(self):
|
|
cruise_button = None
|
|
self.button_count += 1
|
|
if self.button_count > self.t_interval:
|
|
self.button_type = 0
|
|
return cruise_button
|
|
|
|
def get_curve_speed(self, target_speed_kph, v_cruise_kph_prev):
|
|
if self.v_tsc_state != 0:
|
|
vision_v_cruise_kph = self.v_tsc * CV.MS_TO_KPH
|
|
if int(vision_v_cruise_kph) == int(v_cruise_kph_prev):
|
|
vision_v_cruise_kph = 255
|
|
else:
|
|
vision_v_cruise_kph = 255
|
|
if self.m_tsc_state > 1:
|
|
map_v_cruise_kph = self.m_tsc * CV.MS_TO_KPH
|
|
if int(map_v_cruise_kph) == 0.0:
|
|
map_v_cruise_kph = 255
|
|
else:
|
|
map_v_cruise_kph = 255
|
|
curve_speed = self.curve_speed_hysteresis(min(vision_v_cruise_kph, map_v_cruise_kph) + 2 * CV.MPH_TO_KPH)
|
|
return min(target_speed_kph, curve_speed)
|
|
|
|
def get_button_control(self, CS, final_speed, v_cruise_kph_prev):
|
|
self.init_speed = round(min(final_speed, v_cruise_kph_prev) * (CV.KPH_TO_MPH if not CS.params_list.is_metric else 1))
|
|
self.v_set_dis = round(CS.out.cruiseState.speed * (CV.MS_TO_MPH if not CS.params_list.is_metric else CV.MS_TO_KPH))
|
|
cruise_button = self.get_button_type(self.button_type)
|
|
return cruise_button
|
|
|
|
def curve_speed_hysteresis(self, cur_speed: float, hyst=(0.75 * CV.MPH_TO_KPH)):
|
|
if cur_speed > self.steady_speed:
|
|
self.steady_speed = cur_speed
|
|
elif cur_speed < self.steady_speed - hyst:
|
|
self.steady_speed = cur_speed
|
|
return self.steady_speed
|
|
|
|
def get_cruise_buttons(self, CS, v_cruise_kph_prev):
|
|
cruise_button = None
|
|
if not self.get_cruise_buttons_status(CS):
|
|
pass
|
|
elif CS.out.cruiseState.enabled:
|
|
set_speed_kph = self.get_target_speed(v_cruise_kph_prev)
|
|
if self.slc_state > 1:
|
|
target_speed_kph = set_speed_kph
|
|
else:
|
|
target_speed_kph = min(v_cruise_kph_prev, set_speed_kph)
|
|
if self.v_tsc_state != 0 or self.m_tsc_state > 1:
|
|
self.final_speed_kph = self.get_curve_speed(target_speed_kph, v_cruise_kph_prev)
|
|
else:
|
|
self.final_speed_kph = target_speed_kph
|
|
|
|
cruise_button = self.get_button_control(CS, self.final_speed_kph, v_cruise_kph_prev) # MPH/KPH based button presses
|
|
return cruise_button
|