mirror of
https://github.com/sunnypilot/sunnypilot.git
synced 2026-07-24 07:42:05 +08:00
decouple and implement for mazda
This commit is contained in:
Jason Wen
@@ -1,14 +1,9 @@
|
||||
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
|
||||
@@ -21,63 +16,9 @@ class CarController(CarControllerBase):
|
||||
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:
|
||||
@@ -102,11 +43,6 @@ class CarController(CarControllerBase):
|
||||
# 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
|
||||
|
||||
@@ -128,125 +64,3 @@ class CarController(CarControllerBase):
|
||||
|
||||
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
|
||||
|
||||
@@ -89,8 +89,6 @@ class CarInterface(CarInterfaceBase):
|
||||
if self.CS.low_speed_alert:
|
||||
events.add(EventName.belowSteerSpeed)
|
||||
|
||||
ret.customStockLong = self.update_custom_stock_long()
|
||||
|
||||
ret.events = events.to_msg()
|
||||
|
||||
return ret
|
||||
|
||||
@@ -92,22 +92,20 @@ def create_button_cmd(packer, CP, counter, button):
|
||||
|
||||
can = int(button == Buttons.CANCEL)
|
||||
res = int(button == Buttons.RESUME)
|
||||
inc = int(button == Buttons.SET_PLUS)
|
||||
dec = int(button == Buttons.SET_MINUS)
|
||||
|
||||
if CP.flags & MazdaFlags.GEN1:
|
||||
values = {
|
||||
"CAN_OFF": can,
|
||||
"CAN_OFF_INV": (can + 1) % 2,
|
||||
|
||||
"SET_P": inc,
|
||||
"SET_P_INV": (inc + 1) % 2,
|
||||
"SET_P": 0,
|
||||
"SET_P_INV": 1,
|
||||
|
||||
"RES": res,
|
||||
"RES_INV": (res + 1) % 2,
|
||||
|
||||
"SET_M": dec,
|
||||
"SET_M_INV": (dec + 1) % 2,
|
||||
"SET_M": 0,
|
||||
"SET_M_INV": 1,
|
||||
|
||||
"DISTANCE_LESS": 0,
|
||||
"DISTANCE_LESS_INV": 1,
|
||||
|
||||
@@ -0,0 +1,25 @@
|
||||
from cereal import car
|
||||
from openpilot.selfdrive.car.mazda.values import Buttons
|
||||
from openpilot.sunnypilot.controls.lib.custom_stock_longitudinal_controller.controllers import CustomStockLongitudinalControllerBase, \
|
||||
SendCan
|
||||
from openpilot.sunnypilot.selfdrive.car.mazda.custom_stock_longitudinal_controller import helpers
|
||||
|
||||
ButtonType = car.CarState.ButtonEvent.Type
|
||||
|
||||
|
||||
class CustomStockLongitudinalController(CustomStockLongitudinalControllerBase):
|
||||
def __init__(self, car, car_controller, car_state, CP):
|
||||
super().__init__(car, car_controller, car_state, CP)
|
||||
self.accel_button = Buttons.SET_PLUS
|
||||
self.decel_button = Buttons.SET_MINUS
|
||||
self.cruise_buttons = {ButtonType.decelCruise: 0, ButtonType.accelCruise: 0,
|
||||
ButtonType.resumeCruise: 0, ButtonType.setCruise: 0}
|
||||
|
||||
def create_mock_button_messages(self) -> list[SendCan]:
|
||||
can_sends = []
|
||||
|
||||
if self.cruise_button is not None:
|
||||
if self.car_controller.frame % 10 == 0:
|
||||
can_sends.append(helpers.create_button_cmd(self.car_controller.packer, self.CP, self.car_controller.frame // 10, self.cruise_button))
|
||||
|
||||
return can_sends
|
||||
@@ -0,0 +1,40 @@
|
||||
def create_button_cmd(packer, CP, counter, button):
|
||||
|
||||
can = int(button == Buttons.CANCEL)
|
||||
res = int(button == Buttons.RESUME)
|
||||
inc = int(button == Buttons.SET_PLUS)
|
||||
dec = int(button == Buttons.SET_MINUS)
|
||||
|
||||
if CP.flags & MazdaFlags.GEN1:
|
||||
values = {
|
||||
"CAN_OFF": can,
|
||||
"CAN_OFF_INV": (can + 1) % 2,
|
||||
|
||||
"SET_P": inc,
|
||||
"SET_P_INV": (inc + 1) % 2,
|
||||
|
||||
"RES": res,
|
||||
"RES_INV": (res + 1) % 2,
|
||||
|
||||
"SET_M": dec,
|
||||
"SET_M_INV": (dec + 1) % 2,
|
||||
|
||||
"DISTANCE_LESS": 0,
|
||||
"DISTANCE_LESS_INV": 1,
|
||||
|
||||
"DISTANCE_MORE": 0,
|
||||
"DISTANCE_MORE_INV": 1,
|
||||
|
||||
"MODE_X": 0,
|
||||
"MODE_X_INV": 1,
|
||||
|
||||
"MODE_Y": 0,
|
||||
"MODE_Y_INV": 1,
|
||||
|
||||
"BIT1": 1,
|
||||
"BIT2": 1,
|
||||
"BIT3": 1,
|
||||
"CTR": (counter + 1) % 16,
|
||||
}
|
||||
|
||||
return packer.make_can_msg("CRZ_BTNS", 0, values)
|
||||
Reference in New Issue
Block a user