mawei/StarPilot
1
0
Fork 0
mirror of https://github.com/firestar5683/StarPilot.git synced 2026-07-15 22:32:11 +08:00
Code Issues Packages Projects Releases Wiki Activity

CC longitudinal control

Browse Source 
Co-Authored-By: Eric Brown <13560103+nworb-cire@users.noreply.github.com>
This commit is contained in:
FrogAi
2024-04-01 06:19:58 -07:00
parent 409872bf13
commit 536bc5c7c5
9 changed files with 156 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files
panda/board/safety/safety_gm.h
+26 -11
View File
@@ -46,15 +46,18 @@ const CanMsg GM_ASCM_TX_MSGS[] = {{0x180, 0, 4}, {0x409, 0, 7}, {0x40A, 0, 7}, {
{0x315, 2, 5}, // ch bus
{0x104c006c, 3, 3}, {0x10400060, 3, 5}}; // gmlan
const CanMsg GM_CAM_TX_MSGS[] = {{0x180, 0, 4}, // pt bus
const CanMsg GM_CAM_TX_MSGS[] = {{0x180, 0, 4}, {0x200, 0, 6}, {0x1E1, 0, 7}, // pt bus
{0x1E1, 2, 7}, {0x184, 2, 8}}; // camera bus
const CanMsg GM_CAM_LONG_TX_MSGS[] = {{0x180, 0, 4}, {0x315, 0, 5}, {0x2CB, 0, 8}, {0x370, 0, 6}, // pt bus
{0x184, 2, 8}}; // camera bus
{0x1E1, 2, 7}, {0x184, 2, 8}}; // camera bus
const CanMsg GM_SDGM_TX_MSGS[] = {{0x180, 0, 4}, {0x1E1, 0, 7}, // pt bus
{0x184, 2, 8}}; // camera bus
const CanMsg GM_CC_LONG_TX_MSGS[] = {{0x180, 0, 4}, {0x1E1, 0, 7}, // pt bus
{0x184, 2, 8}, {0x1E1, 2, 7}}; // camera bus
// TODO: do checksum and counter checks. Add correct timestep, 0.1s for now.
RxCheck gm_rx_checks[] = {
{.msg = {{0x184, 0, 8, .frequency = 10U}, { 0 }, { 0 }}},
@@ -63,9 +66,6 @@ RxCheck gm_rx_checks[] = {
{0x1E1, 2, 7, .frequency = 100000U}}}, // SDGM Car
{.msg = {{0xF1, 0, 6, .frequency = 10U}, // Non-SDGM Car
{0xF1, 2, 6, .frequency = 100000U}}}, // SDGM Car
{.msg = {{0xBE, 0, 6, .frequency = 10U}, // Volt, Silverado, Acadia Denali
{0xBE, 0, 7, .frequency = 10U}, // Bolt EUV
{0xBE, 0, 8, .frequency = 10U}}}, // Escalade
{.msg = {{0x1C4, 0, 8, .frequency = 10U}, { 0 }, { 0 }}},
{.msg = {{0xC9, 0, 8, .frequency = 10U}, { 0 }, { 0 }}},
};
@@ -88,9 +88,11 @@ enum {
enum {GM_ASCM, GM_CAM, GM_SDGM} gm_hw = GM_ASCM;
bool gm_cam_long = false;
bool gm_pcm_cruise = false;
bool gm_cc_long = false;
static void handle_gm_wheel_buttons(const CANPacket_t *to_push) {
int button = (GET_BYTE(to_push, 5) & 0x70U) >> 4;
if ((addr == 0x1E1) && gm_cc_long) {
int button = (GET_BYTE(to_push, 5) & 0x70U) >> 4;
// enter controls on falling edge of set or rising edge of resume (avoids fault)
bool set = (button != GM_BTN_SET) && (cruise_button_prev == GM_BTN_SET);
@@ -213,11 +215,16 @@ static bool gm_tx_hook(const CANPacket_t *to_send) {
}
// BUTTONS: used for resume spamming and cruise cancellation with stock longitudinal
if ((addr == 0x1E1) && gm_pcm_cruise) {
if ((addr == 0x1E1) && (gm_pcm_cruise || gm_cc_long)) {
int button = (GET_BYTE(to_send, 5) >> 4) & 0x7U;
bool allowed_cancel = (button == 6) && cruise_engaged_prev;
if (!allowed_cancel) {
bool allowed_btn = (button == GM_BTN_CANCEL) && cruise_engaged_prev;
// For standard CC, allow spamming of SET / RESUME
if (gm_cc_long) {
allowed_btn |= cruise_engaged_prev && (button == GM_BTN_SET || button == GM_BTN_RESUME || button == GM_BTN_UNPRESS);
}
if (!allowed_btn) {
tx = false;
}
}
@@ -280,11 +287,19 @@ static safety_config gm_init(uint16_t param) {
#ifdef ALLOW_DEBUG
gm_cam_long = GET_FLAG(param, GM_PARAM_HW_CAM_LONG);
#endif
gm_pcm_cruise = (gm_hw == GM_CAM) && !gm_cam_long || (gm_hw == GM_SDGM);
gm_cc_long = GET_FLAG(param, GM_PARAM_CC_LONG);
gm_cam_long = GET_FLAG(param, GM_PARAM_HW_CAM_LONG) && !gm_cc_long;
gm_pcm_cruise = (gm_hw == GM_CAM) && (!gm_cam_long || gm_cc_long) || (gm_hw == GM_SDGM);
safety_config ret = BUILD_SAFETY_CFG(gm_rx_checks, GM_ASCM_TX_MSGS);
if (gm_hw == GM_CAM) {
ret = gm_cam_long ? BUILD_SAFETY_CFG(gm_rx_checks, GM_CAM_LONG_TX_MSGS) : BUILD_SAFETY_CFG(gm_rx_checks, GM_CAM_TX_MSGS);
if (gm_cc_long) {
ret = BUILD_SAFETY_CFG(gm_rx_checks, GM_CC_LONG_TX_MSGS);
} else if (gm_cam_long) {
ret = BUILD_SAFETY_CFG(gm_rx_checks, GM_CAM_LONG_TX_MSGS);
} else {
ret = BUILD_SAFETY_CFG(gm_rx_checks, GM_CAM_TX_MSGS);
}
} else if (gm_hw == GM_SDGM) {
ret = BUILD_SAFETY_CFG(gm_rx_checks, GM_SDGM_TX_MSGS);
}
panda/python/__init__.py
+1
View File
@@ -231,6 +231,7 @@ class Panda:
FLAG_GM_HW_CAM = 1
FLAG_GM_HW_CAM_LONG = 2
FLAG_GM_HW_SDGM = 4
FLAG_GM_CC_LONG = 8
FLAG_GM_HW_ASCM_LONG = 16
FLAG_GM_NO_ACC = 64
panda/tests/safety/common.py
+1 -1
View File
@@ -849,7 +849,7 @@ class PandaSafetyTest(PandaSafetyTestBase):
continue
if {attr, current_test}.issubset({'TestVolkswagenPqSafety', 'TestVolkswagenPqStockSafety', 'TestVolkswagenPqLongSafety'}):
continue
if {attr, current_test}.issubset({'TestGmCameraSafety', 'TestGmCameraLongitudinalSafety', 'TestGmSdgmSafety'}):
if {attr, current_test}.issubset({'TestGmCameraSafety', 'TestGmCameraLongitudinalSafety', 'TestGmSdgmSafety', 'TestGmInterceptorSafety', 'TestGmCcLongitudinalSafety'}):
continue
if attr.startswith('TestFord') and current_test.startswith('TestFord'):
continue
panda/tests/safety/test_gm.py
+39
View File
@@ -255,5 +255,44 @@ class TestGmSdgmSafety(TestGmSafetyBase):
self._rx(self._pcm_status_msg(enabled))
self.assertEqual(enabled, self._tx(self._button_msg(Buttons.CANCEL)))
class TestGmCcLongitudinalSafety(TestGmCameraSafety):
TX_MSGS = [[384, 0], [481, 0], [388, 2]]
FWD_BLACKLISTED_ADDRS = {2: [384], 0: [388]} # block LKAS message and PSCMStatus
BUTTONS_BUS = 0 # tx only
MAX_GAS = 3400
MAX_REGEN = 1514
INACTIVE_REGEN = 1554
MAX_BRAKE = 400
def setUp(self):
self.packer = CANPackerPanda("gm_global_a_powertrain_generated")
self.packer_chassis = CANPackerPanda("gm_global_a_chassis")
self.safety = libpanda_py.libpanda
self.safety.set_safety_hooks(Panda.SAFETY_GM, Panda.FLAG_GM_HW_CAM | Panda.FLAG_GM_NO_ACC | Panda.FLAG_GM_CC_LONG)
self.safety.init_tests()
def _pcm_status_msg(self, enable):
values = {"CruiseActive": enable}
return self.packer.make_can_msg_panda("ECMCruiseControl", 0, values)
def test_fwd_hook(self):
pass
def test_buttons(self):
self.safety.set_controls_allowed(0)
for btn in range(8):
self.assertFalse(self._tx(self._button_msg(btn)))
self.safety.set_controls_allowed(1)
for btn in range(8):
self.assertFalse(self._tx(self._button_msg(btn)))
for enabled in (True, False):
for btn in (Buttons.RES_ACCEL, Buttons.DECEL_SET, Buttons.CANCEL):
self._rx(self._pcm_status_msg(enabled))
self.assertEqual(enabled, self._tx(self._button_msg(btn)))
if __name__ == "__main__":
unittest.main()
selfdrive/car/gm/carcontroller.py
+9
View File
@@ -11,6 +11,8 @@ from openpilot.selfdrive.car.interfaces import CarControllerBase
VisualAlert = car.CarControl.HUDControl.VisualAlert
NetworkLocation = car.CarParams.NetworkLocation
LongCtrlState = car.CarControl.Actuators.LongControlState
GearShifter = car.CarState.GearShifter
TransmissionType = car.CarParams.TransmissionType
# Camera cancels up to 0.1s after brake is pressed, ECM allows 0.5s
CAMERA_CANCEL_DELAY_FRAMES = 10
@@ -25,10 +27,12 @@ class CarController(CarControllerBase):
self.apply_steer_last = 0
self.apply_gas = 0
self.apply_brake = 0
self.apply_speed = 0
self.frame = 0
self.last_steer_frame = 0
self.last_button_frame = 0
self.cancel_counter = 0
self.pedal_steady = 0.
self.lka_steering_cmd_counter = 0
self.lka_icon_status_last = (False, False)
@@ -108,6 +112,10 @@ class CarController(CarControllerBase):
at_full_stop = CC.longActive and CS.out.standstill
near_stop = CC.longActive and (CS.out.vEgo < self.params.NEAR_STOP_BRAKE_PHASE)
if self.CP.flags & GMFlags.CC_LONG.value:
if CC.longActive and CS.out.vEgo > self.CP.minEnableSpeed:
# Using extend instead of append since the message is only sent intermittently
can_sends.extend(gmcan.create_gm_cc_spam_command(self.packer_pt, self, CS, actuators))
if self.CP.carFingerprint not in CC_ONLY_CAR:
friction_brake_bus = CanBus.CHASSIS
# GM Camera exceptions
@@ -169,6 +177,7 @@ class CarController(CarControllerBase):
new_actuators.steerOutputCan = self.apply_steer_last
new_actuators.gas = self.apply_gas
new_actuators.brake = self.apply_brake
new_actuators.speed = self.apply_speed
self.frame += 1
return new_actuators, can_sends
selfdrive/car/gm/gmcan.py
+52 -1
View File
@@ -1,5 +1,10 @@
import math
from cereal import log
from openpilot.common.conversions import Conversions as CV
from openpilot.common.realtime import DT_CTRL
from openpilot.selfdrive.car import make_can_msg
from openpilot.selfdrive.car.gm.values import CAR
from openpilot.selfdrive.car.gm.values import CAR, CruiseButtons, CanBus
def create_buttons(packer, bus, idx, button):
@@ -171,3 +176,49 @@ def create_lka_icon_command(bus, active, critical, steer):
else:
dat = b"\x00\x00\x00"
return make_can_msg(0x104c006c, dat, bus)
def create_gm_cc_spam_command(packer, controller, CS, actuators):
if controller.params_.get_bool("IsMetric"):
_CV = CV.MS_TO_KPH
RATE_UP_MAX = 0.04
RATE_DOWN_MAX = 0.04
else:
_CV = CV.MS_TO_MPH
RATE_UP_MAX = 0.2
RATE_DOWN_MAX = 0.2
accel = actuators.accel * _CV # m/s/s to mph/s
speedSetPoint = int(round(CS.out.cruiseState.speed * _CV))
cruiseBtn = CruiseButtons.INIT
if speedSetPoint == CS.CP.minEnableSpeed and accel < -1:
cruiseBtn = CruiseButtons.CANCEL
controller.apply_speed = 0
rate = 0.04
elif accel < 0:
cruiseBtn = CruiseButtons.DECEL_SET
if speedSetPoint > (CS.out.vEgo * _CV) + 3.0: # If accel is changing directions, bring set speed to current speed as fast as possible
rate = RATE_DOWN_MAX
else:
rate = max(-1 / accel, RATE_DOWN_MAX)
controller.apply_speed = speedSetPoint - 1
elif accel > 0:
cruiseBtn = CruiseButtons.RES_ACCEL
if speedSetPoint < (CS.out.vEgo * _CV) - 3.0:
rate = RATE_UP_MAX
else:
rate = max(1 / accel, RATE_UP_MAX)
controller.apply_speed = speedSetPoint + 1
else:
controller.apply_speed = speedSetPoint
rate = float('inf')
# Check rlogs closely - our message shouldn't show up on the pt bus for us
# Or bus 2, since we're forwarding... but I think it does
if (cruiseBtn != CruiseButtons.INIT) and ((controller.frame - controller.last_button_frame) * DT_CTRL > rate):
controller.last_button_frame = controller.frame
idx = (CS.buttons_counter + 1) % 4 # Need to predict the next idx for '22-23 EUV
return [create_buttons(packer, CanBus.POWERTRAIN, idx, cruiseBtn)]
else:
return []
selfdrive/car/gm/interface.py
+24 -1
View File
@@ -107,7 +107,7 @@ class CarInterface(CarInterfaceBase):
ret.longitudinalTuning.kiBP = [0.]
if candidate in CAMERA_ACC_CAR:
ret.experimentalLongitudinalAvailable = True
ret.experimentalLongitudinalAvailable = candidate not in CC_ONLY_CAR
ret.networkLocation = NetworkLocation.fwdCamera
ret.radarUnavailable = True # no radar
ret.pcmCruise = True
@@ -235,6 +235,26 @@ class CarInterface(CarInterfaceBase):
elif candidate == CAR.CT6_CC:
CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning)
elif candidate in CC_ONLY_CAR:
ret.flags |= GMFlags.CC_LONG.value
ret.safetyConfigs[0].safetyParam |= Panda.FLAG_GM_CC_LONG
ret.radarUnavailable = True
ret.experimentalLongitudinalAvailable = False
ret.minEnableSpeed = 24 * CV.MPH_TO_MS
ret.openpilotLongitudinalControl = True
ret.pcmCruise = False
ret.longitudinalTuning.deadzoneBP = [0.]
ret.longitudinalTuning.deadzoneV = [0.56] # == 2 km/h/s, 1.25 mph/s
ret.stoppingDecelRate = 11.18 # == 25 mph/s (.04 rate)
ret.longitudinalActuatorDelayLowerBound = 1. # TODO: measure this
ret.longitudinalActuatorDelayUpperBound = 2.
ret.longitudinalTuning.kpBP = [10.7, 10.8, 28.] # 10.7 m/s == 24 mph
ret.longitudinalTuning.kpV = [0., 20., 20.] # set lower end to 0 since we can't drive below that speed
ret.longitudinalTuning.kiBP = [0.]
ret.longitudinalTuning.kiV = [0.1]
if candidate in CC_ONLY_CAR:
ret.safetyConfigs[0].safetyParam |= Panda.FLAG_GM_NO_ACC
@@ -278,6 +298,9 @@ class CarInterface(CarInterfaceBase):
if ret.vEgo < self.CP.minSteerSpeed:
events.add(EventName.belowSteerSpeed)
if (self.CP.flags & GMFlags.CC_LONG.value) and ret.vEgo < self.CP.minEnableSpeed and ret.cruiseState.enabled:
events.add(EventName.speedTooLow)
ret.events = events.to_msg()
return ret
selfdrive/car/gm/values.py
+1
View File
@@ -257,6 +257,7 @@ class CanBus:
DROPPED = 192
class GMFlags(IntFlag):
CC_LONG = 2
NO_ACCELERATOR_POS_MSG = 8
# In a Data Module, an identifier is a string used to recognize an object,
selfdrive/controls/controlsd.py
+3
View File
@@ -604,6 +604,9 @@ class Controls:
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)
if len(long_plan.speeds):
actuators.speed = long_plan.speeds[-1]
# Steering PID loop and lateral MPC
self.desired_curvature = clip_curvature(CS.vEgo, self.desired_curvature, model_v2.action.desiredCurvature)
actuators.curvature = self.desired_curvature