mirror of https://github.com/commaai/panda.git
Subaru: infrastructure to support the new angle based controllers (#1590)
This commit is contained in:
Justin Newberry
GitHub
parent
b47b8259ef
commit
ea16a54166
|
|
@ -13,6 +13,17 @@
|
|||
const SteeringLimits SUBARU_STEERING_LIMITS = SUBARU_STEERING_LIMITS_GENERATOR(2047, 50, 70);
|
||||
const SteeringLimits SUBARU_GEN2_STEERING_LIMITS = SUBARU_STEERING_LIMITS_GENERATOR(1000, 40, 40);
|
||||
|
||||
const SteeringLimits SUBARU_ANGLE_STEERING_LIMITS = {
|
||||
.angle_deg_to_can = 100,
|
||||
.angle_rate_up_lookup = {
|
||||
{0., 5., 15.},
|
||||
{5., .8, .15}
|
||||
},
|
||||
.angle_rate_down_lookup = {
|
||||
{0., 5., 15.},
|
||||
{5., 3.5, .4}
|
||||
},
|
||||
};
|
||||
|
||||
const LongitudinalLimits SUBARU_LONG_LIMITS = {
|
||||
.min_gas = 808, // appears to be engine braking
|
||||
|
|
@ -31,6 +42,7 @@ const LongitudinalLimits SUBARU_LONG_LIMITS = {
|
|||
#define MSG_SUBARU_Wheel_Speeds 0x13a
|
||||
|
||||
#define MSG_SUBARU_ES_LKAS 0x122
|
||||
#define MSG_SUBARU_ES_LKAS_ANGLE 0x124
|
||||
#define MSG_SUBARU_ES_Brake 0x220
|
||||
#define MSG_SUBARU_ES_Distance 0x221
|
||||
#define MSG_SUBARU_ES_Status 0x222
|
||||
|
|
@ -42,8 +54,8 @@ const LongitudinalLimits SUBARU_LONG_LIMITS = {
|
|||
#define SUBARU_ALT_BUS 1
|
||||
#define SUBARU_CAM_BUS 2
|
||||
|
||||
#define SUBARU_COMMON_TX_MSGS(alt_bus) \
|
||||
{MSG_SUBARU_ES_LKAS, SUBARU_MAIN_BUS, 8}, \
|
||||
#define SUBARU_COMMON_TX_MSGS(alt_bus, lkas_msg) \
|
||||
{lkas_msg, SUBARU_MAIN_BUS, 8}, \
|
||||
{MSG_SUBARU_ES_Distance, alt_bus, 8}, \
|
||||
{MSG_SUBARU_ES_DashStatus, SUBARU_MAIN_BUS, 8}, \
|
||||
{MSG_SUBARU_ES_LKAS_State, SUBARU_MAIN_BUS, 8}, \
|
||||
|
|
@ -61,18 +73,18 @@ const LongitudinalLimits SUBARU_LONG_LIMITS = {
|
|||
{.msg = {{MSG_SUBARU_CruiseControl, alt_bus, 8, .check_checksum = true, .max_counter = 15U, .expected_timestep = 50000U}, { 0 }, { 0 }}}, \
|
||||
|
||||
const CanMsg SUBARU_TX_MSGS[] = {
|
||||
SUBARU_COMMON_TX_MSGS(SUBARU_MAIN_BUS)
|
||||
SUBARU_COMMON_TX_MSGS(SUBARU_MAIN_BUS, MSG_SUBARU_ES_LKAS)
|
||||
};
|
||||
#define SUBARU_TX_MSGS_LEN (sizeof(SUBARU_TX_MSGS) / sizeof(SUBARU_TX_MSGS[0]))
|
||||
|
||||
const CanMsg SUBARU_LONG_TX_MSGS[] = {
|
||||
SUBARU_COMMON_TX_MSGS(SUBARU_MAIN_BUS)
|
||||
SUBARU_COMMON_TX_MSGS(SUBARU_MAIN_BUS, MSG_SUBARU_ES_LKAS)
|
||||
SUBARU_COMMON_LONG_TX_MSGS(SUBARU_MAIN_BUS)
|
||||
};
|
||||
#define SUBARU_LONG_TX_MSGS_LEN (sizeof(SUBARU_LONG_TX_MSGS) / sizeof(SUBARU_LONG_TX_MSGS[0]))
|
||||
|
||||
const CanMsg SUBARU_GEN2_TX_MSGS[] = {
|
||||
SUBARU_COMMON_TX_MSGS(SUBARU_ALT_BUS)
|
||||
SUBARU_COMMON_TX_MSGS(SUBARU_ALT_BUS, MSG_SUBARU_ES_LKAS)
|
||||
};
|
||||
#define SUBARU_GEN2_TX_MSGS_LEN (sizeof(SUBARU_GEN2_TX_MSGS) / sizeof(SUBARU_GEN2_TX_MSGS[0]))
|
||||
|
||||
|
|
@ -91,9 +103,11 @@ addr_checks subaru_gen2_rx_checks = {subaru_gen2_addr_checks, SUBARU_GEN2_ADDR_C
|
|||
|
||||
const uint16_t SUBARU_PARAM_GEN2 = 1;
|
||||
const uint16_t SUBARU_PARAM_LONGITUDINAL = 2;
|
||||
const uint16_t SUBARU_PARAM_LKAS_ANGLE = 4;
|
||||
|
||||
bool subaru_gen2 = false;
|
||||
bool subaru_longitudinal = false;
|
||||
bool subaru_lkas_angle = false;
|
||||
|
||||
|
||||
static uint32_t subaru_get_checksum(CANPacket_t *to_push) {
|
||||
|
|
@ -191,6 +205,15 @@ static int subaru_tx_hook(CANPacket_t *to_send) {
|
|||
violation |= steer_torque_cmd_checks(desired_torque, -1, limits);
|
||||
}
|
||||
|
||||
if (addr == MSG_SUBARU_ES_LKAS_ANGLE) {
|
||||
int desired_angle = (GET_BYTES(to_send, 5, 3) & 0x1FFFFU);
|
||||
desired_angle = to_signed(desired_angle, 17);
|
||||
bool lkas_request = GET_BIT(to_send, 12U);
|
||||
|
||||
const SteeringLimits limits = SUBARU_ANGLE_STEERING_LIMITS;
|
||||
violation |= steer_angle_cmd_checks(desired_angle, lkas_request, limits);
|
||||
}
|
||||
|
||||
// check es_brake brake_pressure limits
|
||||
if (addr == MSG_SUBARU_ES_Brake) {
|
||||
int es_brake_pressure = GET_BYTES(to_send, 2, 2);
|
||||
|
|
@ -255,7 +278,8 @@ static const addr_checks* subaru_init(uint16_t param) {
|
|||
subaru_gen2 = GET_FLAG(param, SUBARU_PARAM_GEN2);
|
||||
|
||||
#ifdef ALLOW_DEBUG
|
||||
subaru_longitudinal = GET_FLAG(param, SUBARU_PARAM_LONGITUDINAL) && !subaru_gen2;
|
||||
subaru_longitudinal = GET_FLAG(param, SUBARU_PARAM_LONGITUDINAL);
|
||||
subaru_lkas_angle = GET_FLAG(param, SUBARU_PARAM_LKAS_ANGLE);
|
||||
#endif
|
||||
|
||||
if (subaru_gen2) {
|
||||
|
|
|
|||
|
|
@ -249,6 +249,7 @@ class Panda:
|
|||
|
||||
FLAG_SUBARU_GEN2 = 1
|
||||
FLAG_SUBARU_LONG = 2
|
||||
FLAG_SUBARU_LKAS_ANGLE = 4
|
||||
|
||||
FLAG_GM_HW_CAM = 1
|
||||
FLAG_GM_HW_CAM_LONG = 2
|
||||
|
|
|
|||
|
|
@ -13,6 +13,7 @@ MSG_SUBARU_Throttle = 0x40
|
|||
MSG_SUBARU_Steering_Torque = 0x119
|
||||
MSG_SUBARU_Wheel_Speeds = 0x13a
|
||||
MSG_SUBARU_ES_LKAS = 0x122
|
||||
MSG_SUBARU_ES_LKAS_ANGLE = 0x124
|
||||
MSG_SUBARU_ES_Brake = 0x220
|
||||
MSG_SUBARU_ES_Distance = 0x221
|
||||
MSG_SUBARU_ES_Status = 0x222
|
||||
|
|
@ -25,8 +26,8 @@ SUBARU_ALT_BUS = 1
|
|||
SUBARU_CAM_BUS = 2
|
||||
|
||||
|
||||
def lkas_tx_msgs(alt_bus):
|
||||
return [[MSG_SUBARU_ES_LKAS, SUBARU_MAIN_BUS],
|
||||
def lkas_tx_msgs(alt_bus, lkas_msg=MSG_SUBARU_ES_LKAS):
|
||||
return [[lkas_msg, SUBARU_MAIN_BUS],
|
||||
[MSG_SUBARU_ES_Distance, alt_bus],
|
||||
[MSG_SUBARU_ES_DashStatus, SUBARU_MAIN_BUS],
|
||||
[MSG_SUBARU_ES_LKAS_State, SUBARU_MAIN_BUS],
|
||||
|
|
@ -36,8 +37,8 @@ def long_tx_msgs():
|
|||
return [[MSG_SUBARU_ES_Brake, SUBARU_MAIN_BUS],
|
||||
[MSG_SUBARU_ES_Status, SUBARU_MAIN_BUS]]
|
||||
|
||||
def fwd_blacklisted_addr():
|
||||
return {SUBARU_CAM_BUS: [MSG_SUBARU_ES_LKAS, MSG_SUBARU_ES_DashStatus, MSG_SUBARU_ES_LKAS_State, MSG_SUBARU_ES_Infotainment]}
|
||||
def fwd_blacklisted_addr(lkas_msg=MSG_SUBARU_ES_LKAS):
|
||||
return {SUBARU_CAM_BUS: [lkas_msg, MSG_SUBARU_ES_DashStatus, MSG_SUBARU_ES_LKAS_State, MSG_SUBARU_ES_Infotainment]}
|
||||
|
||||
class TestSubaruSafetyBase(common.PandaSafetyTest, MeasurementSafetyTest):
|
||||
FLAGS = 0
|
||||
|
|
@ -151,6 +152,30 @@ class TestSubaruTorqueSafetyBase(TestSubaruSafetyBase, common.DriverTorqueSteeri
|
|||
return self.packer.make_can_msg_panda("ES_LKAS", SUBARU_MAIN_BUS, values)
|
||||
|
||||
|
||||
class TestSubaruAngleSafetyBase(TestSubaruSafetyBase, common.AngleSteeringSafetyTest):
|
||||
TX_MSGS = lkas_tx_msgs(SUBARU_MAIN_BUS, MSG_SUBARU_ES_LKAS_ANGLE)
|
||||
RELAY_MALFUNCTION_ADDR = MSG_SUBARU_ES_LKAS_ANGLE
|
||||
FWD_BLACKLISTED_ADDRS = fwd_blacklisted_addr(MSG_SUBARU_ES_LKAS_ANGLE)
|
||||
|
||||
FLAGS = Panda.FLAG_SUBARU_LKAS_ANGLE
|
||||
|
||||
ANGLE_RATE_BP = [0, 5, 15]
|
||||
ANGLE_RATE_UP = [5, 0.8, 0.15]
|
||||
ANGLE_RATE_DOWN = [5, 3.5, 0.4]
|
||||
|
||||
def _angle_cmd_msg(self, angle, enabled=1):
|
||||
values = {"LKAS_Output": angle, "LKAS_Request": enabled}
|
||||
return self.packer.make_can_msg_panda("ES_LKAS_ANGLE", 0, values)
|
||||
|
||||
def _angle_meas_msg(self, angle):
|
||||
values = {"Steering_Angle": angle}
|
||||
return self.packer.make_can_msg_panda("Steering_Torque", 0, values)
|
||||
|
||||
def _pcm_status_msg(self, enable):
|
||||
values = {"Cruise_Activated": enable}
|
||||
return self.packer.make_can_msg_panda("ES_Status", self.ALT_CAM_BUS, values)
|
||||
|
||||
|
||||
class TestSubaruGen1TorqueStockLongitudinalSafety(TestSubaruStockLongitudinalSafetyBase, TestSubaruTorqueSafetyBase):
|
||||
FLAGS = 0
|
||||
TX_MSGS = lkas_tx_msgs(SUBARU_MAIN_BUS)
|
||||
|
|
|
|||
Loading…
Reference in New Issue