From 2f4243f66c28bd6592fca3dde34c5e1a5debcafe Mon Sep 17 00:00:00 2001 From: FrogAi <91348155+FrogAi@users.noreply.github.com> Date: Wed, 3 Apr 2024 11:23:11 -0700 Subject: [PATCH] Acceleration/deceleration profiles Added toggle to use custom acceleration/deceleration profiles. --- cereal/custom.capnp | 2 + common/params.cc | 2 + 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 | 5 +- selfdrive/car/gm/interface.py | 7 ++- selfdrive/car/gm/values.py | 5 ++ 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 | 3 +- .../controls/lib/longitudinal_planner.py | 3 +- .../frogpilot/controls/frogpilot_planner.py | 46 +++++++++++++++++++ .../ui/qt/offroad/control_settings.cc | 17 +++++++ .../ui/qt/offroad/control_settings.h | 2 +- 31 files changed, 239 insertions(+), 30 deletions(-) mode change 100755 => 100644 selfdrive/controls/controlsd.py diff --git a/cereal/custom.capnp b/cereal/custom.capnp index 5bc3212ee..2b6f6be6b 100644 --- a/cereal/custom.capnp +++ b/cereal/custom.capnp @@ -22,6 +22,8 @@ struct FrogPilotNavigation @0xda96579883444c35 { struct FrogPilotPlan @0x80ae746ee2596b11 { jerk @7 :Float32; + minAcceleration @10 :Float32; + maxAcceleration @11 :Float32; tFollow @20 :Float32; vCruise @22 :Float32; } diff --git a/common/params.cc b/common/params.cc index 80d534864..5024ce96e 100644 --- a/common/params.cc +++ b/common/params.cc @@ -210,9 +210,11 @@ std::unordered_map keys = { // FrogPilot parameters {"AccelerationPath", PERSISTENT}, + {"AccelerationProfile", PERSISTENT}, {"CustomAlerts", PERSISTENT}, {"CustomPaths", 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 82a5d9cd3..357c970a2 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 78bbb7f0b..b79b6c49d 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 5a324bff2..a849a87e1 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 50c00b38a..f65e86980 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 d880a69a6..0a84342f5 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 de147cb58..c8838cf48 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 64b55f203..4c625968b 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 b0df13f75..1e0298b8a 100755 --- a/selfdrive/car/card.py +++ b/selfdrive/car/card.py @@ -54,6 +54,8 @@ class CarD: if not disengage_on_accelerator: self.CP.alternativeExperience |= ALTERNATIVE_EXPERIENCE.DISABLE_DISENGAGE_ON_GAS + 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 5781e573b..dabc454b1 100644 --- a/selfdrive/car/ford/carcontroller.py +++ b/selfdrive/car/ford/carcontroller.py @@ -87,7 +87,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 eeafc47ae..cdf9b211d 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 d95941ad2..ee9acc1f3 100644 --- a/selfdrive/car/gm/carcontroller.py +++ b/selfdrive/car/gm/carcontroller.py @@ -94,7 +94,10 @@ class CarController(CarControllerBase): 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 0d78d16f3..a2cb7f5ec 100755 --- a/selfdrive/car/gm/interface.py +++ b/selfdrive/car/gm/interface.py @@ -33,8 +33,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 392ca85eb..a6e3dcb9b 100644 --- a/selfdrive/car/gm/values.py +++ b/selfdrive/car/gm/values.py @@ -31,6 +31,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): @@ -40,6 +41,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. @@ -48,6 +50,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, @@ -55,7 +58,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 5c7ece7a9..deb25e5d2 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 4f8109a38..e58201257 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 6789667fe..27f1fd938 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 015e5eaf4..6937f787a 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 d81d9ad51..6a00fe772 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 c3a645a4f..8c94d6b07 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 @@ -105,8 +106,11 @@ class CarInterfaceBase(ABC): lateral_tune = self.params.get_bool("LateralTune") @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 0d60708e1..08938dc63 100644 --- a/selfdrive/car/toyota/carcontroller.py +++ b/selfdrive/car/toyota/carcontroller.py @@ -120,7 +120,10 @@ class CarController(CarControllerBase): 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 249261e52..f08078ddc 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 9a3e73048..252d7ace6 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 f40135c95..e4cde9873 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 9019d857b..57c7c0a1b 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 old mode 100755 new mode 100644 index 260fec00b..39b6c5ba1 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -594,7 +594,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) @@ -897,6 +897,7 @@ class Controls: lateral_tune = self.params.get_bool("LateralTune") longitudinal_tune = self.CP.openpilotLongitudinalControl and 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 1bd950a1b..d144bba52 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -92,11 +92,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 = [sm['frogpilotPlan'].minAcceleration, 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 ba9ce23c9..c1df870bf 100644 --- a/selfdrive/frogpilot/controls/frogpilot_planner.py +++ b/selfdrive/frogpilot/controls/frogpilot_planner.py @@ -15,6 +15,30 @@ from openpilot.system.version import get_short_branch from openpilot.selfdrive.frogpilot.controls.lib.frogpilot_functions import CITY_SPEED_LIMIT, CRUISING_SPEED, calculate_lane_width, calculate_road_curvature +# 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] + +def get_min_accel_eco(v_ego): + return interp(v_ego, A_CRUISE_MIN_BP_CUSTOM, A_CRUISE_MIN_VALS_ECO) + +def get_max_accel_eco(v_ego): + return interp(v_ego, A_CRUISE_MAX_BP_CUSTOM, A_CRUISE_MAX_VALS_ECO) + +def get_min_accel_sport(v_ego): + return interp(v_ego, A_CRUISE_MIN_BP_CUSTOM, A_CRUISE_MIN_VALS_SPORT) + +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, CP): self.CP = CP @@ -33,6 +57,24 @@ class FrogPilotPlanner: v_ego = max(carState.vEgo, 0) v_lead = radarState.leadOne.vLead + if self.acceleration_profile == 1: + self.max_accel = get_max_accel_eco(v_ego) + elif self.acceleration_profile in (2, 3): + self.max_accel = get_max_accel_sport(v_ego) + elif not controlsState.experimentalMode: + self.max_accel = get_max_accel(v_ego) + else: + self.max_accel = ACCEL_MAX + + if self.deceleration_profile == 1: + self.min_accel = get_min_accel_eco(v_ego) + elif self.deceleration_profile == 2: + self.min_accel = get_min_accel_sport(v_ego) + elif not controlsState.experimentalMode: + self.min_accel = A_CRUISE_MIN + else: + self.min_accel = ACCEL_MIN + road_curvature = calculate_road_curvature(modelData, v_ego) if radarState.leadOne.status and self.CP.openpilotLongitudinalControl: @@ -69,6 +111,8 @@ class FrogPilotPlanner: frogpilotPlan = frogpilot_plan_send.frogpilotPlan frogpilotPlan.jerk = float(self.jerk) + frogpilotPlan.minAcceleration = self.min_accel + frogpilotPlan.maxAcceleration = self.max_accel frogpilotPlan.tFollow = float(self.t_follow) frogpilotPlan.vCruise = float(self.v_cruise) @@ -82,3 +126,5 @@ class FrogPilotPlanner: custom_ui = self.params.get_bool("CustomUI") longitudinal_tune = self.CP.openpilotLongitudinalControl and self.params.get_bool("LongitudinalTune") + self.acceleration_profile = self.params.get_int("AccelerationProfile") if longitudinal_tune else 0 + self.deceleration_profile = self.params.get_int("DecelerationProfile") if longitudinal_tune else 0 diff --git a/selfdrive/frogpilot/ui/qt/offroad/control_settings.cc b/selfdrive/frogpilot/ui/qt/offroad/control_settings.cc index c1cebe7f8..02ec158bb 100644 --- a/selfdrive/frogpilot/ui/qt/offroad/control_settings.cc +++ b/selfdrive/frogpilot/ui/qt/offroad/control_settings.cc @@ -8,6 +8,8 @@ FrogPilotControlsPanel::FrogPilotControlsPanel(SettingsWindow *parent) : FrogPil {"LateralTune", tr("Lateral Tuning"), tr("Modify openpilot's steering behavior."), "../frogpilot/assets/toggle_icons/icon_lateral_tune.png"}, {"LongitudinalTune", tr("Longitudinal Tuning"), tr("Modify openpilot's acceleration and braking behavior."), "../frogpilot/assets/toggle_icons/icon_longitudinal_tune.png"}, + {"AccelerationProfile", tr("Acceleration Profile"), tr("Change the acceleration rate to be either sporty or eco-friendly."), ""}, + {"DecelerationProfile", tr("Deceleration Profile"), tr("Change the deceleration rate to be either sporty or eco-friendly."), ""}, {"QOLControls", tr("Quality of Life"), tr("Miscellaneous quality of life changes to improve your overall openpilot experience."), "../frogpilot/assets/toggle_icons/quality_of_life.png"}, }; @@ -34,6 +36,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(tr("WARNING: This maxes out openpilot's acceleration from 2.0 m/s to 4.0 m/s and may cause oscillations when accelerating!"), + tr("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/qt/offroad/control_settings.h b/selfdrive/frogpilot/ui/qt/offroad/control_settings.h index 8f76170b3..8b7f1b9c8 100644 --- a/selfdrive/frogpilot/ui/qt/offroad/control_settings.h +++ b/selfdrive/frogpilot/ui/qt/offroad/control_settings.h @@ -31,7 +31,7 @@ private: std::set experimentalModeActivationKeys = {}; std::set laneChangeKeys = {}; std::set lateralTuneKeys = {}; - std::set longitudinalTuneKeys = {}; + std::set longitudinalTuneKeys = {"AccelerationProfile", "DecelerationProfile"}; std::set mtscKeys = {}; std::set qolKeys = {}; std::set speedLimitControllerKeys = {};