From 856c3c4214acbb8c9d103a05962aedc35dc64506 Mon Sep 17 00:00:00 2001 From: FrogAi <91348155+FrogAi@users.noreply.github.com> Date: Sat, 11 May 2024 20:43:31 -0700 Subject: [PATCH] Controls - Longitudinal Tuning - Acceleration Profile Change the acceleration rate to be either sporty or eco-friendly. --- panda/board/safety/safety_gm.h | 28 +++++++++++++++++-- panda/board/safety/safety_honda.h | 25 +++++++++++++++-- panda/board/safety/safety_hyundai.h | 16 +++++++++-- panda/board/safety/safety_toyota.h | 13 ++++++++- panda/board/safety/safety_volkswagen_mqb.h | 16 ++++++++++- panda/board/safety/safety_volkswagen_pq.h | 18 ++++++++++-- panda/board/safety_declarations.h | 1 + selfdrive/car/card.py | 2 ++ selfdrive/car/ford/carcontroller.py | 5 +++- selfdrive/car/ford/values.py | 1 + selfdrive/car/gm/carcontroller.py | 10 +++++-- selfdrive/car/gm/interface.py | 7 +++-- selfdrive/car/gm/values.py | 7 +++++ selfdrive/car/honda/carcontroller.py | 5 +++- selfdrive/car/honda/interface.py | 12 ++++++-- selfdrive/car/honda/values.py | 2 ++ selfdrive/car/hyundai/carcontroller.py | 5 +++- selfdrive/car/hyundai/values.py | 1 + selfdrive/car/interfaces.py | 8 ++++-- selfdrive/car/toyota/carcontroller.py | 5 +++- selfdrive/car/toyota/interface.py | 7 +++-- selfdrive/car/toyota/values.py | 1 + selfdrive/car/volkswagen/carcontroller.py | 5 +++- selfdrive/car/volkswagen/values.py | 1 + selfdrive/controls/controlsd.py | 2 +- .../controls/lib/longitudinal_planner.py | 3 +- .../frogpilot/controls/frogpilot_planner.py | 25 +++++++++++++++++ 27 files changed, 201 insertions(+), 30 deletions(-) diff --git a/panda/board/safety/safety_gm.h b/panda/board/safety/safety_gm.h index d722e36..fef5dda 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 @@ -287,6 +301,8 @@ 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; + if GET_FLAG(param, GM_PARAM_HW_CAM) { gm_hw = GM_CAM; } else if GET_FLAG(param, GM_PARAM_HW_SDGM) { @@ -298,9 +314,17 @@ static safety_config gm_init(uint16_t param) { gm_force_ascm = GET_FLAG(param, GM_PARAM_HW_ASCM_LONG); if (gm_hw == GM_ASCM || gm_force_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_hw == GM_SDGM)) { - 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 ca4527b..9af384b 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, @@ -275,6 +283,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); @@ -318,8 +328,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; } @@ -331,7 +346,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 0e9742c..538fc79 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 @@ -233,6 +238,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); @@ -253,7 +260,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..0a84342 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,13 @@ 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 { + 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 de147cb..c8838cf 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 686e4e1..7c5ddb9 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/card.py b/selfdrive/car/card.py index 6ba7a7b..2e92e5f 100755 --- a/selfdrive/car/card.py +++ b/selfdrive/car/card.py @@ -58,6 +58,8 @@ class CarD: if self.always_on_lateral: self.CP.alternativeExperience |= ALTERNATIVE_EXPERIENCE.ALWAYS_ON_LATERAL + self.CP.alternativeExperience |= ALTERNATIVE_EXPERIENCE.RAISE_LONGITUDINAL_LIMITS_TO_ISO_MAX + car_recognized = self.CP.carName != 'mock' openpilot_enabled_toggle = self.params.get_bool("OpenpilotEnabledToggle") diff --git a/selfdrive/car/ford/carcontroller.py b/selfdrive/car/ford/carcontroller.py index ed99c65..28e7b9b 100644 --- a/selfdrive/car/ford/carcontroller.py +++ b/selfdrive/car/ford/carcontroller.py @@ -94,7 +94,10 @@ class CarController(CarControllerBase): # 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 eeafc47..cdf9b21 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 53bf754..742730c 100644 --- a/selfdrive/car/gm/carcontroller.py +++ b/selfdrive/car/gm/carcontroller.py @@ -124,10 +124,16 @@ class CarController(CarControllerBase): # Normal operation if self.CP.carFingerprint in EV_CAR: self.params.update_ev_gas_brake_threshold(CS.out.vEgo) - self.apply_gas = int(round(interp(actuators.accel, self.params.EV_GAS_LOOKUP_BP, self.params.GAS_LOOKUP_V))) + if frogpilot_variables.sport_plus: + self.apply_gas = int(round(interp(actuators.accel, self.params.EV_GAS_LOOKUP_BP_PLUS, self.params.GAS_LOOKUP_V_PLUS))) + else: + self.apply_gas = int(round(interp(actuators.accel, self.params.EV_GAS_LOOKUP_BP, self.params.GAS_LOOKUP_V))) self.apply_brake = int(round(interp(actuators.accel, self.params.EV_BRAKE_LOOKUP_BP, self.params.BRAKE_LOOKUP_V))) 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 6b25cef..789c26c 100644 --- a/selfdrive/car/gm/interface.py +++ b/selfdrive/car/gm/interface.py @@ -38,8 +38,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 6e9c456..0d5b6bf 100644 --- a/selfdrive/car/gm/values.py +++ b/selfdrive/car/gm/values.py @@ -34,6 +34,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): @@ -43,6 +44,7 @@ class CarControllerParams: if CP.carFingerprint in CAMERA_ACC_CAR and CP.carFingerprint not in CC_ONLY_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. @@ -51,12 +53,14 @@ class CarControllerParams: elif CP.carFingerprint in SDGM_CAR: self.MAX_GAS = 3400 + self.MAX_GAS_PLUS = 8848 self.MAX_ACC_REGEN = 1514 self.INACTIVE_REGEN = 1554 max_regen_acceleration = 0. 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, @@ -64,7 +68,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.] @@ -78,6 +84,7 @@ class CarControllerParams: def update_ev_gas_brake_threshold(self, v_ego): gas_brake_threshold = interp(v_ego, self.EV_GAS_BRAKE_THRESHOLD_BP, self.EV_GAS_BRAKE_THRESHOLD_V) self.EV_GAS_LOOKUP_BP = [gas_brake_threshold, max(0., gas_brake_threshold), self.ACCEL_MAX] + self.EV_GAS_LOOKUP_BP_PLUS = [gas_brake_threshold, max(0., gas_brake_threshold), self.ACCEL_MAX_PLUS] self.EV_BRAKE_LOOKUP_BP = [self.ACCEL_MIN, gas_brake_threshold] diff --git a/selfdrive/car/honda/carcontroller.py b/selfdrive/car/honda/carcontroller.py index 36c4280..d1ad8ce 100644 --- a/selfdrive/car/honda/carcontroller.py +++ b/selfdrive/car/honda/carcontroller.py @@ -216,7 +216,10 @@ class CarController(CarControllerBase): 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 56b67f7..4a48e9e 100755 --- a/selfdrive/car/honda/interface.py +++ b/selfdrive/car/honda/interface.py @@ -22,11 +22,17 @@ FrogPilotButtonType = custom.FrogPilotCarState.ButtonEvent.Type 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 d649af9..3cfbcfc 100644 --- a/selfdrive/car/honda/values.py +++ b/selfdrive/car/honda/values.py @@ -20,6 +20,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] @@ -32,6 +33,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 479cf2c..a61da9f 100644 --- a/selfdrive/car/hyundai/carcontroller.py +++ b/selfdrive/car/hyundai/carcontroller.py @@ -79,7 +79,10 @@ class CarController(CarControllerBase): 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 a4e89c2..55957ad 100644 --- a/selfdrive/car/hyundai/values.py +++ b/selfdrive/car/hyundai/values.py @@ -15,6 +15,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 a4b9445..4825500 100644 --- a/selfdrive/car/interfaces.py +++ b/selfdrive/car/interfaces.py @@ -28,6 +28,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 @@ -238,8 +239,11 @@ class CarInterfaceBase(ABC): return (self.lat_torque_nn_model is not None) @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 a55afb3..92cfa58 100644 --- a/selfdrive/car/toyota/carcontroller.py +++ b/selfdrive/car/toyota/carcontroller.py @@ -130,7 +130,10 @@ class CarController(CarControllerBase): interceptor_gas_cmd = 0.12 if CS.out.standstill else 0. 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 8b83f59..600ea10 100644 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -15,8 +15,11 @@ FrogPilotButtonType = custom.FrogPilotCarState.ButtonEvent.Type 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 9ad1896..fa61d29 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 fc718b3..ba1ee43 100644 --- a/selfdrive/car/volkswagen/carcontroller.py +++ b/selfdrive/car/volkswagen/carcontroller.py @@ -79,7 +79,10 @@ class CarController(CarControllerBase): 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 9019d85..57c7c0a 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 af748cb..4b5b0ec 100644 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -610,7 +610,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_toggles) 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) diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 6dd102a..5bc9ff8 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -99,11 +99,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) + accel_limits = [A_CRUISE_MIN, sm['frogpilotPlan'].maxAcceleration] 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: diff --git a/selfdrive/frogpilot/controls/frogpilot_planner.py b/selfdrive/frogpilot/controls/frogpilot_planner.py index c3c5373..1712621 100644 --- a/selfdrive/frogpilot/controls/frogpilot_planner.py +++ b/selfdrive/frogpilot/controls/frogpilot_planner.py @@ -20,6 +20,20 @@ from openpilot.selfdrive.frogpilot.controls.lib.frogpilot_variables import CITY_ GearShifter = car.CarState.GearShifter +# Acceleration profiles - Credit goes to the DragonPilot team! + # MPH = [0., 6.71, 13.4, 17.9, 24.6, 33.6, 44.7, 55.9, 89.5] +A_CRUISE_MAX_BP_CUSTOM = [0., 3, 6., 8., 11., 15., 20., 25., 40.] + +A_CRUISE_MAX_VALS_ECO = [3.5, 3.2, 2.3, 2.0, 1.15, .80, .58, .36, .30] + +A_CRUISE_MAX_VALS_SPORT = [3.5, 3.5, 3.3, 2.8, 1.5, 1.0, 0.75, 0.65, 0.6] + +def get_max_accel_eco(v_ego): + return interp(v_ego, A_CRUISE_MAX_BP_CUSTOM, A_CRUISE_MAX_VALS_ECO) + +def get_max_accel_sport(v_ego): + return interp(v_ego, A_CRUISE_MAX_BP_CUSTOM, A_CRUISE_MAX_VALS_SPORT) + class FrogPilotPlanner: def __init__(self): self.params_memory = Params("/dev/shm/params") @@ -44,6 +58,15 @@ class FrogPilotPlanner: lead_distance = self.lead_one.dRel + if frogpilot_toggles.acceleration_profile == 1: + self.max_accel = get_max_accel_eco(v_ego) + elif frogpilot_toggles.acceleration_profile in (2, 3): + self.max_accel = get_max_accel_sport(v_ego) + elif controlsState.experimentalMode: + self.max_accel = ACCEL_MAX + else: + self.max_accel = get_max_accel(v_ego) + check_lane_width = frogpilot_toggles.lane_detection if check_lane_width and v_ego >= frogpilot_toggles.minimum_lane_change_speed: self.lane_width_left = float(calculate_lane_width(modelData.laneLines[0], modelData.laneLines[1], modelData.roadEdges[0])) @@ -106,6 +129,8 @@ class FrogPilotPlanner: frogpilotPlan.conditionalExperimental = self.cem.experimental_mode + frogpilotPlan.maxAcceleration = self.max_accel + frogpilotPlan.vCruise = float(self.v_cruise) pm.send('frogpilotPlan', frogpilot_plan_send)