diff --git a/selfdrive/car/mazda/carcontroller.py b/selfdrive/car/mazda/carcontroller.py index 16206f6..b5cc0f7 100644 --- a/selfdrive/car/mazda/carcontroller.py +++ b/selfdrive/car/mazda/carcontroller.py @@ -4,6 +4,12 @@ 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.common.params import Params +from openpilot.selfdrive.controls.lib.drive_helpers import V_CRUISE_MAX +from openpilot.common.conversions import Conversions as CV + + +params_memory = Params("/dev/shm/params") VisualAlert = car.CarControl.HUDControl.VisualAlert @@ -17,6 +23,13 @@ class CarController(CarControllerBase): self.frame = 0 def update(self, CC, CS, now_nanos, frogpilot_variables): + hud_control = CC.hudControl + hud_v_cruise = hud_control.setSpeed + if hud_v_cruise > 70: + hud_v_cruise = 0 + actuators = CC.actuators + accel = actuators.accel + can_sends = [] apply_steer = 0 @@ -43,6 +56,11 @@ 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)) + # ACC Spam + elif frogpilot_variables.CSLC: + if CC.enabled and self.frame % 10 == 0 and CS.cruise_buttons == Buttons.NONE and not CS.out.gasPressed and not CS.distance_button: + slcSet = get_set_speed(self, hud_v_cruise) + can_sends.extend(mazdacan.create_mazda_acc_spam_command(self.packer, self, CS, slcSet, CS.out.vEgo, frogpilot_variables, accel)) self.apply_steer_last = apply_steer @@ -64,3 +82,13 @@ class CarController(CarControllerBase): self.frame += 1 return new_actuators, can_sends + +def get_set_speed(self, hud_v_cruise): + v_cruise = min(hud_v_cruise, V_CRUISE_MAX * CV.KPH_TO_MS) + + v_cruise_slc: float = 0. + v_cruise_slc = params_memory.get_float("CSLCSpeed") + + if v_cruise_slc > 0.: + v_cruise = v_cruise_slc + return v_cruise diff --git a/selfdrive/car/mazda/carstate.py b/selfdrive/car/mazda/carstate.py index bc5ee00..03b4d23 100644 --- a/selfdrive/car/mazda/carstate.py +++ b/selfdrive/car/mazda/carstate.py @@ -3,7 +3,7 @@ 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, Buttons class CarState(CarStateBase): def __init__(self, CP): @@ -29,6 +29,17 @@ class CarState(CarStateBase): self.prev_distance_button = self.distance_button self.distance_button = cp.vl["CRZ_BTNS"]["DISTANCE_LESS"] + self.prev_cruise_buttons = self.cruise_buttons + + if bool(cp.vl["CRZ_BTNS"]["SET_P"]): + self.cruise_buttons = Buttons.SET_PLUS + elif bool(cp.vl["CRZ_BTNS"]["SET_M"]): + self.cruise_buttons = Buttons.SET_MINUS + elif bool(cp.vl["CRZ_BTNS"]["RES"]): + self.cruise_buttons = Buttons.RESUME + else: + self.cruise_buttons = Buttons.NONE + ret.wheelSpeeds = self.get_wheel_speeds( cp.vl["WHEEL_SPEEDS"]["FL"], cp.vl["WHEEL_SPEEDS"]["FR"], diff --git a/selfdrive/car/mazda/interface.py b/selfdrive/car/mazda/interface.py index d966a9b..1329b36 100755 --- a/selfdrive/car/mazda/interface.py +++ b/selfdrive/car/mazda/interface.py @@ -1,13 +1,18 @@ #!/usr/bin/env python3 from cereal import car, custom 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, Buttons from openpilot.selfdrive.car import create_button_events, get_safety_config from openpilot.selfdrive.car.interfaces import CarInterfaceBase +from openpilot.common.params import Params + +params = Params() ButtonType = car.CarState.ButtonEvent.Type EventName = car.CarEvent.EventName FrogPilotButtonType = custom.FrogPilotCarState.ButtonEvent.Type +BUTTONS_DICT = {Buttons.SET_PLUS: ButtonType.accelCruise, Buttons.SET_MINUS: ButtonType.decelCruise, + Buttons.RESUME: ButtonType.resumeCruise, Buttons.CANCEL: ButtonType.cancel} class CarInterface(CarInterfaceBase): @@ -29,6 +34,20 @@ class CarInterface(CarInterfaceBase): ret.centerToFront = ret.wheelbase * 0.41 + if params.get_bool("CSLCEnabled"): + # Used for CEM with CSLC + ret.openpilotLongitudinalControl = True + ret.longitudinalTuning.deadzoneBP = [0.] + ret.longitudinalTuning.deadzoneV = [0.9] # == 2 mph allowable delta + ret.stoppingDecelRate = 4.5 # == 10 mph/s + ret.longitudinalActuatorDelayLowerBound = 1. + ret.longitudinalActuatorDelayUpperBound = 2. + + ret.longitudinalTuning.kpBP = [8.94, 7.2, 28.] # 8.94 m/s == 20 mph + ret.longitudinalTuning.kpV = [0., 4., 2.] # set lower end to 0 since we can't drive below that speed + ret.longitudinalTuning.kiBP = [0.] + ret.longitudinalTuning.kiV = [0.1] + return ret # returns a car.CarState @@ -37,6 +56,7 @@ class CarInterface(CarInterfaceBase): # TODO: add button types for inc and dec ret.buttonEvents = [ + *create_button_events(self.CS.cruise_buttons, self.CS.prev_cruise_buttons, BUTTONS_DICT), *create_button_events(self.CS.distance_button, self.CS.prev_distance_button, {1: ButtonType.gapAdjustCruise}), *create_button_events(self.CS.lkas_enabled, self.CS.lkas_previously_enabled, {1: FrogPilotButtonType.lkas}), ] diff --git a/selfdrive/car/mazda/mazdacan.py b/selfdrive/car/mazda/mazdacan.py index 74f6af0..1b1a186 100644 --- a/selfdrive/car/mazda/mazdacan.py +++ b/selfdrive/car/mazda/mazdacan.py @@ -1,4 +1,5 @@ from openpilot.selfdrive.car.mazda.values import Buttons, MazdaFlags +from openpilot.common.conversions import Conversions as CV def create_steering_control(packer, CP, frame, apply_steer, lkas): @@ -92,20 +93,22 @@ 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": 0, - "SET_P_INV": 1, + "SET_P": inc, + "SET_P_INV": (inc + 1) % 2, "RES": res, "RES_INV": (res + 1) % 2, - "SET_M": 0, - "SET_M_INV": 1, + "SET_M": dec, + "SET_M_INV": (dec + 1) % 2, "DISTANCE_LESS": 0, "DISTANCE_LESS_INV": 1, @@ -126,3 +129,33 @@ def create_button_cmd(packer, CP, counter, button): } return packer.make_can_msg("CRZ_BTNS", 0, values) + +def create_mazda_acc_spam_command(packer, controller, CS, slcSet, Vego, frogpilot_variables, accel): + cruiseBtn = Buttons.NONE + + MS_CONVERT = CV.MS_TO_KPH if frogpilot_variables.is_metric else CV.MS_TO_MPH + + speedSetPoint = int(round(CS.out.cruiseState.speed * MS_CONVERT)) + slcSet = int(round(slcSet * MS_CONVERT)) + + if not frogpilot_variables.experimentalMode: + if slcSet + 5 < Vego * MS_CONVERT: + slcSet = slcSet - 10 # 10 lower to increase deceleration until with 5 + else: + slcSet = int(round((Vego + 5 * accel) * MS_CONVERT)) + + if frogpilot_variables.is_metric: # Default is by 5 kph + slcSet = int(round(slcSet/5.0)*5.0) + speedSetPoint = int(round(speedSetPoint/5.0)*5.0) + + if slcSet < speedSetPoint and speedSetPoint > (30 if frogpilot_variables.is_metric else 20): + cruiseBtn = Buttons.SET_MINUS + elif slcSet > speedSetPoint: + cruiseBtn = Buttons.SET_PLUS + else: + cruiseBtn = Buttons.NONE + + if (cruiseBtn != Buttons.NONE): + return [create_button_cmd(packer, controller.CP, controller.frame // 10, cruiseBtn)] + else: + return [] diff --git a/selfdrive/frogpilot/ui/qt/offroad/vehicle_settings.cc b/selfdrive/frogpilot/ui/qt/offroad/vehicle_settings.cc index 5f8043a..42b3a01 100644 --- a/selfdrive/frogpilot/ui/qt/offroad/vehicle_settings.cc +++ b/selfdrive/frogpilot/ui/qt/offroad/vehicle_settings.cc @@ -113,7 +113,8 @@ FrogPilotVehiclesPanel::FrogPilotVehiclesPanel(SettingsWindow *parent) : FrogPil std::vector> vehicleToggles { {"LongPitch", tr("Long Pitch Compensation"), tr("Smoothen out the gas and pedal controls."), ""}, {"GasRegenCmd", tr("Truck Tune"), tr("Increase the acceleration and smoothen out the brake control when coming to a stop. For use on Silverado/Sierra only."), ""}, - {"CSLCEnabled", tr("GM CSLC"), tr("Set cars cruise speed based on SLC, MTSC, VTSC, & CEM.\n\nTurns OpenPilot Longitudnal Control off for camera ACC cars."), ""}, + + {"CSLCEnabled", tr("CSLC"), tr("Set cars cruise speed based on SLC, MTSC, VTSC, & CEM.\n\nTurns OpenPilot Longitudnal Control off."), ""}, {"CrosstrekTorque", tr("Subaru Crosstrek Torque Increase"), tr("Increases the maximum allowed torque for the Subaru Crosstrek."), ""}, @@ -243,6 +244,7 @@ void FrogPilotVehiclesPanel::hideToggles() { selectModelButton->setVisible(!carMake.isEmpty()); bool gm = carMake == "Buick" || carMake == "Cadillac" || carMake == "Chevrolet" || carMake == "GM" || carMake == "GMC"; + bool mazda = carMake == "Mazda"; bool subaru = carMake == "Subaru"; bool toyota = carMake == "Lexus" || carMake == "Toyota"; @@ -277,6 +279,8 @@ void FrogPilotVehiclesPanel::hideToggles() { toggle->setVisible(subaruKeys.find(key.c_str()) != subaruKeys.end()); } else if (toyota) { toggle->setVisible(toyotaKeys.find(key.c_str()) != toyotaKeys.end()); + } else if (mazda) { + toggle->setVisible(mazdaKeys.find(key.c_str()) != mazdaKeys.end()); } } } diff --git a/selfdrive/frogpilot/ui/qt/offroad/vehicle_settings.h b/selfdrive/frogpilot/ui/qt/offroad/vehicle_settings.h index d6a6401..698f86f 100644 --- a/selfdrive/frogpilot/ui/qt/offroad/vehicle_settings.h +++ b/selfdrive/frogpilot/ui/qt/offroad/vehicle_settings.h @@ -30,6 +30,7 @@ private: QStringList models; std::set gmKeys = {"CSLCEnabled", "GasRegenCmd", "LongPitch"}; + std::set mazdaKeys = {"CSLCEnabled"}; std::set subaruKeys = {"CrosstrekTorque"}; std::set toyotaKeys = {"ClusterOffset", "ToyotaTune", "SNGHack", "ToyotaDoors"};