diff --git a/common/params.cc b/common/params.cc index cb67360..b4b19b1 100644 --- a/common/params.cc +++ b/common/params.cc @@ -212,6 +212,7 @@ std::unordered_map keys = { {"WheeledBody", PERSISTENT}, // FrogPilot parameters + {"AccelerationProfile", PERSISTENT}, {"FrogPilotTogglesUpdated", PERSISTENT}, {"LateralTune", PERSISTENT}, {"LongitudinalTune", PERSISTENT}, diff --git a/panda/board/safety/safety_honda.h b/panda/board/safety/safety_honda.h index dde27cd..c8071af 100644 --- a/panda/board/safety/safety_honda.h +++ b/panda/board/safety/safety_honda.h @@ -12,7 +12,7 @@ const int HONDA_GAS_INTERCEPTOR_THRESHOLD = 492; #define HONDA_GET_INTERCEPTOR(msg) (((GET_BYTE((msg), 0) << 8) + GET_BYTE((msg), 1) + (GET_BYTE((msg), 2) << 8) + GET_BYTE((msg), 3)) / 2U) // avg between 2 tracks const LongitudinalLimits HONDA_BOSCH_LONG_LIMITS = { - .max_accel = 200, // accel is used for brakes + .max_accel = 400, // accel is used for brakes .min_accel = -350, .max_gas = 2000, diff --git a/panda/board/safety/safety_hyundai.h b/panda/board/safety/safety_hyundai.h index 1f468e6..cc2dc41 100644 --- a/panda/board/safety/safety_hyundai.h +++ b/panda/board/safety/safety_hyundai.h @@ -21,7 +21,7 @@ const SteeringLimits HYUNDAI_STEERING_LIMITS = HYUNDAI_LIMITS(384, 3, 7); const SteeringLimits HYUNDAI_STEERING_LIMITS_ALT = HYUNDAI_LIMITS(270, 2, 3); const LongitudinalLimits HYUNDAI_LONG_LIMITS = { - .max_accel = 200, // 1/100 m/s2 + .max_accel = 400, // 1/100 m/s2 .min_accel = -350, // 1/100 m/s2 }; diff --git a/panda/board/safety/safety_toyota.h b/panda/board/safety/safety_toyota.h index fc87c48..4f33e8f 100644 --- a/panda/board/safety/safety_toyota.h +++ b/panda/board/safety/safety_toyota.h @@ -33,7 +33,7 @@ const int TOYOTA_LTA_MAX_DRIVER_TORQUE = 150; // longitudinal limits const LongitudinalLimits TOYOTA_LONG_LIMITS = { - .max_accel = 2000, // 2.0 m/s2 + .max_accel = 4000, // 4.0 m/s2 .min_accel = -3500, // -3.5 m/s2 }; diff --git a/panda/board/safety/safety_volkswagen_mqb.h b/panda/board/safety/safety_volkswagen_mqb.h index f671443..91764f4 100644 --- a/panda/board/safety/safety_volkswagen_mqb.h +++ b/panda/board/safety/safety_volkswagen_mqb.h @@ -15,7 +15,7 @@ const SteeringLimits VOLKSWAGEN_MQB_STEERING_LIMITS = { // longitudinal limits // acceleration in m/s2 * 1000 to avoid floating point math const LongitudinalLimits VOLKSWAGEN_MQB_LONG_LIMITS = { - .max_accel = 2000, + .max_accel = 4000, .min_accel = -3500, .inactive_accel = 3010, // VW sends one increment above the max range when inactive }; @@ -231,7 +231,9 @@ static bool volkswagen_mqb_tx_hook(CANPacket_t *to_send) { desired_accel = (((GET_BYTE(to_send, 7) << 3) | ((GET_BYTE(to_send, 6) & 0xE0U) >> 5)) * 5U) - 7220U; } - violation |= longitudinal_accel_checks(desired_accel, VOLKSWAGEN_MQB_LONG_LIMITS); + if (desired_accel != 0) { + violation |= longitudinal_accel_checks(desired_accel, VOLKSWAGEN_MQB_LONG_LIMITS); + } if (violation) { tx = false; diff --git a/panda/board/safety/safety_volkswagen_pq.h b/panda/board/safety/safety_volkswagen_pq.h index b81e0f0..a373479 100644 --- a/panda/board/safety/safety_volkswagen_pq.h +++ b/panda/board/safety/safety_volkswagen_pq.h @@ -15,7 +15,7 @@ const SteeringLimits VOLKSWAGEN_PQ_STEERING_LIMITS = { // longitudinal limits // acceleration in m/s2 * 1000 to avoid floating point math const LongitudinalLimits VOLKSWAGEN_PQ_LONG_LIMITS = { - .max_accel = 2000, + .max_accel = 4000, .min_accel = -3500, .inactive_accel = 3010, // VW sends one increment above the max range when inactive }; diff --git a/selfdrive/car/body/interface.py b/selfdrive/car/body/interface.py index 12a2d5f..652b49b 100644 --- a/selfdrive/car/body/interface.py +++ b/selfdrive/car/body/interface.py @@ -42,5 +42,5 @@ class CarInterface(CarInterfaceBase): return ret - def apply(self, c, now_nanos): + def apply(self, c, now_nanos, sport_plus): return self.CC.update(c, self.CS, now_nanos) diff --git a/selfdrive/car/chrysler/interface.py b/selfdrive/car/chrysler/interface.py index 10cdd0f..a397e85 100755 --- a/selfdrive/car/chrysler/interface.py +++ b/selfdrive/car/chrysler/interface.py @@ -105,5 +105,5 @@ class CarInterface(CarInterfaceBase): return ret - def apply(self, c, now_nanos): + def apply(self, c, now_nanos, sport_plus): return self.CC.update(c, self.CS, now_nanos) diff --git a/selfdrive/car/ford/carcontroller.py b/selfdrive/car/ford/carcontroller.py index 88ec6a5..211183c 100644 --- a/selfdrive/car/ford/carcontroller.py +++ b/selfdrive/car/ford/carcontroller.py @@ -35,7 +35,7 @@ class CarController: self.lkas_enabled_last = False self.steer_alert_last = False - def update(self, CC, CS, now_nanos): + def update(self, CC, CS, now_nanos, sport_plus): can_sends = [] actuators = CC.actuators @@ -85,7 +85,10 @@ class CarController: # send acc msg at 50Hz if self.CP.openpilotLongitudinalControl and (self.frame % CarControllerParams.ACC_CONTROL_STEP) == 0: # Both gas and accel are in m/s^2, accel is used solely for braking - accel = clip(actuators.accel, CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX) + if sport_plus: + accel = clip(actuators.accel, CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX_PLUS) + else: + accel = clip(actuators.accel, CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX) gas = accel if not CC.longActive or gas < CarControllerParams.MIN_GAS: gas = CarControllerParams.INACTIVE_GAS diff --git a/selfdrive/car/ford/interface.py b/selfdrive/car/ford/interface.py index 8a8ce36..3de62d6 100644 --- a/selfdrive/car/ford/interface.py +++ b/selfdrive/car/ford/interface.py @@ -112,5 +112,5 @@ class CarInterface(CarInterfaceBase): return ret - def apply(self, c, now_nanos): - return self.CC.update(c, self.CS, now_nanos) + def apply(self, c, now_nanos, sport_plus): + return self.CC.update(c, self.CS, now_nanos, sport_plus) diff --git a/selfdrive/car/ford/values.py b/selfdrive/car/ford/values.py index 2c4415b..3f47c53 100644 --- a/selfdrive/car/ford/values.py +++ b/selfdrive/car/ford/values.py @@ -32,6 +32,7 @@ class CarControllerParams: CURVATURE_ERROR = 0.002 # ~6 degrees at 10 m/s, ~10 degrees at 35 m/s ACCEL_MAX = 2.0 # m/s^2 max acceleration + ACCEL_MAX_PLUS = 4.0 # m/s^2 max acceleration ACCEL_MIN = -3.5 # m/s^2 max deceleration MIN_GAS = -0.5 INACTIVE_GAS = -5.0 diff --git a/selfdrive/car/gm/interface.py b/selfdrive/car/gm/interface.py index 3e18a7d..4807e94 100755 --- a/selfdrive/car/gm/interface.py +++ b/selfdrive/car/gm/interface.py @@ -30,8 +30,11 @@ NON_LINEAR_TORQUE_PARAMS = { class CarInterface(CarInterfaceBase): @staticmethod - def get_pid_accel_limits(CP, current_speed, cruise_speed): - return CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX + def get_pid_accel_limits(CP, current_speed, cruise_speed, sport_plus): + if sport_plus: + return CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX_PLUS + else: + return CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX # Determined by iteratively plotting and minimizing error for f(angle, speed) = steer. @staticmethod @@ -291,5 +294,5 @@ class CarInterface(CarInterfaceBase): return ret - def apply(self, c, now_nanos): + def apply(self, c, now_nanos, sport_plus): return self.CC.update(c, self.CS, now_nanos) diff --git a/selfdrive/car/gm/values.py b/selfdrive/car/gm/values.py index dbbf151..65d2d75 100644 --- a/selfdrive/car/gm/values.py +++ b/selfdrive/car/gm/values.py @@ -30,6 +30,7 @@ class CarControllerParams: # Our controller should still keep the 2 second average above # -3.5 m/s^2 as per planner limits ACCEL_MAX = 2. # m/s^2 + ACCEL_MAX_PLUS = 4. # m/s^2 ACCEL_MIN = -4. # m/s^2 def __init__(self, CP): diff --git a/selfdrive/car/honda/carcontroller.py b/selfdrive/car/honda/carcontroller.py index 056b47c..26facb0 100644 --- a/selfdrive/car/honda/carcontroller.py +++ b/selfdrive/car/honda/carcontroller.py @@ -124,7 +124,7 @@ class CarController: self.brake = 0.0 self.last_steer = 0.0 - def update(self, CC, CS, now_nanos): + def update(self, CC, CS, now_nanos, sport_plus): actuators = CC.actuators hud_control = CC.hudControl conversion = hondacan.get_cruise_speed_conversion(self.CP.carFingerprint, CS.is_metric) @@ -214,7 +214,10 @@ class CarController: ts = self.frame * DT_CTRL if self.CP.carFingerprint in HONDA_BOSCH: - self.accel = clip(accel, self.params.BOSCH_ACCEL_MIN, self.params.BOSCH_ACCEL_MAX) + if sport_plus: + self.accel = clip(accel, self.params.BOSCH_ACCEL_MIN, self.params.BOSCH_ACCEL_MAX_PLUS) + else: + self.accel = clip(accel, self.params.BOSCH_ACCEL_MIN, self.params.BOSCH_ACCEL_MAX) self.gas = interp(accel, self.params.BOSCH_GAS_LOOKUP_BP, self.params.BOSCH_GAS_LOOKUP_V) stopping = actuators.longControlState == LongCtrlState.stopping diff --git a/selfdrive/car/honda/interface.py b/selfdrive/car/honda/interface.py index 9f228cd..7845031 100755 --- a/selfdrive/car/honda/interface.py +++ b/selfdrive/car/honda/interface.py @@ -19,11 +19,17 @@ BUTTONS_DICT = {CruiseButtons.RES_ACCEL: ButtonType.accelCruise, CruiseButtons.D class CarInterface(CarInterfaceBase): @staticmethod - def get_pid_accel_limits(CP, current_speed, cruise_speed): + def get_pid_accel_limits(CP, current_speed, cruise_speed, sport_plus): if CP.carFingerprint in HONDA_BOSCH: - return CarControllerParams.BOSCH_ACCEL_MIN, CarControllerParams.BOSCH_ACCEL_MAX + if sport_plus: + return CarControllerParams.BOSCH_ACCEL_MIN, CarControllerParams.BOSCH_ACCEL_MAX_PLUS + else: + return CarControllerParams.BOSCH_ACCEL_MIN, CarControllerParams.BOSCH_ACCEL_MAX elif CP.enableGasInterceptor: - return CarControllerParams.NIDEC_ACCEL_MIN, CarControllerParams.NIDEC_ACCEL_MAX + if sport_plus: + return CarControllerParams.NIDEC_ACCEL_MIN, CarControllerParams.NIDEC_ACCEL_MAX_PLUS + else: + return CarControllerParams.NIDEC_ACCEL_MIN, CarControllerParams.NIDEC_ACCEL_MAX else: # NIDECs don't allow acceleration near cruise_speed, # so limit limits of pid to prevent windup @@ -342,5 +348,5 @@ class CarInterface(CarInterfaceBase): # pass in a car.CarControl # to be called @ 100hz - def apply(self, c, now_nanos): - return self.CC.update(c, self.CS, now_nanos) + def apply(self, c, now_nanos, sport_plus): + return self.CC.update(c, self.CS, now_nanos, sport_plus) diff --git a/selfdrive/car/honda/values.py b/selfdrive/car/honda/values.py index a10080e..96ea0a5 100644 --- a/selfdrive/car/honda/values.py +++ b/selfdrive/car/honda/values.py @@ -21,6 +21,7 @@ class CarControllerParams: # -3.5 m/s^2 as per planner limits NIDEC_ACCEL_MIN = -4.0 # m/s^2 NIDEC_ACCEL_MAX = 1.6 # m/s^2, lower than 2.0 m/s^2 for tuning reasons + NIDEC_ACCEL_MAX_PLUS = 4.0 # m/s^2 NIDEC_ACCEL_LOOKUP_BP = [-1., 0., .6] NIDEC_ACCEL_LOOKUP_V = [-4.8, 0., 2.0] @@ -33,6 +34,7 @@ class CarControllerParams: BOSCH_ACCEL_MIN = -3.5 # m/s^2 BOSCH_ACCEL_MAX = 2.0 # m/s^2 + BOSCH_ACCEL_MAX_PLUS = 4.0 # m/s^2 BOSCH_GAS_LOOKUP_BP = [-0.2, 2.0] # 2m/s^2 BOSCH_GAS_LOOKUP_V = [0, 1600] diff --git a/selfdrive/car/hyundai/carcontroller.py b/selfdrive/car/hyundai/carcontroller.py index 0b5f724..e956678 100644 --- a/selfdrive/car/hyundai/carcontroller.py +++ b/selfdrive/car/hyundai/carcontroller.py @@ -56,7 +56,7 @@ class CarController: self.car_fingerprint = CP.carFingerprint self.last_button_frame = 0 - def update(self, CC, CS, now_nanos): + def update(self, CC, CS, now_nanos, sport_plus): actuators = CC.actuators hud_control = CC.hudControl @@ -78,7 +78,10 @@ class CarController: self.apply_steer_last = apply_steer # accel + longitudinal - accel = clip(actuators.accel, CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX) + if sport_plus: + accel = clip(actuators.accel, CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX_PLUS) + else: + accel = clip(actuators.accel, CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX) stopping = actuators.longControlState == LongCtrlState.stopping set_speed_in_units = hud_control.setSpeed * (CV.MS_TO_KPH if CS.is_metric else CV.MS_TO_MPH) diff --git a/selfdrive/car/hyundai/interface.py b/selfdrive/car/hyundai/interface.py index cd6657a..3099d87 100644 --- a/selfdrive/car/hyundai/interface.py +++ b/selfdrive/car/hyundai/interface.py @@ -362,5 +362,5 @@ class CarInterface(CarInterfaceBase): return ret - def apply(self, c, now_nanos): - return self.CC.update(c, self.CS, now_nanos) + def apply(self, c, now_nanos, sport_plus): + return self.CC.update(c, self.CS, now_nanos, sport_plus) diff --git a/selfdrive/car/hyundai/values.py b/selfdrive/car/hyundai/values.py index 4b92bc5..f013365 100644 --- a/selfdrive/car/hyundai/values.py +++ b/selfdrive/car/hyundai/values.py @@ -16,6 +16,7 @@ Ecu = car.CarParams.Ecu class CarControllerParams: ACCEL_MIN = -3.5 # m/s ACCEL_MAX = 2.0 # m/s + ACCEL_MAX_PLUS = 4.0 # m/s def __init__(self, CP): self.STEER_DELTA_UP = 3 diff --git a/selfdrive/car/interfaces.py b/selfdrive/car/interfaces.py index b3a7816..070a6f0 100644 --- a/selfdrive/car/interfaces.py +++ b/selfdrive/car/interfaces.py @@ -25,6 +25,7 @@ TorqueFromLateralAccelCallbackType = Callable[[float, car.CarParams.LateralTorqu MAX_CTRL_SPEED = (V_CRUISE_MAX + 4) * CV.KPH_TO_MS ACCEL_MAX = 2.0 +ACCEL_MAX_PLUS = 4.0 ACCEL_MIN = -3.5 FRICTION_THRESHOLD = 0.3 @@ -91,8 +92,11 @@ class CarInterfaceBase(ABC): params = Params() @staticmethod - def get_pid_accel_limits(CP, current_speed, cruise_speed): - return ACCEL_MIN, ACCEL_MAX + def get_pid_accel_limits(CP, current_speed, cruise_speed, sport_plus): + if sport_plus: + return ACCEL_MIN, ACCEL_MAX_PLUS + else: + return ACCEL_MIN, ACCEL_MAX @classmethod def get_non_essential_params(cls, candidate: str): diff --git a/selfdrive/car/mazda/interface.py b/selfdrive/car/mazda/interface.py index 7ac93d9..4a7b398 100755 --- a/selfdrive/car/mazda/interface.py +++ b/selfdrive/car/mazda/interface.py @@ -64,5 +64,5 @@ class CarInterface(CarInterfaceBase): return ret - def apply(self, c, now_nanos): + def apply(self, c, now_nanos, sport_plus): return self.CC.update(c, self.CS, now_nanos) diff --git a/selfdrive/car/mock/interface.py b/selfdrive/car/mock/interface.py index c7821bd..db55a24 100755 --- a/selfdrive/car/mock/interface.py +++ b/selfdrive/car/mock/interface.py @@ -30,6 +30,6 @@ class CarInterface(CarInterfaceBase): return ret - def apply(self, c, now_nanos): + def apply(self, c, now_nanos, sport_plus): actuators = car.CarControl.Actuators.new_message() return actuators, [] diff --git a/selfdrive/car/nissan/interface.py b/selfdrive/car/nissan/interface.py index aedcaa1..5037f27 100644 --- a/selfdrive/car/nissan/interface.py +++ b/selfdrive/car/nissan/interface.py @@ -56,5 +56,5 @@ class CarInterface(CarInterfaceBase): return ret - def apply(self, c, now_nanos): + def apply(self, c, now_nanos, sport_plus): return self.CC.update(c, self.CS, now_nanos) diff --git a/selfdrive/car/subaru/interface.py b/selfdrive/car/subaru/interface.py index 1296aea..875eeab 100644 --- a/selfdrive/car/subaru/interface.py +++ b/selfdrive/car/subaru/interface.py @@ -149,5 +149,5 @@ class CarInterface(CarInterfaceBase): if CP.flags & SubaruFlags.DISABLE_EYESIGHT: disable_ecu(logcan, sendcan, bus=2, addr=GLOBAL_ES_ADDR, com_cont_req=b'\x28\x03\x01') - def apply(self, c, now_nanos): + def apply(self, c, now_nanos, sport_plus): return self.CC.update(c, self.CS, now_nanos) diff --git a/selfdrive/car/tesla/interface.py b/selfdrive/car/tesla/interface.py index e061397..bfa49c8 100755 --- a/selfdrive/car/tesla/interface.py +++ b/selfdrive/car/tesla/interface.py @@ -58,5 +58,5 @@ class CarInterface(CarInterfaceBase): return ret - def apply(self, c, now_nanos): + def apply(self, c, now_nanos, sport_plus): return self.CC.update(c, self.CS, now_nanos) diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py index 98beaf8..bd64d98 100644 --- a/selfdrive/car/toyota/carcontroller.py +++ b/selfdrive/car/toyota/carcontroller.py @@ -45,7 +45,7 @@ class CarController: def update_frogpilot_variables(self, params): - def update(self, CC, CS, now_nanos): + def update(self, CC, CS, now_nanos, sport_plus): actuators = CC.actuators hud_control = CC.hudControl pcm_cancel_cmd = CC.cruiseControl.cancel @@ -118,7 +118,10 @@ class CarController: interceptor_gas_cmd = clip(pedal_command, 0., MAX_INTERCEPTOR_GAS) else: interceptor_gas_cmd = 0. - pcm_accel_cmd = clip(actuators.accel, self.params.ACCEL_MIN, self.params.ACCEL_MAX) + if sport_plus: + pcm_accel_cmd = clip(actuators.accel, self.params.ACCEL_MIN, self.params.ACCEL_MAX_PLUS) + else: + pcm_accel_cmd = clip(actuators.accel, self.params.ACCEL_MIN, self.params.ACCEL_MAX) # TODO: probably can delete this. CS.pcm_acc_status uses a different signal # than CS.cruiseState.enabled. confirm they're not meaningfully different diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py index a0da6de..8dad552 100644 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -15,8 +15,11 @@ SteerControlType = car.CarParams.SteerControlType class CarInterface(CarInterfaceBase): @staticmethod - def get_pid_accel_limits(CP, current_speed, cruise_speed): - return CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX + def get_pid_accel_limits(CP, current_speed, cruise_speed, sport_plus): + if sport_plus: + return CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX_PLUS + else: + return CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX @staticmethod def _get_params(ret, candidate, fingerprint, car_fw, experimental_long, docs): @@ -316,5 +319,5 @@ class CarInterface(CarInterfaceBase): # pass in a car.CarControl # to be called @ 100hz - def apply(self, c, now_nanos): - return self.CC.update(c, self.CS, now_nanos) + def apply(self, c, now_nanos, sport_plus): + return self.CC.update(c, self.CS, now_nanos, sport_plus) diff --git a/selfdrive/car/toyota/values.py b/selfdrive/car/toyota/values.py index b3b6fd7..af768fe 100644 --- a/selfdrive/car/toyota/values.py +++ b/selfdrive/car/toyota/values.py @@ -17,6 +17,7 @@ PEDAL_TRANSITION = 10. * CV.MPH_TO_MS class CarControllerParams: ACCEL_MAX = 1.5 # m/s2, lower than allowed 2.0 m/s2 for tuning reasons + ACCEL_MAX_PLUS = 4.0 ACCEL_MIN = -3.5 # m/s2 STEER_STEP = 1 diff --git a/selfdrive/car/volkswagen/carcontroller.py b/selfdrive/car/volkswagen/carcontroller.py index 0cee2c7..126e00d 100644 --- a/selfdrive/car/volkswagen/carcontroller.py +++ b/selfdrive/car/volkswagen/carcontroller.py @@ -25,7 +25,7 @@ class CarController: self.hca_frame_timer_running = 0 self.hca_frame_same_torque = 0 - def update(self, CC, CS, ext_bus, now_nanos): + def update(self, CC, CS, ext_bus, now_nanos, sport_plus): actuators = CC.actuators hud_control = CC.hudControl can_sends = [] @@ -78,7 +78,10 @@ class CarController: if self.frame % self.CCP.ACC_CONTROL_STEP == 0 and self.CP.openpilotLongitudinalControl: acc_control = self.CCS.acc_control_value(CS.out.cruiseState.available, CS.out.accFaulted, CC.longActive) - accel = clip(actuators.accel, self.CCP.ACCEL_MIN, self.CCP.ACCEL_MAX) if CC.longActive else 0 + if sport_plus: + accel = clip(actuators.accel, self.CCP.ACCEL_MIN, self.CCP.ACCEL_MAX_PLUS) if CC.longActive else 0 + else: + accel = clip(actuators.accel, self.CCP.ACCEL_MIN, self.CCP.ACCEL_MAX) if CC.longActive else 0 stopping = actuators.longControlState == LongCtrlState.stopping starting = actuators.longControlState == LongCtrlState.pid and (CS.esp_hold_confirmation or CS.out.vEgo < self.CP.vEgoStopping) can_sends.extend(self.CCS.create_acc_accel_control(self.packer_pt, CANBUS.pt, CS.acc_type, CC.longActive, accel, diff --git a/selfdrive/car/volkswagen/interface.py b/selfdrive/car/volkswagen/interface.py index 710e779..4d38b65 100644 --- a/selfdrive/car/volkswagen/interface.py +++ b/selfdrive/car/volkswagen/interface.py @@ -253,6 +253,6 @@ class CarInterface(CarInterfaceBase): return ret - def apply(self, c, now_nanos): - new_actuators, can_sends, self.eps_timer_soft_disable_alert = self.CC.update(c, self.CS, self.ext_bus, now_nanos) + def apply(self, c, now_nanos, sport_plus): + new_actuators, can_sends, self.eps_timer_soft_disable_alert = self.CC.update(c, self.CS, self.ext_bus, now_nanos, sport_plus) return new_actuators, can_sends diff --git a/selfdrive/car/volkswagen/values.py b/selfdrive/car/volkswagen/values.py index f4809e4..edb9100 100644 --- a/selfdrive/car/volkswagen/values.py +++ b/selfdrive/car/volkswagen/values.py @@ -35,6 +35,7 @@ class CarControllerParams: STEER_TIME_STUCK_TORQUE = 1.9 # EPS limits same torque to 6 seconds, reset timer 3x within that period ACCEL_MAX = 2.0 # 2.0 m/s max acceleration + ACCEL_MAX_PLUS = 4.0 # 4.0 m/s max acceleration ACCEL_MIN = -3.5 # 3.5 m/s max deceleration def __init__(self, CP): diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 0fda87e..e6ba4fc 100644 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -108,6 +108,8 @@ class Controls: if not self.disengage_on_accelerator: self.CP.alternativeExperience |= ALTERNATIVE_EXPERIENCE.DISABLE_DISENGAGE_ON_GAS + self.CP.alternativeExperience |= ALTERNATIVE_EXPERIENCE.RAISE_LONGITUDINAL_LIMITS_TO_ISO_MAX + # read params self.is_metric = self.params.get_bool("IsMetric") self.is_ldw_enabled = self.params.get_bool("IsLdwEnabled") @@ -620,7 +622,7 @@ class Controls: if not self.joystick_mode: # accel PID loop - pid_accel_limits = self.CI.get_pid_accel_limits(self.CP, CS.vEgo, self.v_cruise_helper.v_cruise_kph * CV.KPH_TO_MS) + pid_accel_limits = self.CI.get_pid_accel_limits(self.CP, CS.vEgo, self.v_cruise_helper.v_cruise_kph * CV.KPH_TO_MS, self.sport_plus) t_since_plan = (self.sm.frame - self.sm.rcv_frame['longitudinalPlan']) * DT_CTRL actuators.accel = self.LoC.update(CC.longActive, CS, long_plan, pid_accel_limits, t_since_plan) @@ -764,7 +766,7 @@ class Controls: if not self.CP.passive and self.initialized: # send car controls over can now_nanos = self.can_log_mono_time if REPLAY else int(time.monotonic() * 1e9) - self.last_actuators, can_sends = self.CI.apply(CC, now_nanos) + self.last_actuators, can_sends = self.CI.apply(CC, now_nanos, self.sport_plus) self.pm.send('sendcan', can_list_to_can_capnp(can_sends, msgtype='sendcan', valid=CS.canValid)) CC.actuatorsOutput = self.last_actuators if self.CP.steerControlType == car.CarParams.SteerControlType.angle: @@ -920,6 +922,7 @@ class Controls: obj.update_frogpilot_params(self.params) longitudinal_tune = self.params.get_bool("LongitudinalTune") + self.sport_plus = self.params.get_int("AccelerationProfile") == 3 and longitudinal_tune def main(): controls = Controls() diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 731eac5..c02d30d 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -109,7 +109,10 @@ class LongitudinalPlanner: # No change cost when user is controlling the speed, or when standstill prev_accel_constraint = not (reset_state or sm['carState'].standstill) - if self.mpc.mode == 'acc': + if frogpilot_planner.acceleration_profile: + accel_limits = frogpilot_planner.accel_limits + accel_limits_turns = limit_accel_in_turns(v_ego, sm['carState'].steeringAngleDeg, accel_limits, self.CP) + elif self.mpc.mode == 'acc': accel_limits = [A_CRUISE_MIN, get_max_accel(v_ego)] accel_limits_turns = limit_accel_in_turns(v_ego, sm['carState'].steeringAngleDeg, accel_limits, self.CP) else: diff --git a/selfdrive/frogpilot/functions/frogpilot_planner.py b/selfdrive/frogpilot/functions/frogpilot_planner.py index 9e4cbaf..b29bcae 100644 --- a/selfdrive/frogpilot/functions/frogpilot_planner.py +++ b/selfdrive/frogpilot/functions/frogpilot_planner.py @@ -1,7 +1,33 @@ import cereal.messaging as messaging from openpilot.common.conversions import Conversions as CV +from openpilot.common.numpy_fast import interp from openpilot.selfdrive.controls.lib.drive_helpers import V_CRUISE_MAX +from openpilot.selfdrive.controls.lib.longitudinal_planner import A_CRUISE_MIN, A_CRUISE_MAX_VALS, A_CRUISE_MAX_BP, get_max_accel + +# Acceleration profiles - Credit goes to the DragonPilot team! + # MPH = [0., 35, 35, 40, 40, 45, 45, 67, 67, 67, 123] +A_CRUISE_MIN_BP_CUSTOM = [0., 2.0, 2.01, 11., 11.01, 18., 18.01, 28., 28.01, 33., 55.] + # MPH = [0., 6.71, 13.4, 17.9, 24.6, 33.6, 44.7, 55.9, 67.1, 123] +A_CRUISE_MAX_BP_CUSTOM = [0., 3, 6., 8., 11., 15., 20., 25., 30., 55.] + +A_CRUISE_MIN_VALS_ECO_TUNE = [-0.480, -0.480, -0.40, -0.40, -0.40, -0.36, -0.32, -0.28, -0.28, -0.25, -0.25] +A_CRUISE_MAX_VALS_ECO_TUNE = [3.5, 3.3, 1.7, 1.1, .76, .62, .47, .36, .28, .09] + +A_CRUISE_MIN_VALS_SPORT_TUNE = [-0.500, -0.500, -0.42, -0.42, -0.42, -0.42, -0.40, -0.35, -0.35, -0.30, -0.30] +A_CRUISE_MAX_VALS_SPORT_TUNE = [3.5, 3.5, 3.0, 2.6, 1.4, 1.0, 0.7, 0.6, .38, .2] + +def get_min_accel_eco_tune(v_ego): + return interp(v_ego, A_CRUISE_MIN_BP_CUSTOM, A_CRUISE_MIN_VALS_ECO_TUNE) + +def get_max_accel_eco_tune(v_ego): + return interp(v_ego, A_CRUISE_MAX_BP_CUSTOM, A_CRUISE_MAX_VALS_ECO_TUNE) + +def get_min_accel_sport_tune(v_ego): + return interp(v_ego, A_CRUISE_MIN_BP_CUSTOM, A_CRUISE_MIN_VALS_SPORT_TUNE) + +def get_max_accel_sport_tune(v_ego): + return interp(v_ego, A_CRUISE_MAX_BP_CUSTOM, A_CRUISE_MAX_VALS_SPORT_TUNE) class FrogPilotPlanner: @@ -19,6 +45,14 @@ class FrogPilotPlanner: v_cruise = v_cruise_kph * CV.KPH_TO_MS v_ego = carState.vEgo + # Acceleration profiles + if self.acceleration_profile == 1: + self.accel_limits = [get_min_accel_eco_tune(v_ego), get_max_accel_eco_tune(v_ego)] + elif self.acceleration_profile in (2, 3): + self.accel_limits = [get_min_accel_sport_tune(v_ego), get_max_accel_sport_tune(v_ego)] + else: + self.accel_limits = [A_CRUISE_MIN, get_max_accel(v_ego)] + self.v_cruise = self.update_v_cruise(carState, controlsState, modelData, enabled, v_cruise, v_ego) def update_v_cruise(self, carState, controlsState, modelData, enabled, v_cruise, v_ego): @@ -45,3 +79,4 @@ class FrogPilotPlanner: lateral_tune = params.get_bool("LateralTune") longitudinal_tune = params.get_bool("LongitudinalTune") + self.acceleration_profile = params.get_int("AccelerationProfile") if longitudinal_tune else 0 diff --git a/selfdrive/frogpilot/ui/control_settings.cc b/selfdrive/frogpilot/ui/control_settings.cc index 68bde08..f991f08 100644 --- a/selfdrive/frogpilot/ui/control_settings.cc +++ b/selfdrive/frogpilot/ui/control_settings.cc @@ -6,6 +6,7 @@ FrogPilotControlsPanel::FrogPilotControlsPanel(SettingsWindow *parent) : FrogPil {"LateralTune", "Lateral Tuning", "Modify openpilot's steering behavior.", "../frogpilot/assets/toggle_icons/icon_lateral_tune.png"}, {"LongitudinalTune", "Longitudinal Tuning", "Modify openpilot's acceleration and braking behavior.", "../frogpilot/assets/toggle_icons/icon_longitudinal_tune.png"}, + {"AccelerationProfile", "Acceleration Profile", "Change the acceleration rate to be either sporty or eco-friendly.", ""}, }; for (const auto &[param, title, desc, icon] : controlToggles) { @@ -30,6 +31,17 @@ FrogPilotControlsPanel::FrogPilotControlsPanel(SettingsWindow *parent) : FrogPil } }); toggle = longitudinalTuneToggle; + } else if (param == "AccelerationProfile") { + std::vector profileOptions{tr("Standard"), tr("Eco"), tr("Sport"), tr("Sport+")}; + FrogPilotButtonParamControl *profileSelection = new FrogPilotButtonParamControl(param, title, desc, icon, profileOptions); + toggle = profileSelection; + + QObject::connect(static_cast(toggle), &FrogPilotButtonParamControl::buttonClicked, [this](int id) { + if (id == 3) { + FrogPilotConfirmationDialog::toggleAlert("WARNING: This maxes out openpilot's acceleration from 2.0 m/s to 4.0 m/s and may cause oscillations when accelerating!", + "I understand the risks.", this); + } + }); } else { toggle = new ParamControl(param, title, desc, icon, this); @@ -68,7 +80,7 @@ FrogPilotControlsPanel::FrogPilotControlsPanel(SettingsWindow *parent) : FrogPil fireTheBabysitterKeys = {}; laneChangeKeys = {}; lateralTuneKeys = {}; - longitudinalTuneKeys = {}; + longitudinalTuneKeys = {"AccelerationProfile"}; speedLimitControllerKeys = {}; visionTurnControlKeys = {};