diff --git a/common/params.cc b/common/params.cc index 0eb18ef..38060ca 100644 --- a/common/params.cc +++ b/common/params.cc @@ -210,8 +210,10 @@ std::unordered_map keys = { // FrogPilot parameters {"AccelerationPath", PERSISTENT}, + {"AccelerationProfile", PERSISTENT}, {"CustomAlerts", PERSISTENT}, {"CustomUI", PERSISTENT}, + {"DecelerationProfile", PERSISTENT}, {"FrogPilotTogglesUpdated", PERSISTENT}, {"FrogsGoMoo", PERSISTENT}, {"LateralTune", PERSISTENT}, diff --git a/panda/board/safety/safety_gm.h b/panda/board/safety/safety_gm.h index 82a5d9c..357c970 100644 --- a/panda/board/safety/safety_gm.h +++ b/panda/board/safety/safety_gm.h @@ -16,6 +16,13 @@ const LongitudinalLimits GM_ASCM_LONG_LIMITS = { .max_brake = 400, }; +const LongitudinalLimits GM_ASCM_LONG_LIMITS_SPORT = { + .max_gas = 8191, + .min_gas = 1404, + .inactive_gas = 1404, + .max_brake = 400, +}; + const LongitudinalLimits GM_CAM_LONG_LIMITS = { .max_gas = 3400, .min_gas = 1514, @@ -23,6 +30,13 @@ const LongitudinalLimits GM_CAM_LONG_LIMITS = { .max_brake = 400, }; +const LongitudinalLimits GM_CAM_LONG_LIMITS_SPORT = { + .max_gas = 8848, + .min_gas = 1514, + .inactive_gas = 1554, + .max_brake = 400, +}; + const LongitudinalLimits *gm_long_limits; const int GM_STANDSTILL_THRSLD = 10; // 0.311kph @@ -215,12 +229,22 @@ static int gm_fwd_hook(int bus_num, int addr) { } static safety_config gm_init(uint16_t param) { + sport_mode = alternative_experience & ALT_EXP_RAISE_LONGITUDINAL_LIMITS_TO_ISO_MAX; + gm_hw = GET_FLAG(param, GM_PARAM_HW_CAM) ? GM_CAM : GM_ASCM; if (gm_hw == GM_ASCM) { - gm_long_limits = &GM_ASCM_LONG_LIMITS; + if (sport_mode) { + gm_long_limits = &GM_ASCM_LONG_LIMITS_SPORT; + } else { + gm_long_limits = &GM_ASCM_LONG_LIMITS; + } } else if (gm_hw == GM_CAM) { - gm_long_limits = &GM_CAM_LONG_LIMITS; + if (sport_mode) { + gm_long_limits = &GM_CAM_LONG_LIMITS_SPORT; + } else { + gm_long_limits = &GM_CAM_LONG_LIMITS; + } } else { } diff --git a/panda/board/safety/safety_honda.h b/panda/board/safety/safety_honda.h index 78bbb7f..b79b6c4 100644 --- a/panda/board/safety/safety_honda.h +++ b/panda/board/safety/safety_honda.h @@ -19,6 +19,14 @@ const LongitudinalLimits HONDA_BOSCH_LONG_LIMITS = { .inactive_gas = -30000, }; +const LongitudinalLimits HONDA_BOSCH_LONG_LIMITS_SPORT = { + .max_accel = 400, // accel is used for brakes + .min_accel = -350, + + .max_gas = 2000, + .inactive_gas = -30000, +}; + const LongitudinalLimits HONDA_NIDEC_LONG_LIMITS = { .max_gas = 198, // 0xc6 .max_brake = 255, @@ -270,6 +278,8 @@ static void honda_rx_hook(const CANPacket_t *to_push) { } static bool honda_tx_hook(const CANPacket_t *to_send) { + sport_mode = alternative_experience & ALT_EXP_RAISE_LONGITUDINAL_LIMITS_TO_ISO_MAX; + bool tx = true; int addr = GET_ADDR(to_send); int bus = GET_BUS(to_send); @@ -310,8 +320,13 @@ static bool honda_tx_hook(const CANPacket_t *to_send) { gas = to_signed(gas, 16); bool violation = false; - violation |= longitudinal_accel_checks(accel, HONDA_BOSCH_LONG_LIMITS); - violation |= longitudinal_gas_checks(gas, HONDA_BOSCH_LONG_LIMITS); + if (sport_mode) { + violation |= longitudinal_accel_checks(accel, HONDA_BOSCH_LONG_LIMITS_SPORT); + violation |= longitudinal_gas_checks(gas, HONDA_BOSCH_LONG_LIMITS_SPORT); + } else { + violation |= longitudinal_accel_checks(accel, HONDA_BOSCH_LONG_LIMITS); + violation |= longitudinal_gas_checks(gas, HONDA_BOSCH_LONG_LIMITS); + } if (violation) { tx = false; } @@ -323,7 +338,11 @@ static bool honda_tx_hook(const CANPacket_t *to_send) { accel = to_signed(accel, 12); bool violation = false; - violation |= longitudinal_accel_checks(accel, HONDA_BOSCH_LONG_LIMITS); + if (sport_mode) { + violation |= longitudinal_accel_checks(accel, HONDA_BOSCH_LONG_LIMITS_SPORT); + } else { + violation |= longitudinal_accel_checks(accel, HONDA_BOSCH_LONG_LIMITS); + } if (violation) { tx = false; } diff --git a/panda/board/safety/safety_hyundai.h b/panda/board/safety/safety_hyundai.h index 5a324bf..a849a87 100644 --- a/panda/board/safety/safety_hyundai.h +++ b/panda/board/safety/safety_hyundai.h @@ -25,6 +25,11 @@ const LongitudinalLimits HYUNDAI_LONG_LIMITS = { .min_accel = -350, // 1/100 m/s2 }; +const LongitudinalLimits HYUNDAI_LONG_LIMITS_SPORT = { + .max_accel = 400, // 1/100 m/s2 + .min_accel = -350, // 1/100 m/s2 +}; + const CanMsg HYUNDAI_TX_MSGS[] = { {0x340, 0, 8}, // LKAS11 Bus 0 {0x4F1, 0, 4}, // CLU11 Bus 0 @@ -215,6 +220,8 @@ static void hyundai_rx_hook(const CANPacket_t *to_push) { } static bool hyundai_tx_hook(const CANPacket_t *to_send) { + sport_mode = alternative_experience & ALT_EXP_RAISE_LONGITUDINAL_LIMITS_TO_ISO_MAX; + bool tx = true; int addr = GET_ADDR(to_send); @@ -239,8 +246,13 @@ static bool hyundai_tx_hook(const CANPacket_t *to_send) { bool violation = false; - violation |= longitudinal_accel_checks(desired_accel_raw, HYUNDAI_LONG_LIMITS); - violation |= longitudinal_accel_checks(desired_accel_val, HYUNDAI_LONG_LIMITS); + if (sport_mode) { + violation |= longitudinal_accel_checks(desired_accel_raw, HYUNDAI_LONG_LIMITS_SPORT); + violation |= longitudinal_accel_checks(desired_accel_val, HYUNDAI_LONG_LIMITS_SPORT); + } else { + violation |= longitudinal_accel_checks(desired_accel_raw, HYUNDAI_LONG_LIMITS); + violation |= longitudinal_accel_checks(desired_accel_val, HYUNDAI_LONG_LIMITS); + } violation |= (aeb_decel_cmd != 0); violation |= aeb_req; diff --git a/panda/board/safety/safety_toyota.h b/panda/board/safety/safety_toyota.h index 50c00b3..f65e869 100644 --- a/panda/board/safety/safety_toyota.h +++ b/panda/board/safety/safety_toyota.h @@ -37,6 +37,11 @@ const LongitudinalLimits TOYOTA_LONG_LIMITS = { .min_accel = -3500, // -3.5 m/s2 }; +const LongitudinalLimits TOYOTA_LONG_LIMITS_SPORT = { + .max_accel = 4000, // 4.0 m/s2 + .min_accel = -3500, // -3.5 m/s2 +}; + // panda interceptor threshold needs to be equivalent to openpilot threshold to avoid controls mismatches // If thresholds are mismatched then it is possible for panda to see the gas fall and rise while openpilot is in the pre-enabled state // Threshold calculated from DBC gains: round((((15 + 75.555) / 0.159375) + ((15 + 151.111) / 0.159375)) / 2) = 805 @@ -228,6 +233,8 @@ static void toyota_rx_hook(const CANPacket_t *to_push) { } static bool toyota_tx_hook(const CANPacket_t *to_send) { + sport_mode = alternative_experience & ALT_EXP_RAISE_LONGITUDINAL_LIMITS_TO_ISO_MAX; + bool tx = true; int addr = GET_ADDR(to_send); int bus = GET_BUS(to_send); @@ -248,7 +255,11 @@ static bool toyota_tx_hook(const CANPacket_t *to_send) { desired_accel = to_signed(desired_accel, 16); bool violation = false; - violation |= longitudinal_accel_checks(desired_accel, TOYOTA_LONG_LIMITS); + if (sport_mode) { + violation |= longitudinal_accel_checks(desired_accel, TOYOTA_LONG_LIMITS_SPORT); + } else { + violation |= longitudinal_accel_checks(desired_accel, TOYOTA_LONG_LIMITS); + } // only ACC messages that cancel are allowed when openpilot is not controlling longitudinal if (toyota_stock_longitudinal) { diff --git a/panda/board/safety/safety_volkswagen_mqb.h b/panda/board/safety/safety_volkswagen_mqb.h index d880a69..ce152f4 100644 --- a/panda/board/safety/safety_volkswagen_mqb.h +++ b/panda/board/safety/safety_volkswagen_mqb.h @@ -20,6 +20,12 @@ const LongitudinalLimits VOLKSWAGEN_MQB_LONG_LIMITS = { .inactive_accel = 3010, // VW sends one increment above the max range when inactive }; +const LongitudinalLimits VOLKSWAGEN_MQB_LONG_LIMITS_SPORT = { + .max_accel = 4000, + .min_accel = -3500, + .inactive_accel = 3010, // VW sends one increment above the max range when inactive +}; + #define MSG_ESP_19 0x0B2 // RX from ABS, for wheel speeds #define MSG_LH_EPS_03 0x09F // RX from EPS, for driver steering torque #define MSG_ESP_05 0x106 // RX from ABS, for brake switch state @@ -197,6 +203,8 @@ static void volkswagen_mqb_rx_hook(const CANPacket_t *to_push) { } static bool volkswagen_mqb_tx_hook(const CANPacket_t *to_send) { + sport_mode = alternative_experience & ALT_EXP_RAISE_LONGITUDINAL_LIMITS_TO_ISO_MAX; + int addr = GET_ADDR(to_send); bool tx = true; @@ -234,7 +242,15 @@ static bool volkswagen_mqb_tx_hook(const 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 (sport_mode) { + if (desired_accel != 0) { + violation |= longitudinal_accel_checks(desired_accel, VOLKSWAGEN_MQB_LONG_LIMITS_SPORT); + } + } else { + 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 1922cf4..671f006 100644 --- a/panda/board/safety/safety_volkswagen_pq.h +++ b/panda/board/safety/safety_volkswagen_pq.h @@ -20,6 +20,12 @@ const LongitudinalLimits VOLKSWAGEN_PQ_LONG_LIMITS = { .inactive_accel = 3010, // VW sends one increment above the max range when inactive }; +const LongitudinalLimits VOLKSWAGEN_PQ_LONG_LIMITS_SPORT = { + .max_accel = 4000, + .min_accel = -3500, + .inactive_accel = 3010, // VW sends one increment above the max range when inactive +}; + #define MSG_LENKHILFE_3 0x0D0 // RX from EPS, for steering angle and driver steering torque #define MSG_HCA_1 0x0D2 // TX by OP, Heading Control Assist steering torque #define MSG_BREMSE_1 0x1A0 // RX from ABS, for ego speed @@ -170,6 +176,8 @@ static void volkswagen_pq_rx_hook(const CANPacket_t *to_push) { } static bool volkswagen_pq_tx_hook(const CANPacket_t *to_send) { + sport_mode = alternative_experience & ALT_EXP_RAISE_LONGITUDINAL_LIMITS_TO_ISO_MAX; + int addr = GET_ADDR(to_send); bool tx = true; @@ -198,8 +206,14 @@ static bool volkswagen_pq_tx_hook(const CANPacket_t *to_send) { // Signal: ACC_System.ACS_Sollbeschl (acceleration in m/s2, scale 0.005, offset -7.22) int desired_accel = ((((GET_BYTE(to_send, 4) & 0x7U) << 8) | GET_BYTE(to_send, 3)) * 5U) - 7220U; - if (longitudinal_accel_checks(desired_accel, VOLKSWAGEN_PQ_LONG_LIMITS)) { - tx = false; + if (sport_mode) { + if (longitudinal_accel_checks(desired_accel, VOLKSWAGEN_PQ_LONG_LIMITS_SPORT)) { + tx = false; + } + } else { + if (longitudinal_accel_checks(desired_accel, VOLKSWAGEN_PQ_LONG_LIMITS)) { + tx = false; + } } } diff --git a/panda/board/safety_declarations.h b/panda/board/safety_declarations.h index 64b55f2..4c62596 100644 --- a/panda/board/safety_declarations.h +++ b/panda/board/safety_declarations.h @@ -220,6 +220,7 @@ bool brake_pressed_prev = false; bool regen_braking = false; bool regen_braking_prev = false; bool cruise_engaged_prev = false; +bool sport_mode = false; struct sample_t vehicle_speed; bool vehicle_moving = false; bool acc_main_on = false; // referred to as "ACC off" in ISO 15622:2018 diff --git a/selfdrive/car/ford/carcontroller.py b/selfdrive/car/ford/carcontroller.py index d8b2a70..4b7a992 100644 --- a/selfdrive/car/ford/carcontroller.py +++ b/selfdrive/car/ford/carcontroller.py @@ -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 frogpilot_variables.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/values.py b/selfdrive/car/ford/values.py index c080e02..b3ba607 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/carcontroller.py b/selfdrive/car/gm/carcontroller.py index ec5077b..13dfd6f 100644 --- a/selfdrive/car/gm/carcontroller.py +++ b/selfdrive/car/gm/carcontroller.py @@ -93,7 +93,10 @@ class CarController: self.apply_gas = self.params.INACTIVE_REGEN self.apply_brake = 0 else: - self.apply_gas = int(round(interp(actuators.accel, self.params.GAS_LOOKUP_BP, self.params.GAS_LOOKUP_V))) + if frogpilot_variables.sport_plus: + self.apply_gas = int(round(interp(actuators.accel, self.params.GAS_LOOKUP_BP_PLUS, self.params.GAS_LOOKUP_V_PLUS))) + else: + self.apply_gas = int(round(interp(actuators.accel, self.params.GAS_LOOKUP_BP, self.params.GAS_LOOKUP_V))) self.apply_brake = int(round(interp(actuators.accel, self.params.BRAKE_LOOKUP_BP, self.params.BRAKE_LOOKUP_V))) # Don't allow any gas above inactive regen while stopping # FIXME: brakes aren't applied immediately when enabling at a stop diff --git a/selfdrive/car/gm/interface.py b/selfdrive/car/gm/interface.py index 47682c1..5b48b84 100755 --- a/selfdrive/car/gm/interface.py +++ b/selfdrive/car/gm/interface.py @@ -32,8 +32,11 @@ NEURAL_PARAMS_PATH = os.path.join(BASEDIR, 'selfdrive/car/torque_data/neural_ff_ 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, frogpilot_variables): + if frogpilot_variables.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 diff --git a/selfdrive/car/gm/values.py b/selfdrive/car/gm/values.py index 745ac43..0419ea7 100644 --- a/selfdrive/car/gm/values.py +++ b/selfdrive/car/gm/values.py @@ -33,6 +33,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): @@ -42,6 +43,7 @@ class CarControllerParams: if CP.carFingerprint in CAMERA_ACC_CAR: self.MAX_GAS = 3400 + self.MAX_GAS_PLUS = 8848 self.MAX_ACC_REGEN = 1514 self.INACTIVE_REGEN = 1554 # Camera ACC vehicles have no regen while enabled. @@ -50,6 +52,7 @@ class CarControllerParams: else: self.MAX_GAS = 3072 # Safety limit, not ACC max. Stock ACC >4096 from standstill. + self.MAX_GAS_PLUS = 8191 # 8292 uses new bit, possible but not tested. Matches Twilsonco tw-main max self.MAX_ACC_REGEN = 1404 # Max ACC regen is slightly less than max paddle regen self.INACTIVE_REGEN = 1404 # ICE has much less engine braking force compared to regen in EVs, @@ -57,7 +60,9 @@ class CarControllerParams: max_regen_acceleration = -1. if CP.carFingerprint in EV_CAR else -0.1 self.GAS_LOOKUP_BP = [max_regen_acceleration, 0., self.ACCEL_MAX] + self.GAS_LOOKUP_BP_PLUS = [max_regen_acceleration, 0., self.ACCEL_MAX_PLUS] self.GAS_LOOKUP_V = [self.MAX_ACC_REGEN, self.ZERO_GAS, self.MAX_GAS] + self.GAS_LOOKUP_V_PLUS = [self.MAX_ACC_REGEN, self.ZERO_GAS, self.MAX_GAS_PLUS] self.BRAKE_LOOKUP_BP = [self.ACCEL_MIN, max_regen_acceleration] self.BRAKE_LOOKUP_V = [self.MAX_BRAKE, 0.] diff --git a/selfdrive/car/honda/carcontroller.py b/selfdrive/car/honda/carcontroller.py index b34b8b5..eccf1b7 100644 --- a/selfdrive/car/honda/carcontroller.py +++ b/selfdrive/car/honda/carcontroller.py @@ -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 frogpilot_variables.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 5577f9f..873c874 100755 --- a/selfdrive/car/honda/interface.py +++ b/selfdrive/car/honda/interface.py @@ -20,11 +20,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, frogpilot_variables): if CP.carFingerprint in HONDA_BOSCH: - return CarControllerParams.BOSCH_ACCEL_MIN, CarControllerParams.BOSCH_ACCEL_MAX + if frogpilot_variables.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 frogpilot_variables.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 diff --git a/selfdrive/car/honda/values.py b/selfdrive/car/honda/values.py index 517d255..2656cf6 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 4bb0c46..f4a4594 100644 --- a/selfdrive/car/hyundai/carcontroller.py +++ b/selfdrive/car/hyundai/carcontroller.py @@ -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 frogpilot_variables.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/values.py b/selfdrive/car/hyundai/values.py index 20fc298..1eefc30 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 2e0b3a1..ea59e3b 100644 --- a/selfdrive/car/interfaces.py +++ b/selfdrive/car/interfaces.py @@ -26,6 +26,7 @@ EventName = car.CarEvent.EventName 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 @@ -102,8 +103,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, frogpilot_variables): + if frogpilot_variables.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/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py index af80d4e..33bd6e7 100644 --- a/selfdrive/car/toyota/carcontroller.py +++ b/selfdrive/car/toyota/carcontroller.py @@ -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 frogpilot_variables.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 3f0b2f4..6d3a614 100644 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -14,8 +14,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, frogpilot_variables): + if frogpilot_variables.sport_plus: + return CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX_PLUS + else: + return CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX @staticmethod def _get_params(ret, params, candidate, fingerprint, car_fw, experimental_long, docs): diff --git a/selfdrive/car/toyota/values.py b/selfdrive/car/toyota/values.py index ed2b022..f2bec63 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 # m/s2 ACCEL_MIN = -3.5 # m/s2 STEER_STEP = 1 diff --git a/selfdrive/car/volkswagen/carcontroller.py b/selfdrive/car/volkswagen/carcontroller.py index 48f6d42..7dd0c38 100644 --- a/selfdrive/car/volkswagen/carcontroller.py +++ b/selfdrive/car/volkswagen/carcontroller.py @@ -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 frogpilot_variables.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/values.py b/selfdrive/car/volkswagen/values.py index 9271baf..e44abbf 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 a1fc02b..f27bcb6 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -35,6 +35,8 @@ from openpilot.selfdrive.controls.lib.vehicle_model import VehicleModel from openpilot.system.hardware import HARDWARE from openpilot.system.version import get_short_branch +from openpilot.selfdrive.frogpilot.functions.frogpilot_functions import CRUISING_SPEED + SOFT_DISABLE_TIME = 3 # seconds LDW_MIN_SPEED = 31 * CV.MPH_TO_MS LANE_DEPARTURE_THRESHOLD = 0.1 @@ -195,6 +197,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") @@ -706,7 +710,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.frogpilot_variables) t_since_plan = (self.sm.frame - self.sm.recv_frame['longitudinalPlan']) * DT_CTRL actuators.accel = self.LoC.update(CC.longActive, CS, long_plan, pid_accel_limits, t_since_plan) @@ -988,6 +992,7 @@ class Controls: lateral_tune = self.params.get_bool("LateralTune") longitudinal_tune = self.params.get_bool("LongitudinalTune") + self.frogpilot_variables.sport_plus = longitudinal_tune and self.params.get_int("AccelerationProfile") == 3 quality_of_life = self.params.get_bool("QOLControls") diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index d740b22..7132081 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -17,11 +17,16 @@ from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import T_IDX from openpilot.selfdrive.controls.lib.drive_helpers import V_CRUISE_MAX, CONTROL_N, get_speed_error from openpilot.common.swaglog import cloudlog +from openpilot.selfdrive.frogpilot.functions.frogpilot_functions import CRUISING_SPEED + LON_MPC_STEP = 0.2 # first step is 0.2s A_CRUISE_MIN = -1.2 A_CRUISE_MAX_VALS = [1.6, 1.2, 0.8, 0.6] A_CRUISE_MAX_BP = [0., 10.0, 25., 40.] +ACCEL_MAX_PLUS = [ACCEL_MAX, 4.0] +ACCEL_MAX_PLUS_BP = [0., CRUISING_SPEED] + # Lookup table for turns _A_TOTAL_MAX_V = [1.7, 3.2] _A_TOTAL_MAX_BP = [20., 40.] @@ -107,17 +112,18 @@ class LongitudinalPlanner: # No change cost when user is controlling the speed, or when standstill prev_accel_constraint = not (reset_state or sm['carState'].standstill) + accel_limits = frogpilot_planner.accel_limits if 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: - accel_limits = [ACCEL_MIN, ACCEL_MAX] accel_limits_turns = [ACCEL_MIN, ACCEL_MAX] if reset_state: self.v_desired_filter.x = v_ego + # Slowly ramp up the acceleration to prevent jerky behaviors from a standstill + max_acceleration = min(interp(v_ego, ACCEL_MAX_PLUS_BP, ACCEL_MAX_PLUS), accel_limits[1]) # Clip aEgo to cruise limits to prevent large accelerations when becoming active - self.a_desired = clip(sm['carState'].aEgo, accel_limits[0], accel_limits[1]) + self.a_desired = clip(sm['carState'].aEgo, accel_limits[0], max_acceleration) # Prevent divergence, smooth in current v_ego self.v_desired_filter.x = max(0.0, self.v_desired_filter.update(v_ego)) diff --git a/selfdrive/frogpilot/functions/frogpilot_functions.py b/selfdrive/frogpilot/functions/frogpilot_functions.py index 5bbd4aa..e8df939 100644 --- a/selfdrive/frogpilot/functions/frogpilot_functions.py +++ b/selfdrive/frogpilot/functions/frogpilot_functions.py @@ -12,6 +12,34 @@ CRUISING_SPEED = 5 # Roughly the speed cars go when not touching the gas while PROBABILITY = 0.6 # 60% chance of condition being true THRESHOLD = 5 # Time threshold (0.25s) +# Acceleration profiles - Credit goes to the DragonPilot team! + # MPH = [0., 18, 36, 63, 94] +A_CRUISE_MIN_BP_CUSTOM = [0., 8., 16., 28., 42.] + # 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 = [-0.001, -0.010, -0.28, -0.56, -0.56] +A_CRUISE_MAX_VALS_ECO = [3.5, 3.2, 2.3, 2.0, 1.15, .80, .58, .36, .30, .091] + +A_CRUISE_MIN_VALS_SPORT = [-0.50, -0.52, -0.55, -0.57, -0.60] +A_CRUISE_MAX_VALS_SPORT = [3.5, 3.5, 3.3, 2.8, 1.5, 1.0, .75, .6, .38, .2] + class FrogPilotFunctions: def __init__(self) -> None: self.params = Params() + + @staticmethod + def get_min_accel_eco(v_ego): + return interp(v_ego, A_CRUISE_MIN_BP_CUSTOM, A_CRUISE_MIN_VALS_ECO) + + @staticmethod + def get_max_accel_eco(v_ego): + return interp(v_ego, A_CRUISE_MAX_BP_CUSTOM, A_CRUISE_MAX_VALS_ECO) + + @staticmethod + def get_min_accel_sport(v_ego): + return interp(v_ego, A_CRUISE_MIN_BP_CUSTOM, A_CRUISE_MIN_VALS_SPORT) + + @staticmethod + def get_max_accel_sport(v_ego): + return interp(v_ego, A_CRUISE_MAX_BP_CUSTOM, A_CRUISE_MAX_VALS_SPORT) diff --git a/selfdrive/frogpilot/functions/frogpilot_planner.py b/selfdrive/frogpilot/functions/frogpilot_planner.py index e894c89..bf8e1ab 100644 --- a/selfdrive/frogpilot/functions/frogpilot_planner.py +++ b/selfdrive/frogpilot/functions/frogpilot_planner.py @@ -1,6 +1,8 @@ import cereal.messaging as messaging from openpilot.common.conversions import Conversions as CV +from openpilot.selfdrive.car.interfaces import ACCEL_MIN, ACCEL_MAX +from openpilot.selfdrive.controls.lib.longitudinal_planner import A_CRUISE_MIN, get_max_accel from openpilot.selfdrive.frogpilot.functions.frogpilot_functions import CRUISING_SPEED, FrogPilotFunctions @@ -13,11 +15,35 @@ class FrogPilotPlanner: self.v_cruise = 0 + self.accel_limits = [A_CRUISE_MIN, get_max_accel(0)] + self.update_frogpilot_params(params) def update(self, carState, controlsState, modelData, mpc, sm, v_cruise, v_ego): enabled = controlsState.enabled + # Configure the deceleration profile + if self.deceleration_profile == 1: + min_accel = self.fpf.get_min_accel_eco(v_ego) + elif self.deceleration_profile == 2: + min_accel = self.fpf.get_min_accel_sport(v_ego) + elif mpc.mode == 'acc': + min_accel = A_CRUISE_MIN + else: + min_accel = ACCEL_MIN + + # Configure the acceleration profile + if self.acceleration_profile == 1: + max_accel = self.fpf.get_max_accel_eco(v_ego) + elif self.acceleration_profile in (2, 3): + max_accel = self.fpf.get_max_accel_sport(v_ego) + elif mpc.mode == 'acc': + max_accel = get_max_accel(v_ego) + else: + max_accel = ACCEL_MAX + + self.accel_limits = [min_accel, max_accel] + # Update the max allowed speed self.v_cruise = self.update_v_cruise(carState, controlsState, enabled, modelData, v_cruise, v_ego) @@ -47,3 +73,5 @@ class FrogPilotPlanner: custom_ui = params.get_bool("CustomUI") longitudinal_tune = params.get_bool("LongitudinalTune") + self.acceleration_profile = params.get_int("AccelerationProfile") if longitudinal_tune else 0 + self.deceleration_profile = params.get_int("DecelerationProfile") if longitudinal_tune else 0 diff --git a/selfdrive/frogpilot/ui/control_settings.cc b/selfdrive/frogpilot/ui/control_settings.cc index 1f40f49..7f11c13 100644 --- a/selfdrive/frogpilot/ui/control_settings.cc +++ b/selfdrive/frogpilot/ui/control_settings.cc @@ -5,6 +5,8 @@ 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.", ""}, + {"DecelerationProfile", "Deceleration Profile", "Change the deceleration rate to be either sporty or eco-friendly.", ""}, {"QOLControls", "Quality of Life", "Miscellaneous quality of life changes to improve your overall openpilot experience.", "../frogpilot/assets/toggle_icons/quality_of_life.png"}, }; @@ -31,6 +33,21 @@ 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 if (param == "DecelerationProfile") { + std::vector profileOptions{tr("Standard"), tr("Eco"), tr("Sport")}; + FrogPilotButtonParamControl *profileSelection = new FrogPilotButtonParamControl(param, title, desc, icon, profileOptions); + toggle = profileSelection; } else if (param == "QOLControls") { FrogPilotParamManageControl *qolToggle = new FrogPilotParamManageControl(param, title, desc, icon, this); diff --git a/selfdrive/frogpilot/ui/control_settings.h b/selfdrive/frogpilot/ui/control_settings.h index addea26..359ab97 100644 --- a/selfdrive/frogpilot/ui/control_settings.h +++ b/selfdrive/frogpilot/ui/control_settings.h @@ -33,7 +33,7 @@ private: std::set fireTheBabysitterKeys = {}; std::set laneChangeKeys = {}; std::set lateralTuneKeys = {}; - std::set longitudinalTuneKeys = {}; + std::set longitudinalTuneKeys = {"AccelerationProfile", "DecelerationProfile"}; std::set mtscKeys = {}; std::set qolKeys = {}; std::set speedLimitControllerKeys = {};