mirror of
https://github.com/firestar5683/StarPilot.git
synced 2026-06-28 01:52:06 +08:00
DM and volt auto hold
This commit is contained in:
@@ -7,7 +7,7 @@ from opendbc.car.gm import gmcan
|
||||
from opendbc.car.common.conversions import Conversions as CV
|
||||
from opendbc.car.gm.values import (
|
||||
ASCM_INT, CAR, CC_ONLY_CAR, CC_REGEN_PADDLE_CAR, DBC, EV_CAR, SDGM_CAR, AccState, CanBus, CarControllerParams,
|
||||
CruiseButtons, GMFlags,
|
||||
CruiseButtons, GMFlags, GMSafetyFlags,
|
||||
)
|
||||
from opendbc.car.interfaces import CarControllerBase
|
||||
from openpilot.common.params import Params, UnknownKeyName
|
||||
@@ -117,9 +117,12 @@ def get_testing_ground_1_brake_switch_bias(v_ego: float) -> int:
|
||||
|
||||
|
||||
def supports_volt_auto_hold(CP, starpilot_toggles):
|
||||
safety_cfg = getattr(CP, "safetyConfigs", ())
|
||||
safety_param = safety_cfg[0].safetyParam if safety_cfg else 0
|
||||
stock_hold_safety_ready = CP.openpilotLongitudinalControl or bool(safety_param & GMSafetyFlags.FLAG_GM_PANDA_PADDLE_SCHED.value)
|
||||
return (
|
||||
getattr(starpilot_toggles, "gm_auto_hold", False) and
|
||||
CP.openpilotLongitudinalControl and
|
||||
stock_hold_safety_ready and
|
||||
CP.carFingerprint in AUTO_HOLD_VOLT_CARS
|
||||
)
|
||||
|
||||
@@ -130,6 +133,23 @@ def estimate_auto_hold_brake(driver_brake: float, op_brake: float) -> int:
|
||||
return int(round(np.clip(hold_brake, AUTO_HOLD_MIN_BRAKE, AUTO_HOLD_MAX_BRAKE)))
|
||||
|
||||
|
||||
def get_friction_brake_bus(CP):
|
||||
volt_gateway_alt_brake = (
|
||||
CP.carFingerprint == CAR.CHEVROLET_VOLT and
|
||||
CP.networkLocation == NetworkLocation.gateway and
|
||||
bool(CP.flags & GMFlags.NO_ACCELERATOR_POS_MSG.value)
|
||||
)
|
||||
if volt_gateway_alt_brake:
|
||||
return CanBus.POWERTRAIN
|
||||
|
||||
if CP.networkLocation == NetworkLocation.fwdCamera:
|
||||
if CP.carFingerprint in SDGM_CAR:
|
||||
return CanBus.CAMERA
|
||||
return CanBus.POWERTRAIN
|
||||
|
||||
return CanBus.CHASSIS
|
||||
|
||||
|
||||
class CarController(CarControllerBase):
|
||||
def __init__(self, dbc_names, CP):
|
||||
super().__init__(dbc_names, CP)
|
||||
@@ -333,6 +353,7 @@ class CarController(CarControllerBase):
|
||||
accel = actuators.accel
|
||||
press_regen_paddle = False
|
||||
auto_hold_enabled = supports_volt_auto_hold(self.CP, starpilot_toggles)
|
||||
stock_hold_apply_brake = self.apply_brake if self.CP.openpilotLongitudinalControl else 0
|
||||
|
||||
hold_ready = (
|
||||
auto_hold_enabled and
|
||||
@@ -349,8 +370,8 @@ class CarController(CarControllerBase):
|
||||
|
||||
if CS.out.vEgo > 0.1 or CS.out.gasPressed or CS.out.gearShifter not in AUTO_HOLD_DRIVE_GEARS:
|
||||
self.auto_hold_brake = 0
|
||||
elif CS.out.brakePressed or self.apply_brake > 0:
|
||||
self.auto_hold_brake = estimate_auto_hold_brake(CS.out.brake, self.apply_brake)
|
||||
elif CS.out.brakePressed or stock_hold_apply_brake > 0:
|
||||
self.auto_hold_brake = estimate_auto_hold_brake(CS.out.brake, stock_hold_apply_brake)
|
||||
|
||||
if self.frame % 25 == 0:
|
||||
try:
|
||||
@@ -432,6 +453,13 @@ class CarController(CarControllerBase):
|
||||
paddle_sched_feed_active = False
|
||||
|
||||
paddle_spoof_pressed = raw_regen_active and (CS.out.vEgo > 2.68)
|
||||
auto_hold_active = (
|
||||
hold_ready and
|
||||
CS.auto_hold_armed and
|
||||
not CC.longActive and
|
||||
not CS.out.regenBraking and
|
||||
CS.out.vEgo < 0.02
|
||||
)
|
||||
|
||||
# Steering (Active: 50Hz, inactive: 10Hz)
|
||||
steer_step = self.params.STEER_STEP if CC.latActive else self.params.INACTIVE_STEER_STEP
|
||||
@@ -481,13 +509,6 @@ class CarController(CarControllerBase):
|
||||
interceptor_gas_cmd = 0
|
||||
at_full_stop = CC.longActive and CS.out.standstill
|
||||
near_stop = CC.longActive and (CS.out.vEgo < self.params.NEAR_STOP_BRAKE_PHASE)
|
||||
auto_hold_active = (
|
||||
hold_ready and
|
||||
CS.auto_hold_armed and
|
||||
not CC.longActive and
|
||||
not CS.out.regenBraking and
|
||||
CS.out.vEgo < 0.02
|
||||
)
|
||||
if not CC.longActive:
|
||||
# ASCM sends max regen when not enabled
|
||||
self.apply_gas = self.params.INACTIVE_REGEN
|
||||
@@ -607,19 +628,11 @@ class CarController(CarControllerBase):
|
||||
if self.CP.enableGasInterceptorDEPRECATED:
|
||||
can_sends.append(create_gas_interceptor_command(self.packer_pt, interceptor_gas_cmd, idx))
|
||||
if self.CP.carFingerprint not in CC_ONLY_CAR:
|
||||
volt_gateway_alt_brake = (
|
||||
self.CP.carFingerprint == CAR.CHEVROLET_VOLT and
|
||||
self.CP.networkLocation == NetworkLocation.gateway and
|
||||
bool(self.CP.flags & GMFlags.NO_ACCELERATOR_POS_MSG.value)
|
||||
)
|
||||
friction_brake_bus = CanBus.POWERTRAIN if volt_gateway_alt_brake else CanBus.CHASSIS
|
||||
friction_brake_bus = get_friction_brake_bus(self.CP)
|
||||
# GM Camera exceptions
|
||||
# TODO: can we always check the longControlState?
|
||||
if self.CP.networkLocation == NetworkLocation.fwdCamera:
|
||||
at_full_stop = at_full_stop and stopping
|
||||
friction_brake_bus = CanBus.POWERTRAIN
|
||||
if self.CP.carFingerprint in SDGM_CAR:
|
||||
friction_brake_bus = CanBus.CAMERA
|
||||
|
||||
if self.CP.autoResumeSng:
|
||||
resume = actuators.longControlState != LongCtrlState.starting or CC.cruiseControl.resume
|
||||
@@ -704,6 +717,18 @@ class CarController(CarControllerBase):
|
||||
can_sends.append(gmcan.create_buttons(self.packer_pt, cancel_bus, (CS.buttons_counter + 1) % 4, CruiseButtons.CANCEL))
|
||||
|
||||
else:
|
||||
if self.frame % 4 == 0 and auto_hold_active:
|
||||
idx = (self.frame // 4) % 4
|
||||
hold_brake = self.auto_hold_brake or estimate_auto_hold_brake(CS.out.brake, stock_hold_apply_brake)
|
||||
hold_standstill = CS.pcm_acc_status == AccState.STANDSTILL
|
||||
hold_near_stop = CS.out.vEgo < self.params.NEAR_STOP_BRAKE_PHASE
|
||||
can_sends.append(gmcan.create_friction_brake_command(
|
||||
self.packer_ch, get_friction_brake_bus(self.CP), hold_brake, idx, False, hold_near_stop, hold_standstill, self.CP))
|
||||
CS.auto_hold_engaged = True
|
||||
CS.auto_hold_fault_suppression_timer = 1.0
|
||||
elif self.frame % 4 == 0:
|
||||
CS.auto_hold_engaged = False
|
||||
|
||||
# While car is braking, cancel button causes ECM to enter a soft disable state with a fault status.
|
||||
# A delayed cancellation allows camera to cancel and avoids a fault when user depresses brake quickly
|
||||
self.cancel_counter = self.cancel_counter + 1 if CC.cruiseControl.cancel else 0
|
||||
|
||||
@@ -189,6 +189,10 @@ class CarInterface(CarInterfaceBase):
|
||||
disable_openpilot_long = params.get_bool("DisableOpenpilotLongitudinal")
|
||||
except UnknownKeyName:
|
||||
disable_openpilot_long = False
|
||||
try:
|
||||
gm_auto_hold = params.get_bool("GMAutoHold")
|
||||
except UnknownKeyName:
|
||||
gm_auto_hold = False
|
||||
|
||||
ret.brand = "gm"
|
||||
ret.safetyConfigs = [get_safety_config(structs.CarParams.SafetyModel.gm)]
|
||||
@@ -632,6 +636,21 @@ class CarInterface(CarInterfaceBase):
|
||||
if remote_start_boots_comma:
|
||||
ret.safetyConfigs[0].safetyParam |= GMSafetyFlags.FLAG_GM_REMOTE_START_BOOTS_COMMA.value
|
||||
|
||||
volt_stock_auto_hold_safety = (
|
||||
gm_auto_hold and
|
||||
not ret.openpilotLongitudinalControl and
|
||||
candidate in {
|
||||
CAR.CHEVROLET_VOLT,
|
||||
CAR.CHEVROLET_VOLT_2019,
|
||||
CAR.CHEVROLET_VOLT_ASCM,
|
||||
CAR.CHEVROLET_VOLT_CAMERA,
|
||||
}
|
||||
)
|
||||
if volt_stock_auto_hold_safety:
|
||||
# Reuse the paddle-scheduler safety bit as a stock-Volt auto-hold marker on
|
||||
# non-pedal paths. The scheduler logic remains inactive without pedal-long.
|
||||
ret.safetyConfigs[0].safetyParam |= GMSafetyFlags.FLAG_GM_PANDA_PADDLE_SCHED.value
|
||||
|
||||
use_panda_3d1_sched = (
|
||||
ret.openpilotLongitudinalControl and
|
||||
ret.enableGasInterceptorDEPRECATED and
|
||||
|
||||
@@ -60,6 +60,7 @@ static bool gm_panda_3d1_sched = false;
|
||||
static bool gm_panda_paddle_sched = false;
|
||||
static bool gm_bolt_2022_pedal = false;
|
||||
static bool gm_alt_brake = false;
|
||||
static bool gm_volt_auto_hold = false;
|
||||
|
||||
static bool gm_cc_long = false;
|
||||
static bool gm_has_acc = true;
|
||||
@@ -96,6 +97,7 @@ static const uint32_t GM_PADDLE_TX_OFFSET_US = 0U;
|
||||
static const uint32_t GM_PADDLE_LOCK_TOLERANCE_US = 5000U;
|
||||
static const uint32_t GM_PADDLE_STALE_US = 100000U;
|
||||
static const uint32_t GM_PADDLE_FEED_STALE_US = 100000U;
|
||||
static const int GM_VOLT_AUTO_HOLD_MAX_BRAKE = 240;
|
||||
|
||||
void can_send(CANPacket_t *to_push, uint8_t bus_number, bool skip_tx_hook);
|
||||
void can_set_checksum(CANPacket_t *packet);
|
||||
@@ -363,7 +365,15 @@ static bool gm_tx_hook(const CANPacket_t *msg) {
|
||||
if (msg->addr == 0x315U) {
|
||||
int brake = ((msg->data[0] & 0xFU) << 8) + msg->data[1];
|
||||
brake = (0x1000 - brake) & 0xFFF;
|
||||
if (longitudinal_brake_checks(brake, *gm_long_limits)) {
|
||||
bool stock_auto_hold_brake_allowed = gm_volt_auto_hold && !vehicle_moving && !gas_pressed_prev;
|
||||
bool violation = false;
|
||||
violation |= !(get_longitudinal_allowed() || stock_auto_hold_brake_allowed) && (brake != 0);
|
||||
if (stock_auto_hold_brake_allowed && !get_longitudinal_allowed()) {
|
||||
violation |= brake > GM_VOLT_AUTO_HOLD_MAX_BRAKE;
|
||||
} else {
|
||||
violation |= brake > gm_long_limits->max_brake;
|
||||
}
|
||||
if (violation) {
|
||||
tx = false;
|
||||
}
|
||||
}
|
||||
@@ -602,12 +612,22 @@ static safety_config gm_init(uint16_t param) {
|
||||
{0x200, 0, 6, .check_relay = false},
|
||||
{0x1E1, 0, 7, .check_relay = false},
|
||||
{0xBD, 0, 7, .check_relay = false}, {0x1F5, 0, 8, .check_relay = false}}; // pt bus
|
||||
static const CanMsg GM_CAM_VOLT_AUTO_HOLD_TX_MSGS[] = {{0x180, 0, 4, .check_relay = true}, {0x370, 0, 6, .check_relay = false}, {0x3D1, 0, 8, .check_relay = false}, {0x315, 0, 5, .check_relay = true}, // pt bus
|
||||
{0x1E1, 2, 7, .check_relay = false}, {0x184, 2, 8, .check_relay = true}, // camera bus
|
||||
{0x200, 0, 6, .check_relay = false},
|
||||
{0x1E1, 0, 7, .check_relay = false},
|
||||
{0xBD, 0, 7, .check_relay = false}, {0x1F5, 0, 8, .check_relay = false}}; // pt bus
|
||||
|
||||
static const CanMsg GM_SDGM_TX_MSGS[] = {{0x180, 0, 4, .check_relay = true}, {0x370, 0, 6, .check_relay = false}, {0x3D1, 0, 8, .check_relay = false}, // pt bus
|
||||
{0x1E1, 2, 7, .check_relay = false}, {0x184, 2, 8, .check_relay = false}, // camera bus
|
||||
{0x200, 0, 6, .check_relay = false},
|
||||
{0x1E1, 0, 7, .check_relay = false},
|
||||
{0xBD, 0, 7, .check_relay = false}, {0x1F5, 0, 8, .check_relay = false}}; // pt bus
|
||||
static const CanMsg GM_SDGM_VOLT_AUTO_HOLD_TX_MSGS[] = {{0x180, 0, 4, .check_relay = true}, {0x370, 0, 6, .check_relay = false}, {0x3D1, 0, 8, .check_relay = false}, // pt bus
|
||||
{0x1E1, 2, 7, .check_relay = false}, {0x184, 2, 8, .check_relay = false}, {0x315, 2, 5, .check_relay = false}, // camera bus
|
||||
{0x200, 0, 6, .check_relay = false},
|
||||
{0x1E1, 0, 7, .check_relay = false},
|
||||
{0xBD, 0, 7, .check_relay = false}, {0x1F5, 0, 8, .check_relay = false}}; // pt bus
|
||||
|
||||
static const CanMsg GM_CAM_NO_CAMERA_TX_MSGS[] = {{0x180, 0, 4, .check_relay = false}, {0x370, 0, 6, .check_relay = false}, {0x3D1, 0, 8, .check_relay = false}, // pt bus
|
||||
{0x409, 0, 7, .check_relay = false}, {0x40A, 0, 7, .check_relay = false},
|
||||
@@ -615,6 +635,12 @@ static safety_config gm_init(uint16_t param) {
|
||||
{0x200, 0, 6, .check_relay = false},
|
||||
{0x1E1, 0, 7, .check_relay = false},
|
||||
{0xBD, 0, 7, .check_relay = false}, {0x1F5, 0, 8, .check_relay = false}}; // pt bus
|
||||
static const CanMsg GM_CAM_NO_CAMERA_VOLT_AUTO_HOLD_TX_MSGS[] = {{0x180, 0, 4, .check_relay = false}, {0x370, 0, 6, .check_relay = false}, {0x3D1, 0, 8, .check_relay = false}, {0x315, 0, 5, .check_relay = false}, // pt bus
|
||||
{0x409, 0, 7, .check_relay = false}, {0x40A, 0, 7, .check_relay = false},
|
||||
{0x1E1, 2, 7, .check_relay = false}, {0x184, 2, 8, .check_relay = false}, // camera bus
|
||||
{0x200, 0, 6, .check_relay = false},
|
||||
{0x1E1, 0, 7, .check_relay = false},
|
||||
{0xBD, 0, 7, .check_relay = false}, {0x1F5, 0, 8, .check_relay = false}}; // pt bus
|
||||
|
||||
static RxCheck gm_no_acc_rx_checks[] = {
|
||||
GM_COMMON_RX_CHECKS
|
||||
@@ -671,6 +697,8 @@ static safety_config gm_init(uint16_t param) {
|
||||
gm_remote_start_boots_comma = GET_FLAG(param, GM_PARAM_REMOTE_START_BOOTS_COMMA);
|
||||
gm_panda_3d1_sched = GET_FLAG(param, GM_PARAM_PANDA_3D1_SCHED) && gm_pedal_long && !gm_has_acc && !gm_bolt_2022_pedal;
|
||||
gm_panda_paddle_sched = GET_FLAG(param, GM_PARAM_PANDA_PADDLE_SCHED) && gm_pedal_long && enable_gas_interceptor;
|
||||
// Reuse the paddle-scheduler bit as a stock-Volt auto-hold marker on non-pedal ACC paths.
|
||||
gm_volt_auto_hold = GET_FLAG(param, GM_PARAM_PANDA_PADDLE_SCHED) && !gm_pedal_long && !gm_cc_long && gm_has_acc;
|
||||
gm_alt_brake = GET_FLAG(param, GM_PARAM_NO_CAMERA) && (gm_hw == GM_ASCM) && !gm_sdgm && !gm_ascm_int;
|
||||
|
||||
gm_3d1_spoof_valid = false;
|
||||
@@ -714,7 +742,11 @@ static safety_config gm_init(uint16_t param) {
|
||||
const bool gm_sdgm_stock = gm_sdgm && !gm_cc_long && !gm_cam_long && !gm_no_camera;
|
||||
// SDGM behaves like a forwarding camera path for whitelist/forwarding purposes.
|
||||
if (gm_sdgm_stock) {
|
||||
ret = BUILD_SAFETY_CFG(gm_rx_checks, GM_SDGM_TX_MSGS);
|
||||
if (gm_volt_auto_hold) {
|
||||
ret = BUILD_SAFETY_CFG(gm_rx_checks, GM_SDGM_VOLT_AUTO_HOLD_TX_MSGS);
|
||||
} else {
|
||||
ret = BUILD_SAFETY_CFG(gm_rx_checks, GM_SDGM_TX_MSGS);
|
||||
}
|
||||
} else if ((gm_hw == GM_CAM) || gm_sdgm) {
|
||||
// FIXME: cppcheck thinks that gm_cam_long is always false. This is not true
|
||||
// if ALLOW_DEBUG is defined but cppcheck is run without ALLOW_DEBUG
|
||||
@@ -732,10 +764,20 @@ static safety_config gm_init(uint16_t param) {
|
||||
ret = BUILD_SAFETY_CFG(gm_rx_checks, GM_CAM_LONG_TX_MSGS);
|
||||
}
|
||||
} else {
|
||||
if (gm_no_camera) {
|
||||
ret = BUILD_SAFETY_CFG(gm_rx_checks, GM_CAM_NO_CAMERA_TX_MSGS);
|
||||
if (gm_volt_auto_hold && gm_sdgm) {
|
||||
ret = BUILD_SAFETY_CFG(gm_rx_checks, GM_SDGM_VOLT_AUTO_HOLD_TX_MSGS);
|
||||
} else if (gm_no_camera) {
|
||||
if (gm_volt_auto_hold) {
|
||||
ret = BUILD_SAFETY_CFG(gm_rx_checks, GM_CAM_NO_CAMERA_VOLT_AUTO_HOLD_TX_MSGS);
|
||||
} else {
|
||||
ret = BUILD_SAFETY_CFG(gm_rx_checks, GM_CAM_NO_CAMERA_TX_MSGS);
|
||||
}
|
||||
} else {
|
||||
ret = BUILD_SAFETY_CFG(gm_rx_checks, GM_CAM_TX_MSGS);
|
||||
if (gm_volt_auto_hold) {
|
||||
ret = BUILD_SAFETY_CFG(gm_rx_checks, GM_CAM_VOLT_AUTO_HOLD_TX_MSGS);
|
||||
} else {
|
||||
ret = BUILD_SAFETY_CFG(gm_rx_checks, GM_CAM_TX_MSGS);
|
||||
}
|
||||
}
|
||||
}
|
||||
} else if (gm_alt_brake) {
|
||||
|
||||
@@ -533,6 +533,64 @@ class TestGmCcLongitudinalPandaSchedSafety(TestGmCcLongitudinalSafety):
|
||||
for btn in range(8):
|
||||
self.assertEqual(enabled and btn in allowed_btns, self._tx(self._button_msg(btn)))
|
||||
|
||||
|
||||
class TestGmVoltAutoHoldCameraSafety(TestGmCameraSafetyBase):
|
||||
TX_MSGS = TestGmCameraSafety.TX_MSGS + [[0x315, 0]]
|
||||
EXTRA_SAFETY_PARAM = GMSafetyFlags.FLAG_GM_PANDA_PADDLE_SCHED
|
||||
|
||||
def setUp(self):
|
||||
self.packer = CANPackerSafety("gm_global_a_powertrain_generated")
|
||||
self.packer_chassis = CANPackerSafety("gm_global_a_chassis")
|
||||
self.safety = libsafety_py.libsafety
|
||||
self.safety.set_safety_hooks(CarParams.SafetyModel.gm, GMSafetyFlags.HW_CAM | self.EXTRA_SAFETY_PARAM)
|
||||
self.safety.init_tests()
|
||||
|
||||
def _send_brake_msg(self, brake):
|
||||
values = {"FrictionBrakeCmd": -brake}
|
||||
return self.packer_chassis.make_can_msg_safety("EBCMFrictionBrakeCmd", 0, values)
|
||||
|
||||
def test_standstill_brake_allowed_without_controls(self):
|
||||
self._rx(self._speed_msg(0))
|
||||
self.safety.set_controls_allowed(False)
|
||||
self.assertTrue(self._tx(self._send_brake_msg(100)))
|
||||
|
||||
def test_moving_brake_blocked_without_controls(self):
|
||||
self._rx(self._speed_msg(self.STANDSTILL_THRESHOLD + 1))
|
||||
self.safety.set_controls_allowed(False)
|
||||
self.assertFalse(self._tx(self._send_brake_msg(100)))
|
||||
|
||||
def test_gas_blocks_standstill_brake_without_controls(self):
|
||||
self._rx(self._speed_msg(0))
|
||||
self._rx(self._user_gas_msg(True))
|
||||
self.safety.set_controls_allowed(False)
|
||||
self.assertFalse(self._tx(self._send_brake_msg(100)))
|
||||
|
||||
|
||||
class TestGmVoltAutoHoldSdgmSafety(TestGmSafetyBase):
|
||||
TX_MSGS = TestGmSdgmSafety.TX_MSGS + [[0x315, 2]]
|
||||
EXTRA_SAFETY_PARAM = GMSafetyFlags.FLAG_GM_PANDA_PADDLE_SCHED
|
||||
|
||||
def setUp(self):
|
||||
self.packer = CANPackerSafety("gm_global_a_powertrain_generated")
|
||||
self.packer_chassis = CANPackerSafety("gm_global_a_chassis")
|
||||
self.safety = libsafety_py.libsafety
|
||||
self.safety.set_safety_hooks(CarParams.SafetyModel.gm, GMSafetyFlags.HW_SDGM | self.EXTRA_SAFETY_PARAM)
|
||||
self.safety.init_tests()
|
||||
|
||||
def _send_brake_msg(self, brake):
|
||||
values = {"FrictionBrakeCmd": -brake}
|
||||
return self.packer_chassis.make_can_msg_safety("EBCMFrictionBrakeCmd", 2, values)
|
||||
|
||||
def test_standstill_brake_allowed_without_controls(self):
|
||||
self._rx(self._speed_msg(0))
|
||||
self.safety.set_controls_allowed(False)
|
||||
self.assertTrue(self._tx(self._send_brake_msg(100)))
|
||||
|
||||
def test_moving_brake_blocked_without_controls(self):
|
||||
self._rx(self._speed_msg(self.STANDSTILL_THRESHOLD + 1))
|
||||
self.safety.set_controls_allowed(False)
|
||||
self.assertFalse(self._tx(self._send_brake_msg(100)))
|
||||
|
||||
def test_3d1_feed_frame_allowed(self):
|
||||
self.assertTrue(self._tx(self._pcm_status_msg(True)))
|
||||
self.assertTrue(self._tx(self._pcm_status_msg(False)))
|
||||
|
||||
Binary file not shown.
|
Before Width: | Height: | Size: 3.5 KiB After Width: | Height: | Size: 1.8 KiB |
@@ -16,11 +16,15 @@ DEBUG = False
|
||||
LOOKING_CENTER_THRESHOLD_UPPER = math.radians(6)
|
||||
LOOKING_CENTER_THRESHOLD_LOWER = math.radians(3)
|
||||
|
||||
CONE_COLOR_GREEN = (0, 255, 64)
|
||||
CONE_COLOR_ORANGE = (255, 115, 0)
|
||||
|
||||
|
||||
class DriverStateRenderer(Widget):
|
||||
BASE_SIZE = 60
|
||||
LINES_ANGLE_INCREMENT = 5
|
||||
LINES_STALE_ANGLES = 3.0 # seconds
|
||||
AWARENESS_UNFULL_THRESHOLD = 0.95 # ~0.5s
|
||||
|
||||
def __init__(self, lines: bool = False, inset: bool = False):
|
||||
super().__init__()
|
||||
@@ -38,8 +42,10 @@ class DriverStateRenderer(Widget):
|
||||
self._should_draw = False
|
||||
self._force_active = False
|
||||
self._looking_center = False
|
||||
self._awareness_unfull = False
|
||||
|
||||
self._fade_filter = FirstOrderFilter(0.0, 0.05, 1 / gui_app.target_fps)
|
||||
self._color_fade_filter = FirstOrderFilter(1.0, 0.05, 1 / gui_app.target_fps)
|
||||
self._pitch_filter = FirstOrderFilter(0.0, 0.05, 1 / gui_app.target_fps, initialized=False)
|
||||
self._yaw_filter = FirstOrderFilter(0.0, 0.05, 1 / gui_app.target_fps, initialized=False)
|
||||
self._rotation_filter = FirstOrderFilter(0.0, 0.1, 1 / gui_app.target_fps, initialized=False)
|
||||
@@ -97,6 +103,7 @@ class DriverStateRenderer(Widget):
|
||||
self._rect.y + (self._rect.height - self._dm_person.height) / 2), 0.0, 1.0,
|
||||
rl.Color(255, 255, 255, int(255 * 0.9 * self._fade_filter.x)))
|
||||
|
||||
green_amount = self._color_fade_filter.update(0.0 if self._awareness_unfull else 1.0)
|
||||
if self.effective_active:
|
||||
source_rect = rl.Rectangle(0, 0, self._dm_cone.width, self._dm_cone.height)
|
||||
dest_rect = rl.Rectangle(
|
||||
@@ -107,13 +114,16 @@ class DriverStateRenderer(Widget):
|
||||
)
|
||||
|
||||
if not self._lines:
|
||||
r = int(round(CONE_COLOR_GREEN[0] * green_amount + CONE_COLOR_ORANGE[0] * (1 - green_amount)))
|
||||
g = int(round(CONE_COLOR_GREEN[1] * green_amount + CONE_COLOR_ORANGE[1] * (1 - green_amount)))
|
||||
b = int(round(CONE_COLOR_GREEN[2] * green_amount + CONE_COLOR_ORANGE[2] * (1 - green_amount)))
|
||||
rl.draw_texture_pro(
|
||||
self._dm_cone,
|
||||
source_rect,
|
||||
dest_rect,
|
||||
rl.Vector2(dest_rect.width / 2, dest_rect.height / 2),
|
||||
self._rotation_filter.x - 90,
|
||||
rl.Color(255, 255, 255, int(255 * self._fade_filter.x)),
|
||||
rl.Color(r, g, b, int(255 * self._fade_filter.x)),
|
||||
)
|
||||
|
||||
else:
|
||||
@@ -156,6 +166,7 @@ class DriverStateRenderer(Widget):
|
||||
self._is_active = dm_state.isActiveMode
|
||||
self._is_rhd = dm_state.isRHD
|
||||
self._face_detected = dm_state.faceDetected
|
||||
self._awareness_unfull = self.effective_active and dm_state.awarenessStatus < self.AWARENESS_UNFULL_THRESHOLD
|
||||
|
||||
driverstate = sm["driverStateV2"]
|
||||
driver_data = driverstate.rightDriverData if self._is_rhd else driverstate.leftDriverData
|
||||
|
||||
@@ -1197,7 +1197,6 @@ class StarPilotVariables:
|
||||
GM_CAR.CHEVROLET_VOLT_ASCM,
|
||||
GM_CAR.CHEVROLET_VOLT_CAMERA,
|
||||
}
|
||||
gm_auto_hold_supported &= toggle.openpilot_longitudinal
|
||||
toggle.gm_auto_hold = self.get_value("GMAutoHold", condition=gm_auto_hold_supported)
|
||||
|
||||
toggle.volt_sng = self.get_value("VoltSNG", condition=toggle.car_model == "CHEVROLET_VOLT")
|
||||
|
||||
@@ -2189,7 +2189,7 @@
|
||||
{
|
||||
"key": "GMAutoHold",
|
||||
"label": "Volt Auto Hold",
|
||||
"description": "Hold the car at a stop on supported non-CC-only Chevy Volt alpha-longitudinal setups until the gas pedal is pressed.",
|
||||
"description": "Hold the car at a stop on supported non-CC-only Chevy Volts until the gas pedal is pressed.",
|
||||
"data_type": "bool",
|
||||
"ui_type": "toggle"
|
||||
},
|
||||
|
||||
Reference in New Issue
Block a user