suburu sng

This commit is contained in:
firestar5683
2026-06-08 19:45:02 -05:00
parent cabacda0ad
commit bdb0da4fa0
19 changed files with 136 additions and 74 deletions
+1
View File
@@ -586,6 +586,7 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
{"SwitchbackModeCooldown", {PERSISTENT, INT, "5", "0", 2}},
{"SwitchbackModeEnabled", {CLEAR_ON_OFFROAD_TRANSITION, BOOL, "0", "0"}},
{"SubaruSNG", {PERSISTENT, BOOL, "1", "0", 2}},
{"SubaruSNGManualParkingBrake", {PERSISTENT, BOOL, "0", "0", 2}},
{"TacoTune", {PERSISTENT, BOOL, "0", "0", 2}},
{"TestAlert", {CLEAR_ON_MANAGER_START, STRING, "", ""}},
{"TetheringEnabled", {PERSISTENT, INT, "0", "0", 0}},
+5
View File
@@ -22,6 +22,7 @@ from opendbc.car.honda.values import CAR as HONDA, HONDA_BOSCH, HondaFlags, Hond
from opendbc.car.hyundai.hyundaicanfd import CanBus
from opendbc.car.hyundai.values import CAR as HYUNDAI, CANFD_CAR, HyundaiFlags, HyundaiStarPilotFlags, HyundaiStarPilotSafetyFlags, ALT_BUS_LDA_BUTTON_CARS
from opendbc.car.mock.values import CAR as MOCK
from opendbc.car.subaru.values import CAR as SUBARU, SubaruSafetyFlags
from opendbc.car.toyota.values import CAR as TOYOTA, NO_DSU_CAR, TSS2_CAR, UNSUPPORTED_DSU_CAR, ToyotaStarPilotFlags, ToyotaSafetyFlags
from opendbc.car.values import PLATFORMS
from opendbc.can import CANParser
@@ -266,6 +267,10 @@ class CarInterfaceBase(ABC):
elif platform.config.platform_str == "TESLA_MODEL_S_PREAP":
fp_ret.canUsePedal = True
elif platform in SUBARU:
if getattr(starpilot_toggles, "subaru_sng", False):
fp_ret.safetyConfigs[-1].safetyParam |= SubaruSafetyFlags.STOP_AND_GO.value
return fp_ret
@staticmethod
@@ -1,6 +1,6 @@
import numpy as np
from opendbc.can import CANPacker
from opendbc.car import Bus, make_tester_present_msg
from opendbc.car import Bus, DT_CTRL, make_tester_present_msg
from opendbc.car.lateral import apply_driver_steer_torque_limits, common_fault_avoidance
from opendbc.car.interfaces import CarControllerBase
from opendbc.car.subaru import subarucan
@@ -26,14 +26,9 @@ class CarController(CarControllerBase):
self.p = CarControllerParams(CP)
self.packer = CANPacker(DBC[CP.carFingerprint][Bus.pt])
self.manual_hold = False
self.prev_standstill = False
self.sng_acc_resume = False
self.prev_close_distance = 0
self.prev_cruise_state = 0
self.sng_acc_resume_cnt = 0
self.standstill_start = 0
self.epb_resume_frames_remaining = -1
self.last_standstill_frame = 0
def update(self, CC, CS, now_nanos, starpilot_toggles):
actuators = CC.actuators
@@ -70,8 +65,9 @@ class CarController(CarControllerBase):
self.apply_torque_last = apply_torque
# *** stop and go ***
subaru_sng_manual_parking_brake = getattr(starpilot_toggles, "subaru_sng_manual_parking_brake", False)
if starpilot_toggles.subaru_sng:
throttle_cmd, speed_cmd = self.stop_and_go(CC, CS)
throttle_cmd, speed_cmd = self.stop_and_go(CC, CS, subaru_sng_manual_parking_brake)
# *** longitudinal ***
@@ -110,7 +106,11 @@ class CarController(CarControllerBase):
can_sends.append(subarucan.create_preglobal_es_distance(self.packer, cruise_button, CS.es_distance_msg))
if starpilot_toggles.subaru_sng:
can_sends.append(subarucan.create_preglobal_throttle(self.packer, CS.throttle_msg["COUNTER"] + 1, CS.throttle_msg, throttle_cmd))
can_sends.append(subarucan.create_preglobal_throttle(self.packer, CS.throttle_msg["COUNTER"] + 1, CS.throttle_msg,
throttle_cmd and not subaru_sng_manual_parking_brake))
if self.frame % 2 == 0:
can_sends.append(subarucan.create_preglobal_brake_pedal(self.packer, CS.brake_pedal_msg,
speed_cmd and subaru_sng_manual_parking_brake))
else:
if self.frame % 10 == 0:
can_sends.append(subarucan.create_es_dashstatus(self.packer, self.frame // 10, CS.es_dashstatus_msg, CC.enabled,
@@ -124,9 +124,11 @@ class CarController(CarControllerBase):
can_sends.append(subarucan.create_es_infotainment(self.packer, self.frame // 10, CS.es_infotainment_msg, hud_control.visualAlert))
if starpilot_toggles.subaru_sng:
can_sends.append(subarucan.create_throttle(self.packer, CS.throttle_msg["COUNTER"] + 1, CS.throttle_msg, throttle_cmd))
can_sends.append(subarucan.create_throttle(self.packer, CS.throttle_msg["COUNTER"] + 1, CS.throttle_msg,
throttle_cmd and not subaru_sng_manual_parking_brake))
if self.frame % 2 == 0:
can_sends.append(subarucan.create_brake_pedal(self.packer, self.frame // 2, CS.brake_pedal_msg, speed_cmd, pcm_cancel_cmd))
can_sends.append(subarucan.create_brake_pedal(self.packer, self.frame // 2, CS.brake_pedal_msg,
speed_cmd and subaru_sng_manual_parking_brake, pcm_cancel_cmd))
if self.CP.openpilotLongitudinalControl:
if self.frame % 5 == 0:
@@ -166,49 +168,37 @@ class CarController(CarControllerBase):
self.frame += 1
return new_actuators, can_sends
def stop_and_go(self, CC, CS, speed_cmd=False, throttle_cmd=False):
if self.CP.flags & SubaruFlags.PREGLOBAL:
trigger_resume = CC.enabled
trigger_resume &= CS.car_follow == 1
trigger_resume &= CS.close_distance > self.prev_close_distance
trigger_resume &= CS.out.standstill
trigger_resume &= _SNG_ACC_MIN_DIST < CS.close_distance < _SNG_ACC_MAX_DIST
def stop_and_go(self, CC, CS, manual_parking_brake=False):
throttle_cmd = False
speed_cmd = False
if trigger_resume:
self.sng_acc_resume = True
if not CC.enabled or not CC.hudControl.leadVisible:
return throttle_cmd, speed_cmd
close_distance = CS.close_distance
if not CS.out.standstill:
self.last_standstill_frame = self.frame
standstill_timers = (0.75, 0.8) if self.CP.flags & SubaruFlags.PREGLOBAL else (0.5, 0.55)
standstill_duration = (self.frame - self.last_standstill_frame) * DT_CTRL
in_standstill_hold = standstill_duration > standstill_timers[0]
if standstill_duration >= standstill_timers[1]:
self.last_standstill_frame = self.frame
if manual_parking_brake:
speed_cmd = in_standstill_hold
else:
if CS.car_follow == 0 and CS.cruise_state == 3 and CS.out.standstill and self.prev_cruise_state == 1:
self.manual_hold = True
should_resume = (
CS.out.standstill and
_SNG_ACC_MIN_DIST < close_distance < _SNG_ACC_MAX_DIST and
close_distance > self.prev_close_distance
)
if should_resume:
self.epb_resume_frames_remaining = 15
if not CS.out.standstill:
self.manual_hold = False
throttle_cmd = self.epb_resume_frames_remaining > 0
if self.epb_resume_frames_remaining > 0:
self.epb_resume_frames_remaining -= 1
trigger_resume = CC.enabled
trigger_resume &= CS.car_follow == 1
trigger_resume &= CS.close_distance > self.prev_close_distance
trigger_resume &= CS.cruise_state == 3
trigger_resume &= not self.manual_hold
trigger_resume &= _SNG_ACC_MIN_DIST < CS.close_distance < _SNG_ACC_MAX_DIST
if trigger_resume:
self.sng_acc_resume = True
if CC.enabled and CS.car_follow == 1 and CS.out.standstill and self.frame > self.standstill_start + 50:
speed_cmd = True
if CS.out.standstill and not self.prev_standstill:
self.standstill_start = self.frame
self.prev_standstill = CS.out.standstill
self.prev_cruise_state = CS.cruise_state
if self.sng_acc_resume:
if self.sng_acc_resume_cnt < 5:
throttle_cmd = True
self.sng_acc_resume_cnt += 1
else:
self.sng_acc_resume = False
self.sng_acc_resume_cnt = -1
self.prev_close_distance = CS.close_distance
self.prev_close_distance = close_distance
return throttle_cmd, speed_cmd
@@ -15,7 +15,6 @@ class CarState(CarStateBase):
self.shifter_values = can_define.dv["Transmission"]["Gear"]
self.angle_rate_calulator = CanSignalRateCalculator(50)
self.sng_cruise_enabled = False
def update(self, can_parsers, starpilot_toggles) -> structs.CarState:
cp = can_parsers[Bus.pt]
@@ -134,22 +133,6 @@ class CarState(CarStateBase):
self.cruise_state = cp_cam.vl["ES_DashStatus"]["Cruise_State"]
self.throttle_msg = copy.copy(cp.vl["Throttle"])
sng_standstill_hold = (
ret.cruiseState.available and
ret.standstill and
self.car_follow == 1 and
self.cruise_state == 3 and
not ret.gasPressed
)
if ret.cruiseState.enabled:
self.sng_cruise_enabled = True
elif self.sng_cruise_enabled and sng_standstill_hold:
ret.cruiseState.enabled = True
else:
self.sng_cruise_enabled = False
else:
self.sng_cruise_enabled = False
return ret, fp_ret
@staticmethod
@@ -358,6 +358,19 @@ def create_brake_pedal(packer, frame, brake_pedal_msg, speed_cmd, brake_cmd):
return packer.make_can_msg("Brake_Pedal", CanBus.camera, values)
def create_preglobal_brake_pedal(packer, brake_pedal_msg, speed_cmd):
values = {s: brake_pedal_msg[s] for s in sorted([
"Brake_Pedal",
"Signal1",
"Speed",
])}
if speed_cmd:
values["Speed"] = 1
return packer.make_can_msg("Brake_Pedal", CanBus.camera, values)
def create_throttle(packer, frame, throttle_msg, throttle_cmd):
values = {s: throttle_msg[s] for s in sorted([
"CHECKSUM",
@@ -59,6 +59,7 @@ class SubaruSafetyFlags(IntFlag):
GEN2 = 1
LONG = 2
PREGLOBAL_REVERSED_DRIVER_TORQUE = 4
STOP_AND_GO = 8
class SubaruFlags(IntFlag):
@@ -74,6 +74,7 @@ def get_test_starpilot_toggles() -> SimpleNamespace:
reverse_cruise_increase=False,
sng_hack=False,
subaru_sng=False,
subaru_sng_manual_parking_brake=False,
unlock_doors=False,
vEgoStopping=0.5,
volt_sng=False,
@@ -24,6 +24,7 @@
#define MSG_SUBARU_Throttle 0x40U
#define MSG_SUBARU_Steering_Torque 0x119U
#define MSG_SUBARU_Wheel_Speeds 0x13aU
#define MSG_SUBARU_Brake_Pedal 0x139U
#define MSG_SUBARU_ES_LKAS 0x122U
#define MSG_SUBARU_ES_Brake 0x220U
@@ -63,6 +64,10 @@
{MSG_SUBARU_ES_STATIC_1, SUBARU_MAIN_BUS, 8, .check_relay = false}, \
{MSG_SUBARU_ES_STATIC_2, SUBARU_MAIN_BUS, 8, .check_relay = false}, \
#define SUBARU_STOP_AND_GO_ADDITIONAL_TX_MSGS() \
{MSG_SUBARU_Throttle, SUBARU_CAM_BUS, 8, .check_relay = true}, \
{MSG_SUBARU_Brake_Pedal, SUBARU_CAM_BUS, 8, .check_relay = true}, \
#define SUBARU_COMMON_RX_CHECKS(alt_bus) \
{.msg = {{MSG_SUBARU_Throttle, SUBARU_MAIN_BUS, 8, 100U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \
{.msg = {{MSG_SUBARU_Steering_Torque, SUBARU_MAIN_BUS, 8, 50U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \
@@ -72,6 +77,7 @@
static bool subaru_gen2 = false;
static bool subaru_longitudinal = false;
static bool subaru_stop_and_go = false;
static uint32_t subaru_get_checksum(const CANPacket_t *msg) {
return (uint8_t)msg->data[0];
@@ -227,6 +233,12 @@ static safety_config subaru_init(uint16_t param) {
SUBARU_GEN2_LONG_ADDITIONAL_TX_MSGS()
};
static const CanMsg SUBARU_STOP_AND_GO_TX_MSGS[] = {
SUBARU_BASE_TX_MSGS(SUBARU_MAIN_BUS, MSG_SUBARU_ES_LKAS)
SUBARU_COMMON_TX_MSGS(SUBARU_MAIN_BUS)
SUBARU_STOP_AND_GO_ADDITIONAL_TX_MSGS()
};
static RxCheck subaru_rx_checks[] = {
SUBARU_COMMON_RX_CHECKS(SUBARU_MAIN_BUS)
};
@@ -239,6 +251,9 @@ static safety_config subaru_init(uint16_t param) {
subaru_gen2 = GET_FLAG(param, SUBARU_PARAM_GEN2);
const uint16_t SUBARU_PARAM_STOP_AND_GO = 8;
subaru_stop_and_go = GET_FLAG(param, SUBARU_PARAM_STOP_AND_GO);
#ifdef ALLOW_DEBUG
const uint16_t SUBARU_PARAM_LONGITUDINAL = 2;
subaru_longitudinal = GET_FLAG(param, SUBARU_PARAM_LONGITUDINAL);
@@ -250,6 +265,7 @@ static safety_config subaru_init(uint16_t param) {
BUILD_SAFETY_CFG(subaru_gen2_rx_checks, SUBARU_GEN2_TX_MSGS);
} else {
ret = subaru_longitudinal ? BUILD_SAFETY_CFG(subaru_rx_checks, SUBARU_LONG_TX_MSGS) : \
subaru_stop_and_go ? BUILD_SAFETY_CFG(subaru_rx_checks, SUBARU_STOP_AND_GO_TX_MSGS) : \
BUILD_SAFETY_CFG(subaru_rx_checks, SUBARU_TX_MSGS);
}
return ret;
@@ -17,7 +17,16 @@
#define SUBARU_PG_MAIN_BUS 0U
#define SUBARU_PG_CAM_BUS 2U
#define SUBARU_PG_COMMON_TX_MSGS() \
{MSG_SUBARU_PG_ES_Distance, SUBARU_PG_MAIN_BUS, 8, .check_relay = true}, \
{MSG_SUBARU_PG_ES_LKAS, SUBARU_PG_MAIN_BUS, 8, .check_relay = true}, \
#define SUBARU_PG_STOP_AND_GO_ADDITIONAL_TX_MSGS() \
{MSG_SUBARU_PG_Throttle, SUBARU_PG_CAM_BUS, 8, .check_relay = false}, \
{MSG_SUBARU_PG_Brake_Pedal, SUBARU_PG_CAM_BUS, 4, .check_relay = false}, \
static bool subaru_pg_reversed_driver_torque = false;
static bool subaru_pg_stop_and_go = false;
static void subaru_preglobal_rx_hook(const CANPacket_t *msg) {
if (msg->bus == SUBARU_PG_MAIN_BUS) {
@@ -81,8 +90,12 @@ static bool subaru_preglobal_tx_hook(const CANPacket_t *msg) {
static safety_config subaru_preglobal_init(uint16_t param) {
static const CanMsg SUBARU_PG_TX_MSGS[] = {
{MSG_SUBARU_PG_ES_Distance, SUBARU_PG_MAIN_BUS, 8, .check_relay = true},
{MSG_SUBARU_PG_ES_LKAS, SUBARU_PG_MAIN_BUS, 8, .check_relay = true}
SUBARU_PG_COMMON_TX_MSGS()
};
static const CanMsg SUBARU_PG_STOP_AND_GO_TX_MSGS[] = {
SUBARU_PG_COMMON_TX_MSGS()
SUBARU_PG_STOP_AND_GO_ADDITIONAL_TX_MSGS()
};
// TODO: do checksum and counter checks after adding the signals to the outback dbc file
@@ -95,9 +108,14 @@ static safety_config subaru_preglobal_init(uint16_t param) {
};
const uint16_t SUBARU_PG_PARAM_REVERSED_DRIVER_TORQUE = 4;
const uint16_t SUBARU_PG_PARAM_STOP_AND_GO = 8;
subaru_pg_reversed_driver_torque = GET_FLAG(param, SUBARU_PG_PARAM_REVERSED_DRIVER_TORQUE);
return BUILD_SAFETY_CFG(subaru_preglobal_rx_checks, SUBARU_PG_TX_MSGS);
subaru_pg_stop_and_go = GET_FLAG(param, SUBARU_PG_PARAM_STOP_AND_GO);
safety_config ret = subaru_pg_stop_and_go ? BUILD_SAFETY_CFG(subaru_preglobal_rx_checks, SUBARU_PG_STOP_AND_GO_TX_MSGS) : \
BUILD_SAFETY_CFG(subaru_preglobal_rx_checks, SUBARU_PG_TX_MSGS);
return ret;
}
const safety_hooks subaru_preglobal_hooks = {
@@ -16,6 +16,7 @@ class SubaruMsg(enum.IntEnum):
Throttle = 0x40
Steering_Torque = 0x119
Wheel_Speeds = 0x13a
Brake_Pedal = 0x139
ES_LKAS = 0x122
ES_LKAS_ANGLE = 0x124
ES_Brake = 0x220
@@ -182,6 +183,12 @@ class TestSubaruGen1TorqueStockLongitudinalSafety(TestSubaruStockLongitudinalSaf
TX_MSGS = lkas_tx_msgs(SUBARU_MAIN_BUS)
class TestSubaruGen1StopAndGoSafety(TestSubaruStockLongitudinalSafetyBase, TestSubaruTorqueSafetyBase):
FLAGS = SubaruSafetyFlags.STOP_AND_GO
TX_MSGS = lkas_tx_msgs(SUBARU_MAIN_BUS) + [[SubaruMsg.Throttle, SUBARU_CAM_BUS],
[SubaruMsg.Brake_Pedal, SUBARU_CAM_BUS]]
class TestSubaruGen2TorqueSafetyBase(TestSubaruTorqueSafetyBase):
ALT_MAIN_BUS = SUBARU_ALT_BUS
ALT_CAM_BUS = SUBARU_ALT_BUS
@@ -70,5 +70,10 @@ class TestSubaruPreglobalReversedDriverTorqueSafety(TestSubaruPreglobalSafety):
DBC = "subaru_outback_2019_generated"
class TestSubaruPreglobalStopAndGoSafety(TestSubaruPreglobalSafety):
FLAGS = SubaruSafetyFlags.STOP_AND_GO
TX_MSGS = [[0x161, 0], [0x164, 0], [0x140, 2], [0xD1, 2]]
if __name__ == "__main__":
unittest.main()
+1
View File
@@ -47,6 +47,7 @@ def get_test_starpilot_toggles() -> SimpleNamespace:
reverse_cruise_increase=False,
sng_hack=False,
subaru_sng=False,
subaru_sng_manual_parking_brake=False,
unlock_doors=False,
vEgoStopping=0.5,
volt_sng=False,
@@ -169,6 +169,12 @@ class VehicleSettingsManagerView(AetherInteractiveMixin, Widget):
"get_state": lambda: self._controller._params.get_bool("SubaruSNG"),
"set_state": lambda s: self._controller._on_toggle("SubaruSNG"),
})
if self._controller._params.get_bool("SubaruSNG"):
toggles.append({
"title": tr("Manual Parking Brake SNG"),
"get_state": lambda: self._controller._params.get_bool("SubaruSNGManualParkingBrake"),
"set_state": lambda s: self._controller._on_toggle("SubaruSNGManualParkingBrake"),
})
if cs.isToyota:
toggles.append({
+1
View File
@@ -181,6 +181,7 @@ SAFE_MODE_MANAGED_KEYS = (
"FrogsGoMoosTweak",
"SNGHack",
"SubaruSNG",
"SubaruSNGManualParkingBrake",
"VoltSNG",
"GMAutoHold",
"GMPedalLongitudinal",
+1
View File
@@ -1320,6 +1320,7 @@ class StarPilotVariables:
toggle.startup_alert_bottom = "Always keep hands on wheel and eyes on road"
toggle.subaru_sng = self.get_value("SubaruSNG", condition=toggle.car_make == "subaru" and not (CP.flags & SubaruFlags.GLOBAL_GEN2 or CP.flags & SubaruFlags.HYBRID))
toggle.subaru_sng_manual_parking_brake = self.get_value("SubaruSNGManualParkingBrake", condition=toggle.subaru_sng)
toggle.tethering_config = self.get_value("TetheringEnabled", cast=float)
@@ -2478,6 +2478,13 @@
"data_type": "bool",
"ui_type": "toggle"
},
{
"key": "SubaruSNGManualParkingBrake",
"label": "Stop and Go for Manual Parking Brake",
"description": "Use the manual-parking-brake Subaru stop-and-go strategy. Enable this for supported Subaru Global models with a manual handbrake. Keep it off on models with an electric parking brake.",
"data_type": "bool",
"ui_type": "toggle"
},
{
"key": "ClusterOffset",
"label": "Dashboard Speed Offset",
@@ -181,6 +181,7 @@ StarPilotVehiclesPanel::StarPilotVehiclesPanel(StarPilotSettingsWindow *parent,
{"SubaruToggles", tr("Subaru Settings"), tr("<b>StarPilot features for Subaru vehicles.</b>"), ""},
{"SubaruSNG", tr("Stop and Go"), tr("Stop and go for supported Subaru vehicles."), ""},
{"SubaruSNGManualParkingBrake", tr("Stop and Go for Manual Parking Brake"), tr("<b>Use the manual-parking-brake Subaru stop-and-go strategy.</b><br><br>Enable this for supported Subaru Global models with a manual handbrake. Keep it off on models with an electric parking brake."), ""},
{"ToyotaToggles", tr("Toyota/Lexus Settings"), tr("<b>StarPilot features for Lexus and Toyota vehicles.</b>"), ""},
{"ToyotaDoors", tr("Automatically Lock/Unlock Doors"), tr("<b>Automatically lock/unlock doors</b> when shifting in and out of drive."), ""},
@@ -419,6 +420,10 @@ void StarPilotVehiclesPanel::updateToggles() {
setVisible &= parent->hasSNG;
}
else if (key == "SubaruSNGManualParkingBrake") {
setVisible &= parent->hasSNG && params.getBool("SubaruSNG");
}
else if (key == "VoltSNG") {
setVisible &= parent->isVolt && !parent->hasSNG;
}
+1 -1
View File
@@ -25,7 +25,7 @@ private:
QSet<QString> gmKeys = {"GMPedalLongitudinal", "GMDashSpoofOffsets", "LongPitch", "RemoteStartBootsComma", "RemapCancelToDistance", "VoltSNG"};
QSet<QString> longitudinalKeys = {"FrogsGoMoosTweak", "GMDashSpoofOffsets", "LongPitch", "RemapCancelToDistance", "SNGHack", "VoltSNG"};
QSet<QString> subaruKeys = {"SubaruSNG"};
QSet<QString> subaruKeys = {"SubaruSNG", "SubaruSNGManualParkingBrake"};
QSet<QString> toyotaKeys = {"ClusterOffset", "FrogsGoMoosTweak", "LockDoorsTimer", "SNGHack", "ToyotaDoors"};
QSet<QString> vehicleInfoKeys = {"BlindSpotSupport", "HardwareDetected", "OpenpilotLongitudinal", "PedalSupport", "RadarSupport", "SDSUSupport", "SNGSupport"};
+1
View File
@@ -344,6 +344,7 @@ StoppedTimer
StoppingDecelRate
StoppingDecelRateStock
SubaruSNG
SubaruSNGManualParkingBrake
SwitchbackModeCooldown
SwitchbackModeEnabled
TacoTune