mirror of
https://github.com/MoreTore/openpilot.git
synced 2026-08-04 15:56:08 +08:00
FrogPilot features - Add back comma pedal support
This commit is contained in:
FrogAi
@@ -1,9 +1,16 @@
|
||||
const CanMsg HONDA_N_TX_MSGS[] = {{0xE4, 0, 5}, {0x194, 0, 4}, {0x1FA, 0, 8}, {0x30C, 0, 8}, {0x33D, 0, 5}};
|
||||
const CanMsg HONDA_N_INTERCEPTOR_TX_MSGS[] = {{0xE4, 0, 5}, {0x194, 0, 4}, {0x1FA, 0, 8}, {0x200, 0, 6}, {0x30C, 0, 8}, {0x33D, 0, 5}};
|
||||
const CanMsg HONDA_BOSCH_TX_MSGS[] = {{0xE4, 0, 5}, {0xE5, 0, 8}, {0x296, 1, 4}, {0x33D, 0, 5}, {0x33DA, 0, 5}, {0x33DB, 0, 8}}; // Bosch
|
||||
const CanMsg HONDA_BOSCH_LONG_TX_MSGS[] = {{0xE4, 1, 5}, {0x1DF, 1, 8}, {0x1EF, 1, 8}, {0x1FA, 1, 8}, {0x30C, 1, 8}, {0x33D, 1, 5}, {0x33DA, 1, 5}, {0x33DB, 1, 8}, {0x39F, 1, 8}, {0x18DAB0F1, 1, 8}}; // Bosch w/ gas and brakes
|
||||
const CanMsg HONDA_RADARLESS_TX_MSGS[] = {{0xE4, 0, 5}, {0x296, 2, 4}, {0x33D, 0, 8}}; // Bosch radarless
|
||||
const CanMsg HONDA_RADARLESS_LONG_TX_MSGS[] = {{0xE4, 0, 5}, {0x33D, 0, 8}, {0x1C8, 0, 8}, {0x30C, 0, 8}}; // Bosch radarless w/ gas and brakes
|
||||
|
||||
// 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(((83.3 / 0.253984064) + (83.3 / 0.126992032)) / 2) = 492
|
||||
const int HONDA_GAS_INTERCEPTOR_THRESHOLD = 492;
|
||||
#define HONDA_GET_INTERCEPTOR(msg) (((GET_BYTE((msg), 0) << 8) + GET_BYTE((msg), 1) + (GET_BYTE((msg), 2) << 8) + GET_BYTE((msg), 3)) / 2U) // avg between 2 tracks
|
||||
|
||||
const LongitudinalLimits HONDA_BOSCH_LONG_LIMITS = {
|
||||
.max_accel = 200, // accel is used for brakes
|
||||
.min_accel = -350,
|
||||
@@ -40,6 +47,11 @@ RxCheck honda_common_rx_checks[] = {
|
||||
HONDA_COMMON_RX_CHECKS(0)
|
||||
};
|
||||
|
||||
RxCheck honda_common_interceptor_rx_checks[] = {
|
||||
HONDA_COMMON_RX_CHECKS(0)
|
||||
{.msg = {{0x201, 0, 6, .check_checksum = false, .max_counter = 15U, .frequency = 50U}, { 0 }, { 0 }}},
|
||||
};
|
||||
|
||||
RxCheck honda_common_alt_brake_rx_checks[] = {
|
||||
HONDA_COMMON_RX_CHECKS(0)
|
||||
HONDA_ALT_BRAKE_ADDR_CHECK(0)
|
||||
@@ -50,6 +62,11 @@ RxCheck honda_nidec_alt_rx_checks[] = {
|
||||
HONDA_COMMON_NO_SCM_FEEDBACK_RX_CHECKS(0)
|
||||
};
|
||||
|
||||
RxCheck honda_nidec_alt_interceptor_rx_checks[] = {
|
||||
HONDA_COMMON_NO_SCM_FEEDBACK_RX_CHECKS(0)
|
||||
{.msg = {{0x201, 0, 6, .check_checksum = false, .max_counter = 15U, .frequency = 50U}, { 0 }, { 0 }}},
|
||||
};
|
||||
|
||||
// Bosch has pt on bus 1, verified 0x1A6 does not exist
|
||||
RxCheck honda_bosch_rx_checks[] = {
|
||||
HONDA_COMMON_RX_CHECKS(1)
|
||||
@@ -64,6 +81,7 @@ const uint16_t HONDA_PARAM_ALT_BRAKE = 1;
|
||||
const uint16_t HONDA_PARAM_BOSCH_LONG = 2;
|
||||
const uint16_t HONDA_PARAM_NIDEC_ALT = 4;
|
||||
const uint16_t HONDA_PARAM_RADARLESS = 8;
|
||||
const uint16_t HONDA_PARAM_GAS_INTERCEPTOR = 16;
|
||||
|
||||
enum {
|
||||
HONDA_BTN_NONE = 0,
|
||||
@@ -110,15 +128,26 @@ static uint32_t honda_compute_checksum(const CANPacket_t *to_push) {
|
||||
}
|
||||
|
||||
static uint8_t honda_get_counter(const CANPacket_t *to_push) {
|
||||
int counter_byte = GET_LEN(to_push) - 1U;
|
||||
return (GET_BYTE(to_push, counter_byte) >> 4U) & 0x3U;
|
||||
int addr = GET_ADDR(to_push);
|
||||
|
||||
uint8_t cnt = 0U;
|
||||
if (addr == 0x201) {
|
||||
// Signal: COUNTER_PEDAL
|
||||
cnt = GET_BYTE(to_push, 4) & 0x0FU;
|
||||
} else {
|
||||
int counter_byte = GET_LEN(to_push) - 1U;
|
||||
cnt = (GET_BYTE(to_push, counter_byte) >> 4U) & 0x3U;
|
||||
}
|
||||
return cnt;
|
||||
}
|
||||
|
||||
static void honda_rx_hook(const CANPacket_t *to_push) {
|
||||
const bool pcm_cruise = ((honda_hw == HONDA_BOSCH) && !honda_bosch_long) || (honda_hw == HONDA_NIDEC);
|
||||
const bool pcm_cruise = ((honda_hw == HONDA_BOSCH) && !honda_bosch_long) || \
|
||||
((honda_hw == HONDA_NIDEC) && !enable_gas_interceptor);
|
||||
int pt_bus = honda_get_pt_bus();
|
||||
|
||||
int addr = GET_ADDR(to_push);
|
||||
int len = GET_LEN(to_push);
|
||||
int bus = GET_BUS(to_push);
|
||||
|
||||
// sample speed
|
||||
@@ -190,8 +219,17 @@ static void honda_rx_hook(const CANPacket_t *to_push) {
|
||||
}
|
||||
}
|
||||
|
||||
if (addr == 0x17C) {
|
||||
gas_pressed = GET_BYTE(to_push, 0) != 0U;
|
||||
// length check because bosch hardware also uses this id (0x201 w/ len = 8)
|
||||
if ((addr == 0x201) && (len == 6) && enable_gas_interceptor) {
|
||||
int gas_interceptor = HONDA_GET_INTERCEPTOR(to_push);
|
||||
gas_pressed = gas_interceptor > HONDA_GAS_INTERCEPTOR_THRESHOLD;
|
||||
gas_interceptor_prev = gas_interceptor;
|
||||
}
|
||||
|
||||
if (!enable_gas_interceptor) {
|
||||
if (addr == 0x17C) {
|
||||
gas_pressed = GET_BYTE(to_push, 0) != 0U;
|
||||
}
|
||||
}
|
||||
|
||||
// disable stock Honda AEB in alternative experience
|
||||
@@ -309,6 +347,13 @@ static bool honda_tx_hook(const CANPacket_t *to_send) {
|
||||
}
|
||||
}
|
||||
|
||||
// GAS: safety check (interceptor)
|
||||
if (addr == 0x200) {
|
||||
if (longitudinal_interceptor_checks(to_send)) {
|
||||
tx = false;
|
||||
}
|
||||
}
|
||||
|
||||
// FORCE CANCEL: safety check only relevant when spamming the cancel button in Bosch HW
|
||||
// ensuring that only the cancel button press is sent (VAL 2) when controls are off.
|
||||
// This avoids unintended engagements while still allowing resume spam
|
||||
@@ -336,18 +381,25 @@ static safety_config honda_nidec_init(uint16_t param) {
|
||||
honda_alt_brake_msg = false;
|
||||
honda_bosch_long = false;
|
||||
honda_bosch_radarless = false;
|
||||
enable_gas_interceptor = GET_FLAG(param, HONDA_PARAM_GAS_INTERCEPTOR);
|
||||
|
||||
safety_config ret;
|
||||
|
||||
bool enable_nidec_alt = GET_FLAG(param, HONDA_PARAM_NIDEC_ALT);
|
||||
|
||||
if (enable_nidec_alt) {
|
||||
SET_RX_CHECKS(honda_nidec_alt_rx_checks, ret);
|
||||
enable_gas_interceptor ? SET_RX_CHECKS(honda_nidec_alt_interceptor_rx_checks, ret) : \
|
||||
SET_RX_CHECKS(honda_nidec_alt_rx_checks, ret);
|
||||
} else {
|
||||
SET_RX_CHECKS(honda_common_rx_checks, ret);
|
||||
enable_gas_interceptor ? SET_RX_CHECKS(honda_common_interceptor_rx_checks, ret) : \
|
||||
SET_RX_CHECKS(honda_common_rx_checks, ret);
|
||||
}
|
||||
SET_TX_MSGS(HONDA_N_TX_MSGS, ret);
|
||||
|
||||
if (enable_gas_interceptor) {
|
||||
SET_TX_MSGS(HONDA_N_INTERCEPTOR_TX_MSGS, ret);
|
||||
} else {
|
||||
SET_TX_MSGS(HONDA_N_TX_MSGS, ret);
|
||||
}
|
||||
return ret;
|
||||
}
|
||||
|
||||
|
||||
@@ -37,6 +37,12 @@ const LongitudinalLimits TOYOTA_LONG_LIMITS = {
|
||||
.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
|
||||
const int TOYOTA_GAS_INTERCEPTOR_THRSLD = 805;
|
||||
#define TOYOTA_GET_INTERCEPTOR(msg) (((GET_BYTE((msg), 0) << 8) + GET_BYTE((msg), 1) + (GET_BYTE((msg), 2) << 8) + GET_BYTE((msg), 3)) / 2U) // avg between 2 tracks
|
||||
|
||||
// Stock longitudinal
|
||||
#define TOYOTA_COMMON_TX_MSGS \
|
||||
{0x2E4, 0, 5}, {0x191, 0, 8}, {0x412, 0, 8}, {0x343, 0, 8}, {0x1D2, 0, 8}, /* LKAS + LTA + ACC & PCM cancel cmds */ \
|
||||
@@ -56,6 +62,11 @@ const CanMsg TOYOTA_LONG_TX_MSGS[] = {
|
||||
TOYOTA_COMMON_LONG_TX_MSGS
|
||||
};
|
||||
|
||||
const CanMsg TOYOTA_INTERCEPTOR_TX_MSGS[] = {
|
||||
TOYOTA_COMMON_LONG_TX_MSGS
|
||||
{0x200, 0, 6}, // gas interceptor
|
||||
};
|
||||
|
||||
#define TOYOTA_COMMON_RX_CHECKS(lta) \
|
||||
{.msg = {{ 0xaa, 0, 8, .check_checksum = false, .frequency = 83U}, { 0 }, { 0 }}}, \
|
||||
{.msg = {{0x260, 0, 8, .check_checksum = true, .quality_flag = (lta), .frequency = 50U}, { 0 }, { 0 }}}, \
|
||||
@@ -67,11 +78,21 @@ RxCheck toyota_lka_rx_checks[] = {
|
||||
TOYOTA_COMMON_RX_CHECKS(false)
|
||||
};
|
||||
|
||||
RxCheck toyota_lka_interceptor_rx_checks[] = {
|
||||
TOYOTA_COMMON_RX_CHECKS(false)
|
||||
{.msg = {{0x201, 0, 6, .check_checksum = false, .max_counter = 15U, .frequency = 50U}, { 0 }, { 0 }}},
|
||||
};
|
||||
|
||||
// Check the quality flag for angle measurement when using LTA, since it's not set on TSS-P cars
|
||||
RxCheck toyota_lta_rx_checks[] = {
|
||||
TOYOTA_COMMON_RX_CHECKS(true)
|
||||
};
|
||||
|
||||
RxCheck toyota_lta_interceptor_rx_checks[] = {
|
||||
TOYOTA_COMMON_RX_CHECKS(true)
|
||||
{.msg = {{0x201, 0, 6, .check_checksum = false, .max_counter = 15U, .frequency = 50U}, { 0 }, { 0 }}},
|
||||
};
|
||||
|
||||
// safety param flags
|
||||
// first byte is for EPS factor, second is for flags
|
||||
const uint32_t TOYOTA_PARAM_OFFSET = 8U;
|
||||
@@ -79,6 +100,7 @@ const uint32_t TOYOTA_EPS_FACTOR = (1UL << TOYOTA_PARAM_OFFSET) - 1U;
|
||||
const uint32_t TOYOTA_PARAM_ALT_BRAKE = 1UL << TOYOTA_PARAM_OFFSET;
|
||||
const uint32_t TOYOTA_PARAM_STOCK_LONGITUDINAL = 2UL << TOYOTA_PARAM_OFFSET;
|
||||
const uint32_t TOYOTA_PARAM_LTA = 4UL << TOYOTA_PARAM_OFFSET;
|
||||
const uint32_t TOYOTA_PARAM_GAS_INTERCEPTOR = 8UL << TOYOTA_PARAM_OFFSET;
|
||||
|
||||
bool toyota_alt_brake = false;
|
||||
bool toyota_stock_longitudinal = false;
|
||||
@@ -100,6 +122,17 @@ static uint32_t toyota_get_checksum(const CANPacket_t *to_push) {
|
||||
return (uint8_t)(GET_BYTE(to_push, checksum_byte));
|
||||
}
|
||||
|
||||
static uint8_t toyota_get_counter(const CANPacket_t *to_push) {
|
||||
int addr = GET_ADDR(to_push);
|
||||
|
||||
uint8_t cnt = 0U;
|
||||
if (addr == 0x201) {
|
||||
// Signal: COUNTER_PEDAL
|
||||
cnt = GET_BYTE(to_push, 4) & 0x0FU;
|
||||
}
|
||||
return cnt;
|
||||
}
|
||||
|
||||
static bool toyota_get_quality_flag_valid(const CANPacket_t *to_push) {
|
||||
int addr = GET_ADDR(to_push);
|
||||
|
||||
@@ -152,7 +185,9 @@ static void toyota_rx_hook(const CANPacket_t *to_push) {
|
||||
pcm_cruise_check(cruise_engaged);
|
||||
|
||||
// sample gas pedal
|
||||
gas_pressed = !GET_BIT(to_push, 4U);
|
||||
if (!enable_gas_interceptor) {
|
||||
gas_pressed = !GET_BIT(to_push, 4U);
|
||||
}
|
||||
}
|
||||
|
||||
// sample speed
|
||||
@@ -175,6 +210,15 @@ static void toyota_rx_hook(const CANPacket_t *to_push) {
|
||||
brake_pressed = GET_BIT(to_push, bit);
|
||||
}
|
||||
|
||||
// sample gas interceptor
|
||||
if ((addr == 0x201) && enable_gas_interceptor) {
|
||||
int gas_interceptor = TOYOTA_GET_INTERCEPTOR(to_push);
|
||||
gas_pressed = gas_interceptor > TOYOTA_GAS_INTERCEPTOR_THRSLD;
|
||||
|
||||
// TODO: remove this, only left in for gas_interceptor_prev test
|
||||
gas_interceptor_prev = gas_interceptor;
|
||||
}
|
||||
|
||||
bool stock_ecu_detected = addr == 0x2E4; // STEERING_LKA
|
||||
if (!toyota_stock_longitudinal && (addr == 0x343)) {
|
||||
stock_ecu_detected = true; // ACC_CONTROL
|
||||
@@ -190,6 +234,14 @@ static bool toyota_tx_hook(const CANPacket_t *to_send) {
|
||||
|
||||
// Check if msg is sent on BUS 0
|
||||
if (bus == 0) {
|
||||
|
||||
// GAS PEDAL: safety check
|
||||
if (addr == 0x200) {
|
||||
if (longitudinal_interceptor_checks(to_send)) {
|
||||
tx = false;
|
||||
}
|
||||
}
|
||||
|
||||
// ACCEL: safety check on byte 1-2
|
||||
if (addr == 0x343) {
|
||||
int desired_accel = (GET_BYTE(to_send, 0) << 8) | GET_BYTE(to_send, 1);
|
||||
@@ -305,17 +357,29 @@ static safety_config toyota_init(uint16_t param) {
|
||||
toyota_alt_brake = GET_FLAG(param, TOYOTA_PARAM_ALT_BRAKE);
|
||||
toyota_stock_longitudinal = GET_FLAG(param, TOYOTA_PARAM_STOCK_LONGITUDINAL);
|
||||
toyota_lta = GET_FLAG(param, TOYOTA_PARAM_LTA);
|
||||
enable_gas_interceptor = GET_FLAG(param, TOYOTA_PARAM_GAS_INTERCEPTOR);
|
||||
toyota_dbc_eps_torque_factor = param & TOYOTA_EPS_FACTOR;
|
||||
|
||||
// Gas interceptor should not be used if openpilot is not controlling longitudinal
|
||||
if (toyota_stock_longitudinal) {
|
||||
enable_gas_interceptor = false;
|
||||
}
|
||||
|
||||
safety_config ret;
|
||||
if (toyota_stock_longitudinal) {
|
||||
SET_TX_MSGS(TOYOTA_TX_MSGS, ret);
|
||||
} else {
|
||||
SET_TX_MSGS(TOYOTA_LONG_TX_MSGS, ret);
|
||||
enable_gas_interceptor ? SET_TX_MSGS(TOYOTA_INTERCEPTOR_TX_MSGS, ret) : \
|
||||
SET_TX_MSGS(TOYOTA_LONG_TX_MSGS, ret);
|
||||
}
|
||||
|
||||
toyota_lta ? SET_RX_CHECKS(toyota_lta_rx_checks, ret) : \
|
||||
SET_RX_CHECKS(toyota_lka_rx_checks, ret);
|
||||
if (enable_gas_interceptor) {
|
||||
toyota_lta ? SET_RX_CHECKS(toyota_lta_interceptor_rx_checks, ret) : \
|
||||
SET_RX_CHECKS(toyota_lka_interceptor_rx_checks, ret);
|
||||
} else {
|
||||
toyota_lta ? SET_RX_CHECKS(toyota_lta_rx_checks, ret) : \
|
||||
SET_RX_CHECKS(toyota_lka_rx_checks, ret);
|
||||
}
|
||||
return ret;
|
||||
}
|
||||
|
||||
@@ -349,5 +413,6 @@ const safety_hooks toyota_hooks = {
|
||||
.fwd = toyota_fwd_hook,
|
||||
.get_checksum = toyota_get_checksum,
|
||||
.compute_checksum = toyota_compute_checksum,
|
||||
.get_counter = toyota_get_counter,
|
||||
.get_quality_flag_valid = toyota_get_quality_flag_valid,
|
||||
};
|
||||
|
||||
@@ -191,11 +191,13 @@ class Panda:
|
||||
FLAG_TOYOTA_ALT_BRAKE = (1 << 8)
|
||||
FLAG_TOYOTA_STOCK_LONGITUDINAL = (2 << 8)
|
||||
FLAG_TOYOTA_LTA = (4 << 8)
|
||||
FLAG_TOYOTA_GAS_INTERCEPTOR = (8 << 8)
|
||||
|
||||
FLAG_HONDA_ALT_BRAKE = 1
|
||||
FLAG_HONDA_BOSCH_LONG = 2
|
||||
FLAG_HONDA_NIDEC_ALT = 4
|
||||
FLAG_HONDA_RADARLESS = 8
|
||||
FLAG_HONDA_GAS_INTERCEPTOR = 16
|
||||
|
||||
FLAG_HYUNDAI_EV_GAS = 1
|
||||
FLAG_HYUNDAI_HYBRID_GAS = 2
|
||||
|
||||
@@ -350,6 +350,21 @@ class TestHondaNidecPcmSafety(HondaPcmEnableBase, TestHondaNidecSafetyBase):
|
||||
pass
|
||||
|
||||
|
||||
class TestHondaNidecGasInterceptorSafety(common.GasInterceptorSafetyTest, HondaButtonEnableBase, TestHondaNidecSafetyBase):
|
||||
"""
|
||||
Covers the Honda Nidec safety mode with a gas interceptor, switches to a button-enable car
|
||||
"""
|
||||
|
||||
TX_MSGS = HONDA_N_COMMON_TX_MSGS + [[0x200, 0]]
|
||||
INTERCEPTOR_THRESHOLD = 492
|
||||
|
||||
def setUp(self):
|
||||
self.packer = CANPackerPanda("honda_civic_touring_2016_can_generated")
|
||||
self.safety = libpanda_py.libpanda
|
||||
self.safety.set_safety_hooks(Panda.SAFETY_HONDA_NIDEC, Panda.FLAG_HONDA_GAS_INTERCEPTOR)
|
||||
self.safety.init_tests()
|
||||
|
||||
|
||||
class TestHondaNidecPcmAltSafety(TestHondaNidecPcmSafety):
|
||||
"""
|
||||
Covers the Honda Nidec safety mode with alt SCM messages
|
||||
@@ -372,6 +387,33 @@ class TestHondaNidecPcmAltSafety(TestHondaNidecPcmSafety):
|
||||
return self.packer.make_can_msg_panda("SCM_BUTTONS", bus, values)
|
||||
|
||||
|
||||
class TestHondaNidecAltGasInterceptorSafety(common.GasInterceptorSafetyTest, HondaButtonEnableBase, TestHondaNidecSafetyBase):
|
||||
"""
|
||||
Covers the Honda Nidec safety mode with alt SCM messages and gas interceptor, switches to a button-enable car
|
||||
"""
|
||||
|
||||
TX_MSGS = HONDA_N_COMMON_TX_MSGS + [[0x200, 0]]
|
||||
INTERCEPTOR_THRESHOLD = 492
|
||||
|
||||
def setUp(self):
|
||||
self.packer = CANPackerPanda("acura_ilx_2016_can_generated")
|
||||
self.safety = libpanda_py.libpanda
|
||||
self.safety.set_safety_hooks(Panda.SAFETY_HONDA_NIDEC, Panda.FLAG_HONDA_NIDEC_ALT | Panda.FLAG_HONDA_GAS_INTERCEPTOR)
|
||||
self.safety.init_tests()
|
||||
|
||||
def _acc_state_msg(self, main_on):
|
||||
values = {"MAIN_ON": main_on, "COUNTER": self.cnt_acc_state % 4}
|
||||
self.__class__.cnt_acc_state += 1
|
||||
return self.packer.make_can_msg_panda("SCM_BUTTONS", self.PT_BUS, values)
|
||||
|
||||
def _button_msg(self, buttons, main_on=False, bus=None):
|
||||
bus = self.PT_BUS if bus is None else bus
|
||||
values = {"CRUISE_BUTTONS": buttons, "MAIN_ON": main_on, "COUNTER": self.cnt_button % 4}
|
||||
self.__class__.cnt_button += 1
|
||||
return self.packer.make_can_msg_panda("SCM_BUTTONS", bus, values)
|
||||
|
||||
|
||||
|
||||
# ********************* Honda Bosch **********************
|
||||
|
||||
|
||||
|
||||
@@ -14,6 +14,7 @@ TOYOTA_COMMON_LONG_TX_MSGS = [[0x283, 0], [0x2E6, 0], [0x2E7, 0], [0x33E, 0], [0
|
||||
[0x128, 1], [0x141, 1], [0x160, 1], [0x161, 1], [0x470, 1], # DSU bus 1
|
||||
[0x411, 0], # PCS_HUD
|
||||
[0x750, 0]] # radar diagnostic address
|
||||
GAS_INTERCEPTOR_TX_MSGS = [[0x200, 0]]
|
||||
|
||||
|
||||
class TestToyotaSafetyBase(common.PandaCarSafetyTest, common.LongitudinalAccelSafetyTest):
|
||||
@@ -126,6 +127,32 @@ class TestToyotaSafetyBase(common.PandaCarSafetyTest, common.LongitudinalAccelSa
|
||||
self.assertFalse(self.safety.get_controls_allowed())
|
||||
|
||||
|
||||
class TestToyotaSafetyGasInterceptorBase(common.GasInterceptorSafetyTest, TestToyotaSafetyBase):
|
||||
|
||||
TX_MSGS = TOYOTA_COMMON_TX_MSGS + TOYOTA_COMMON_LONG_TX_MSGS + GAS_INTERCEPTOR_TX_MSGS
|
||||
INTERCEPTOR_THRESHOLD = 805
|
||||
|
||||
def setUp(self):
|
||||
super().setUp()
|
||||
self.safety.set_safety_hooks(Panda.SAFETY_TOYOTA, self.safety.get_current_safety_param() |
|
||||
Panda.FLAG_TOYOTA_GAS_INTERCEPTOR)
|
||||
self.safety.init_tests()
|
||||
|
||||
def test_stock_longitudinal(self):
|
||||
# If stock longitudinal is set, the gas interceptor safety param should not be respected
|
||||
self.safety.set_safety_hooks(Panda.SAFETY_TOYOTA, self.safety.get_current_safety_param() |
|
||||
Panda.FLAG_TOYOTA_STOCK_LONGITUDINAL)
|
||||
self.safety.init_tests()
|
||||
|
||||
# Spot check a few gas interceptor tests: (1) reading interceptor,
|
||||
# (2) behavior around interceptor, and (3) txing interceptor msgs
|
||||
for test in (self.test_prev_gas_interceptor, self.test_disengage_on_gas_interceptor,
|
||||
self.test_gas_interceptor_safety_check):
|
||||
with self.subTest(test=test.__name__):
|
||||
with self.assertRaises(AssertionError):
|
||||
test()
|
||||
|
||||
|
||||
class TestToyotaSafetyTorque(TestToyotaSafetyBase, common.MotorTorqueSteeringSafetyTest, common.SteerRequestCutSafetyTest):
|
||||
|
||||
MAX_RATE_UP = 15
|
||||
@@ -148,6 +175,10 @@ class TestToyotaSafetyTorque(TestToyotaSafetyBase, common.MotorTorqueSteeringSaf
|
||||
self.safety.init_tests()
|
||||
|
||||
|
||||
class TestToyotaSafetyTorqueGasInterceptor(TestToyotaSafetyGasInterceptorBase, TestToyotaSafetyTorque):
|
||||
pass
|
||||
|
||||
|
||||
class TestToyotaSafetyAngle(TestToyotaSafetyBase, common.AngleSteeringSafetyTest):
|
||||
|
||||
# Angle control limits
|
||||
@@ -261,6 +292,10 @@ class TestToyotaSafetyAngle(TestToyotaSafetyBase, common.AngleSteeringSafetyTest
|
||||
self.assertEqual(self.safety.get_angle_meas_max(), 0)
|
||||
|
||||
|
||||
class TestToyotaSafetyAngleGasInterceptor(TestToyotaSafetyGasInterceptorBase, TestToyotaSafetyAngle):
|
||||
pass
|
||||
|
||||
|
||||
class TestToyotaAltBrakeSafety(TestToyotaSafetyTorque):
|
||||
|
||||
def setUp(self):
|
||||
@@ -278,6 +313,10 @@ class TestToyotaAltBrakeSafety(TestToyotaSafetyTorque):
|
||||
pass
|
||||
|
||||
|
||||
class TestToyotaAltBrakeSafetyGasInterceptor(TestToyotaSafetyGasInterceptorBase, TestToyotaAltBrakeSafety):
|
||||
pass
|
||||
|
||||
|
||||
class TestToyotaStockLongitudinalBase(TestToyotaSafetyBase):
|
||||
|
||||
TX_MSGS = TOYOTA_COMMON_TX_MSGS
|
||||
|
||||
@@ -4,6 +4,7 @@ from cereal import car
|
||||
from openpilot.common.numpy_fast import clip, interp
|
||||
from openpilot.common.realtime import DT_CTRL
|
||||
from opendbc.can.packer import CANPacker
|
||||
from openpilot.selfdrive.car import create_gas_interceptor_command
|
||||
from openpilot.selfdrive.car.honda import hondacan
|
||||
from openpilot.selfdrive.car.honda.values import CruiseButtons, VISUAL_HUD, HONDA_BOSCH, HONDA_BOSCH_RADARLESS, HONDA_NIDEC_ALT_PCM_ACCEL, CarControllerParams
|
||||
from openpilot.selfdrive.car.interfaces import CarControllerBase
|
||||
@@ -182,7 +183,7 @@ class CarController(CarControllerBase):
|
||||
0.5]
|
||||
# The Honda ODYSSEY seems to have different PCM_ACCEL
|
||||
# msgs, is it other cars too?
|
||||
if not CC.longActive:
|
||||
if self.CP.enableGasInterceptor or not CC.longActive:
|
||||
pcm_speed = 0.0
|
||||
pcm_accel = int(0.0)
|
||||
elif self.CP.carFingerprint in HONDA_NIDEC_ALT_PCM_ACCEL:
|
||||
@@ -234,6 +235,19 @@ class CarController(CarControllerBase):
|
||||
self.apply_brake_last = apply_brake
|
||||
self.brake = apply_brake / self.params.NIDEC_BRAKE_MAX
|
||||
|
||||
if self.CP.enableGasInterceptor:
|
||||
# way too aggressive at low speed without this
|
||||
gas_mult = interp(CS.out.vEgo, [0., 10.], [0.4, 1.0])
|
||||
# send exactly zero if apply_gas is zero. Interceptor will send the max between read value and apply_gas.
|
||||
# This prevents unexpected pedal range rescaling
|
||||
# Sending non-zero gas when OP is not enabled will cause the PCM not to respond to throttle as expected
|
||||
# when you do enable.
|
||||
if CC.longActive:
|
||||
self.gas = clip(gas_mult * (gas - brake + wind_brake * 3 / 4), 0., 1.)
|
||||
else:
|
||||
self.gas = 0.0
|
||||
can_sends.append(create_gas_interceptor_command(self.packer, self.gas, self.frame // 2))
|
||||
|
||||
# Send dashboard UI commands.
|
||||
if self.frame % 10 == 0:
|
||||
hud = HUDData(int(pcm_accel), int(round(hud_v_cruise)), hud_control.leadVisible,
|
||||
@@ -242,7 +256,8 @@ class CarController(CarControllerBase):
|
||||
|
||||
if self.CP.openpilotLongitudinalControl and self.CP.carFingerprint not in HONDA_BOSCH:
|
||||
self.speed = pcm_speed
|
||||
self.gas = pcm_accel / self.params.NIDEC_GAS_MAX
|
||||
if not self.CP.enableGasInterceptor:
|
||||
self.gas = pcm_accel / self.params.NIDEC_GAS_MAX
|
||||
|
||||
new_actuators = actuators.as_builder()
|
||||
new_actuators.speed = self.speed
|
||||
|
||||
@@ -72,6 +72,10 @@ def get_can_messages(CP, gearbox_msg):
|
||||
else:
|
||||
messages.append(("DOORS_STATUS", 3))
|
||||
|
||||
# add gas interceptor reading if we are using it
|
||||
if CP.enableGasInterceptor:
|
||||
messages.append(("GAS_SENSOR", 50))
|
||||
|
||||
if CP.carFingerprint in HONDA_BOSCH_RADARLESS:
|
||||
messages.append(("CRUISE_FAULT_STATUS", 50))
|
||||
elif CP.openpilotLongitudinalControl:
|
||||
@@ -188,8 +192,13 @@ class CarState(CarStateBase):
|
||||
gear = int(cp.vl[self.gearbox_msg]["GEAR_SHIFTER"])
|
||||
ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(gear, None))
|
||||
|
||||
ret.gas = cp.vl["POWERTRAIN_DATA"]["PEDAL_GAS"]
|
||||
ret.gasPressed = ret.gas > 1e-5
|
||||
if self.CP.enableGasInterceptor:
|
||||
# Same threshold as panda, equivalent to 1e-5 with previous DBC scaling
|
||||
ret.gas = (cp.vl["GAS_SENSOR"]["INTERCEPTOR_GAS"] + cp.vl["GAS_SENSOR"]["INTERCEPTOR_GAS2"]) // 2
|
||||
ret.gasPressed = ret.gas > 492
|
||||
else:
|
||||
ret.gas = cp.vl["POWERTRAIN_DATA"]["PEDAL_GAS"]
|
||||
ret.gasPressed = ret.gas > 1e-5
|
||||
|
||||
ret.steeringTorque = cp.vl["STEER_STATUS"]["STEER_TORQUE_SENSOR"]
|
||||
ret.steeringTorqueEps = cp.vl["STEER_MOTOR_TORQUE"]["MOTOR_TORQUE"]
|
||||
|
||||
@@ -24,6 +24,8 @@ class CarInterface(CarInterfaceBase):
|
||||
def get_pid_accel_limits(CP, current_speed, cruise_speed):
|
||||
if CP.carFingerprint in HONDA_BOSCH:
|
||||
return CarControllerParams.BOSCH_ACCEL_MIN, CarControllerParams.BOSCH_ACCEL_MAX
|
||||
elif CP.enableGasInterceptor:
|
||||
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
|
||||
@@ -48,9 +50,10 @@ class CarInterface(CarInterfaceBase):
|
||||
ret.pcmCruise = not ret.openpilotLongitudinalControl
|
||||
else:
|
||||
ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.hondaNidec)]
|
||||
ret.enableGasInterceptor = 0x201 in fingerprint[CAN.pt]
|
||||
ret.openpilotLongitudinalControl = True
|
||||
|
||||
ret.pcmCruise = True
|
||||
ret.pcmCruise = not ret.enableGasInterceptor
|
||||
|
||||
if candidate == CAR.HONDA_CRV_5G:
|
||||
ret.enableBsm = 0x12f8bfa7 in fingerprint[CAN.radar]
|
||||
@@ -202,13 +205,16 @@ class CarInterface(CarInterfaceBase):
|
||||
if ret.openpilotLongitudinalControl and candidate in HONDA_BOSCH:
|
||||
ret.safetyConfigs[0].safetyParam |= Panda.FLAG_HONDA_BOSCH_LONG
|
||||
|
||||
if ret.enableGasInterceptor and candidate not in HONDA_BOSCH:
|
||||
ret.safetyConfigs[0].safetyParam |= Panda.FLAG_HONDA_GAS_INTERCEPTOR
|
||||
|
||||
if candidate in HONDA_BOSCH_RADARLESS:
|
||||
ret.safetyConfigs[0].safetyParam |= Panda.FLAG_HONDA_RADARLESS
|
||||
|
||||
# min speed to enable ACC. if car can do stop and go, then set enabling speed
|
||||
# to a negative value, so it won't matter. Otherwise, add 0.5 mph margin to not
|
||||
# conflict with PCM acc
|
||||
ret.autoResumeSng = candidate in (HONDA_BOSCH | {CAR.HONDA_CIVIC})
|
||||
ret.autoResumeSng = candidate in (HONDA_BOSCH | {CAR.HONDA_CIVIC}) or ret.enableGasInterceptor
|
||||
ret.minEnableSpeed = -1. if ret.autoResumeSng else 25.5 * CV.MPH_TO_MS
|
||||
|
||||
ret.steerActuatorDelay = 0.1
|
||||
|
||||
@@ -295,6 +295,8 @@ routes = [
|
||||
CarTestRoute("66c1699b7697267d/2024-03-03--13-09-53", TESLA.TESLA_MODELS_RAVEN),
|
||||
|
||||
# Segments that test specific issues
|
||||
# Controls mismatch due to interceptor threshold
|
||||
CarTestRoute("cfb32f0fb91b173b|2022-04-06--14-54-45", HONDA.CIVIC, segment=21),
|
||||
# Controls mismatch due to standstill threshold
|
||||
CarTestRoute("bec2dcfde6a64235|2022-04-08--14-21-32", HONDA.HONDA_CRV_HYBRID, segment=22),
|
||||
]
|
||||
|
||||
@@ -76,6 +76,10 @@ class TestCarInterfaces:
|
||||
assert len(car_params.longitudinalTuning.kpV) == len(car_params.longitudinalTuning.kpBP)
|
||||
assert len(car_params.longitudinalTuning.kiV) == len(car_params.longitudinalTuning.kiBP)
|
||||
|
||||
# If we're using the interceptor for gasPressed, we should be commanding gas with it
|
||||
if car_params.enableGasInterceptor:
|
||||
self.assertTrue(car_params.openpilotLongitudinalControl)
|
||||
|
||||
# Lateral sanity checks
|
||||
if car_params.steerControlType != car.CarParams.SteerControlType.angle:
|
||||
tune = car_params.lateralTuning
|
||||
|
||||
@@ -1,11 +1,11 @@
|
||||
from cereal import car
|
||||
from openpilot.common.numpy_fast import clip
|
||||
from openpilot.common.numpy_fast import clip, interp
|
||||
from openpilot.common.params import Params
|
||||
from openpilot.selfdrive.car import apply_meas_steer_torque_limits, apply_std_steer_angle_limits, common_fault_avoidance, make_can_msg
|
||||
from openpilot.selfdrive.car import apply_meas_steer_torque_limits, apply_std_steer_angle_limits, common_fault_avoidance, create_gas_interceptor_command, make_can_msg
|
||||
from openpilot.selfdrive.car.interfaces import CarControllerBase
|
||||
from openpilot.selfdrive.car.toyota import toyotacan
|
||||
from openpilot.selfdrive.car.toyota.values import CAR, STATIC_DSU_MSGS, NO_STOP_TIMER_CAR, TSS2_CAR, \
|
||||
CarControllerParams, ToyotaFlags, \
|
||||
MIN_ACC_SPEED, PEDAL_TRANSITION, CarControllerParams, ToyotaFlags, \
|
||||
UNSUPPORTED_DSU_CAR
|
||||
from opendbc.can.packer import CANPacker
|
||||
|
||||
@@ -104,10 +104,25 @@ class CarController(CarControllerBase):
|
||||
lta_active, self.frame // 2, torque_wind_down))
|
||||
|
||||
# *** gas and brake ***
|
||||
if self.CP.enableGasInterceptor and CC.longActive:
|
||||
MAX_INTERCEPTOR_GAS = 0.5
|
||||
# RAV4 has very sensitive gas pedal
|
||||
if self.CP.carFingerprint in (CAR.TOYOTA_RAV4, CAR.TOYOTA_RAV4H, CAR.TOYOTA_HIGHLANDER):
|
||||
PEDAL_SCALE = interp(CS.out.vEgo, [0.0, MIN_ACC_SPEED, MIN_ACC_SPEED + PEDAL_TRANSITION], [0.15, 0.3, 0.0])
|
||||
elif self.CP.carFingerprint in (CAR.TOYOTA_COROLLA,):
|
||||
PEDAL_SCALE = interp(CS.out.vEgo, [0.0, MIN_ACC_SPEED, MIN_ACC_SPEED + PEDAL_TRANSITION], [0.3, 0.4, 0.0])
|
||||
else:
|
||||
PEDAL_SCALE = interp(CS.out.vEgo, [0.0, MIN_ACC_SPEED, MIN_ACC_SPEED + PEDAL_TRANSITION], [0.4, 0.5, 0.0])
|
||||
# offset for creep and windbrake
|
||||
pedal_offset = interp(CS.out.vEgo, [0.0, 2.3, MIN_ACC_SPEED + PEDAL_TRANSITION], [-.4, 0.0, 0.2])
|
||||
pedal_command = PEDAL_SCALE * (actuators.accel + pedal_offset)
|
||||
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)
|
||||
|
||||
# on entering standstill, send standstill request
|
||||
if CS.out.standstill and not self.last_standstill and (self.CP.carFingerprint not in NO_STOP_TIMER_CAR):
|
||||
if CS.out.standstill and not self.last_standstill and (self.CP.carFingerprint not in NO_STOP_TIMER_CAR or self.CP.enableGasInterceptor):
|
||||
self.standstill_req = True
|
||||
if CS.pcm_acc_status != 8:
|
||||
# pcm entered standstill or it's disabled
|
||||
@@ -141,6 +156,12 @@ class CarController(CarControllerBase):
|
||||
else:
|
||||
can_sends.append(toyotacan.create_accel_command(self.packer, 0, pcm_cancel_cmd, False, lead, CS.acc_type, False, self.distance_button, frogpilot_toggles))
|
||||
|
||||
if self.frame % 2 == 0 and self.CP.enableGasInterceptor and self.CP.openpilotLongitudinalControl:
|
||||
# send exactly zero if gas cmd is zero. Interceptor will send the max between read value and gas cmd.
|
||||
# This prevents unexpected pedal range rescaling
|
||||
can_sends.append(create_gas_interceptor_command(self.packer, interceptor_gas_cmd, self.frame // 2))
|
||||
self.gas = interceptor_gas_cmd
|
||||
|
||||
# *** hud ui ***
|
||||
if self.CP.carFingerprint != CAR.TOYOTA_PRIUS_V:
|
||||
# ui mesg is at 1Hz but we send asap if:
|
||||
|
||||
@@ -61,7 +61,12 @@ class CarState(CarStateBase):
|
||||
ret.brakePressed = cp.vl["BRAKE_MODULE"]["BRAKE_PRESSED"] != 0
|
||||
ret.brakeHoldActive = cp.vl["ESP_CONTROL"]["BRAKE_HOLD_ACTIVE"] == 1
|
||||
|
||||
ret.gasPressed = cp.vl["PCM_CRUISE"]["GAS_RELEASED"] == 0
|
||||
if self.CP.enableGasInterceptor:
|
||||
ret.gas = (cp.vl["GAS_SENSOR"]["INTERCEPTOR_GAS"] + cp.vl["GAS_SENSOR"]["INTERCEPTOR_GAS2"]) // 2
|
||||
ret.gasPressed = ret.gas > 805
|
||||
else:
|
||||
# TODO: find a common gas pedal percentage signal
|
||||
ret.gasPressed = cp.vl["PCM_CRUISE"]["GAS_RELEASED"] == 0
|
||||
|
||||
ret.wheelSpeeds = self.get_wheel_speeds(
|
||||
cp.vl["WHEEL_SPEEDS"]["WHEEL_SPEED_FL"],
|
||||
@@ -205,6 +210,10 @@ class CarState(CarStateBase):
|
||||
else:
|
||||
messages.append(("PCM_CRUISE_2", 33))
|
||||
|
||||
# add gas interceptor reading if we are using it
|
||||
if CP.enableGasInterceptor:
|
||||
messages.append(("GAS_SENSOR", 50))
|
||||
|
||||
if CP.enableBsm:
|
||||
messages.append(("BSM", 1))
|
||||
|
||||
|
||||
@@ -133,21 +133,26 @@ class CarInterface(CarInterfaceBase):
|
||||
# - TSS-P DSU-less cars w/ CAN filter installed (no radar parser yet)
|
||||
ret.openpilotLongitudinalControl = use_sdsu or ret.enableDsu or candidate in (TSS2_CAR - RADAR_ACC_CAR) or bool(ret.flags & ToyotaFlags.DISABLE_RADAR.value)
|
||||
ret.autoResumeSng = ret.openpilotLongitudinalControl and candidate in NO_STOP_TIMER_CAR
|
||||
ret.enableGasInterceptor = 0x201 in fingerprint[0] and ret.openpilotLongitudinalControl
|
||||
|
||||
if not ret.openpilotLongitudinalControl:
|
||||
ret.safetyConfigs[0].safetyParam |= Panda.FLAG_TOYOTA_STOCK_LONGITUDINAL
|
||||
|
||||
if ret.enableGasInterceptor:
|
||||
ret.safetyConfigs[0].safetyParam |= Panda.FLAG_TOYOTA_GAS_INTERCEPTOR
|
||||
|
||||
# min speed to enable ACC. if car can do stop and go, then set enabling speed
|
||||
# to a negative value, so it won't matter.
|
||||
ret.minEnableSpeed = -1. if stop_and_go else MIN_ACC_SPEED
|
||||
ret.minEnableSpeed = -1. if (stop_and_go or ret.enableGasInterceptor) else MIN_ACC_SPEED
|
||||
|
||||
tune = ret.longitudinalTuning
|
||||
if candidate in TSS2_CAR:
|
||||
if candidate in TSS2_CAR or ret.enableGasInterceptor:
|
||||
tune.kpV = [0.0]
|
||||
tune.kiV = [0.5]
|
||||
ret.vEgoStopping = 0.25
|
||||
ret.vEgoStarting = 0.25
|
||||
ret.stoppingDecelRate = 0.3 # reach stopping target smoothly
|
||||
if candidate in TSS2_CAR:
|
||||
ret.vEgoStopping = 0.25
|
||||
ret.vEgoStarting = 0.25
|
||||
ret.stoppingDecelRate = 0.3 # reach stopping target smoothly
|
||||
else:
|
||||
tune.kiBP = [0., 5., 35.]
|
||||
tune.kiV = [3.6, 2.4, 1.5]
|
||||
@@ -177,7 +182,7 @@ class CarInterface(CarInterfaceBase):
|
||||
events.add(EventName.vehicleSensorsInvalid)
|
||||
|
||||
if self.CP.openpilotLongitudinalControl:
|
||||
if ret.cruiseState.standstill and not ret.brakePressed:
|
||||
if ret.cruiseState.standstill and not ret.brakePressed and not self.CP.enableGasInterceptor:
|
||||
events.add(EventName.resumeRequired)
|
||||
if self.CS.low_speed_lockout:
|
||||
events.add(EventName.lowSpeedLockout)
|
||||
|
||||
@@ -12,6 +12,8 @@ LongCtrlState = car.CarControl.Actuators.LongControlState
|
||||
|
||||
def long_control_state_trans(CP, active, long_control_state, v_ego,
|
||||
should_stop, brake_pressed, cruise_standstill):
|
||||
# Ignore cruise standstill if car has a gas interceptor
|
||||
cruise_standstill = cruise_standstill and not CP.enableGasInterceptor
|
||||
stopping_condition = should_stop
|
||||
starting_condition = (not should_stop and
|
||||
not cruise_standstill and
|
||||
|
||||
@@ -92,7 +92,7 @@ def migrate_pandaStates(lr):
|
||||
# TODO: safety param migration should be handled automatically
|
||||
safety_param_migration = {
|
||||
"TOYOTA_PRIUS": EPS_SCALE["TOYOTA_PRIUS"] | Panda.FLAG_TOYOTA_STOCK_LONGITUDINAL,
|
||||
"TOYOTA_RAV4": EPS_SCALE["TOYOTA_RAV4"] | Panda.FLAG_TOYOTA_ALT_BRAKE,
|
||||
"TOYOTA_RAV4": EPS_SCALE["TOYOTA_RAV4"] | Panda.FLAG_TOYOTA_ALT_BRAKE | Panda.FLAG_TOYOTA_GAS_INTERCEPTOR,
|
||||
"KIA_EV6": Panda.FLAG_HYUNDAI_EV_GAS | Panda.FLAG_HYUNDAI_CANFD_HDA2,
|
||||
}
|
||||
|
||||
|
||||
Reference in New Issue
Block a user