mirror of https://github.com/commaai/panda.git
Kia EV6 (#905)
* block lkas * taco time * local changes * start * tx checks * counter + vehicle moving * support big can fd * check crc * add torque to rx checks * tests * little more * little more * get some misra coverage Co-authored-by: Comma Device <device@comma.ai> Co-authored-by: Adeeb Shihadeh <adeebshihadeh@gmail.com>
This commit is contained in:
Willem Melching
GitHub
parent
b6a672de5a
commit
36c62afa0c
|
|
@ -2,6 +2,8 @@
|
|||
// FDCAN2_IT0, FDCAN2_IT1
|
||||
// FDCAN3_IT0, FDCAN3_IT1
|
||||
|
||||
#define CANFD
|
||||
|
||||
#define BUS_OFF_FAIL_LIMIT 2U
|
||||
uint8_t bus_off_err[] = {0U, 0U, 0U};
|
||||
|
||||
|
|
|
|||
|
|
@ -17,6 +17,11 @@
|
|||
#include "safety/safety_elm327.h"
|
||||
#include "safety/safety_body.h"
|
||||
|
||||
// CAN-FD only safety modes
|
||||
#ifdef CANFD
|
||||
#include "safety/safety_hyundai_hda2.h"
|
||||
#endif
|
||||
|
||||
// from cereal.car.CarParams.SafetyModel
|
||||
#define SAFETY_SILENT 0U
|
||||
#define SAFETY_HONDA_NIDEC 1U
|
||||
|
|
@ -43,6 +48,7 @@
|
|||
#define SAFETY_STELLANTIS 25U
|
||||
#define SAFETY_FAW 26U
|
||||
#define SAFETY_BODY 27U
|
||||
#define SAFETY_HYUNDAI_HDA2 28U
|
||||
|
||||
uint16_t current_safety_mode = SAFETY_SILENT;
|
||||
uint16_t current_safety_param = 0;
|
||||
|
|
@ -71,14 +77,29 @@ bool get_longitudinal_allowed(void) {
|
|||
|
||||
// Given a CRC-8 poly, generate a static lookup table to use with a fast CRC-8
|
||||
// algorithm. Called at init time for safety modes using CRC-8.
|
||||
void gen_crc_lookup_table(uint8_t poly, uint8_t crc_lut[]) {
|
||||
void gen_crc_lookup_table_8(uint8_t poly, uint8_t crc_lut[]) {
|
||||
for (int i = 0; i < 256; i++) {
|
||||
uint8_t crc = i;
|
||||
for (int j = 0; j < 8; j++) {
|
||||
if ((crc & 0x80U) != 0U)
|
||||
if ((crc & 0x80U) != 0U) {
|
||||
crc = (uint8_t)((crc << 1) ^ poly);
|
||||
else
|
||||
} else {
|
||||
crc <<= 1;
|
||||
}
|
||||
}
|
||||
crc_lut[i] = crc;
|
||||
}
|
||||
}
|
||||
|
||||
void gen_crc_lookup_table_16(uint16_t poly, uint16_t crc_lut[]) {
|
||||
for (uint16_t i = 0; i < 256U; i++) {
|
||||
uint16_t crc = i << 8U;
|
||||
for (uint16_t j = 0; j < 8U; j++) {
|
||||
if ((crc & 0x8000U) != 0U) {
|
||||
crc = (uint16_t)((crc << 1) ^ poly);
|
||||
} else {
|
||||
crc <<= 1;
|
||||
}
|
||||
}
|
||||
crc_lut[i] = crc;
|
||||
}
|
||||
|
|
@ -253,6 +274,9 @@ const safety_hook_config safety_hook_registry[] = {
|
|||
{SAFETY_HYUNDAI_LEGACY, &hyundai_legacy_hooks},
|
||||
{SAFETY_MAZDA, &mazda_hooks},
|
||||
{SAFETY_BODY, &body_hooks},
|
||||
#ifdef CANFD
|
||||
{SAFETY_HYUNDAI_HDA2, &hyundai_hda2_hooks},
|
||||
#endif
|
||||
#ifdef ALLOW_DEBUG
|
||||
{SAFETY_TESLA, &tesla_hooks},
|
||||
{SAFETY_SUBARU_LEGACY, &subaru_legacy_hooks},
|
||||
|
|
|
|||
|
|
@ -0,0 +1,211 @@
|
|||
const int HYUNDAI_HDA2_MAX_STEER = 150;
|
||||
const int HYUNDAI_HDA2_MAX_RT_DELTA = 112; // max delta torque allowed for real time checks
|
||||
const uint32_t HYUNDAI_HDA2_RT_INTERVAL = 250000; // 250ms between real time checks
|
||||
const int HYUNDAI_HDA2_MAX_RATE_UP = 3;
|
||||
const int HYUNDAI_HDA2_MAX_RATE_DOWN = 7;
|
||||
const int HYUNDAI_HDA2_DRIVER_TORQUE_ALLOWANCE = 50;
|
||||
const int HYUNDAI_HDA2_DRIVER_TORQUE_FACTOR = 2;
|
||||
const uint32_t HYUNDAI_HDA2_STANDSTILL_THRSLD = 30; // ~1kph
|
||||
|
||||
const CanMsg HYUNDAI_HDA2_TX_MSGS[] = {
|
||||
{0x50, 0, 16},
|
||||
{0x1CF, 1, 8},
|
||||
};
|
||||
|
||||
AddrCheckStruct hyundai_hda2_addr_checks[] = {
|
||||
{.msg = {{0x35, 1, 32, .check_checksum = true, .max_counter = 0xffU, .expected_timestep = 10000U}, { 0 }, { 0 }}},
|
||||
{.msg = {{0x65, 1, 32, .check_checksum = true, .max_counter = 0xffU, .expected_timestep = 10000U}, { 0 }, { 0 }}},
|
||||
{.msg = {{0xa0, 1, 24, .check_checksum = true, .max_counter = 0xffU, .expected_timestep = 10000U}, { 0 }, { 0 }}},
|
||||
{.msg = {{0xea, 1, 24, .check_checksum = true, .max_counter = 0xffU, .expected_timestep = 10000U}, { 0 }, { 0 }}},
|
||||
{.msg = {{0x175, 1, 24, .check_checksum = true, .max_counter = 0xffU, .expected_timestep = 10000U}, { 0 }, { 0 }}},
|
||||
};
|
||||
#define HYUNDAI_HDA2_ADDR_CHECK_LEN (sizeof(hyundai_hda2_addr_checks) / sizeof(hyundai_hda2_addr_checks[0]))
|
||||
|
||||
addr_checks hyundai_hda2_rx_checks = {hyundai_hda2_addr_checks, HYUNDAI_HDA2_ADDR_CHECK_LEN};
|
||||
|
||||
uint16_t hyundai_hda2_crc_lut[256];
|
||||
|
||||
static uint8_t hyundai_hda2_get_counter(CANPacket_t *to_push) {
|
||||
return GET_BYTE(to_push, 2);
|
||||
}
|
||||
|
||||
static uint32_t hyundai_hda2_get_checksum(CANPacket_t *to_push) {
|
||||
uint32_t chksum = GET_BYTE(to_push, 0) | (GET_BYTE(to_push, 1) << 8);
|
||||
return chksum;
|
||||
}
|
||||
|
||||
static uint32_t hyundai_hda2_compute_checksum(CANPacket_t *to_push) {
|
||||
int len = GET_LEN(to_push);
|
||||
uint32_t address = GET_ADDR(to_push);
|
||||
|
||||
uint16_t crc = 0;
|
||||
|
||||
for (int i = 2; i < len; i++) {
|
||||
crc = (crc << 8U) ^ hyundai_hda2_crc_lut[(crc >> 8U) ^ GET_BYTE(to_push, i)];
|
||||
}
|
||||
|
||||
// Add address to crc
|
||||
crc = (crc << 8U) ^ hyundai_hda2_crc_lut[(crc >> 8U) ^ ((address >> 0U) & 0xFFU)];
|
||||
crc = (crc << 8U) ^ hyundai_hda2_crc_lut[(crc >> 8U) ^ ((address >> 8U) & 0xFFU)];
|
||||
|
||||
if (len == 8) {
|
||||
crc ^= 0x5f29U;
|
||||
} else if (len == 16) {
|
||||
crc ^= 0x041dU;
|
||||
} else if (len == 24) {
|
||||
crc ^= 0x819dU;
|
||||
} else if (len == 32) {
|
||||
crc ^= 0x9f5bU;
|
||||
} else {
|
||||
|
||||
}
|
||||
|
||||
return crc;
|
||||
}
|
||||
|
||||
static int hyundai_hda2_rx_hook(CANPacket_t *to_push) {
|
||||
|
||||
bool valid = addr_safety_check(to_push, &hyundai_hda2_rx_checks,
|
||||
hyundai_hda2_get_checksum, hyundai_hda2_compute_checksum, hyundai_hda2_get_counter);
|
||||
|
||||
int bus = GET_BUS(to_push);
|
||||
int addr = GET_ADDR(to_push);
|
||||
|
||||
if (valid && (bus == 1)) {
|
||||
|
||||
if (addr == 0xea) {
|
||||
int torque_driver_new = ((GET_BYTE(to_push, 11) & 0x1fU) << 8U) | GET_BYTE(to_push, 10);
|
||||
torque_driver_new -= 4095;
|
||||
update_sample(&torque_driver, torque_driver_new);
|
||||
}
|
||||
|
||||
if (addr == 0x175) {
|
||||
bool cruise_engaged = GET_BIT(to_push, 68U);
|
||||
|
||||
if (cruise_engaged && !cruise_engaged_prev) {
|
||||
controls_allowed = 1;
|
||||
}
|
||||
|
||||
if (!cruise_engaged) {
|
||||
controls_allowed = 0;
|
||||
}
|
||||
cruise_engaged_prev = cruise_engaged;
|
||||
}
|
||||
|
||||
if (addr == 0x35) {
|
||||
gas_pressed = GET_BYTE(to_push, 5) != 0U;
|
||||
}
|
||||
|
||||
if (addr == 0x65) {
|
||||
brake_pressed = GET_BIT(to_push, 57U) != 0U;
|
||||
}
|
||||
|
||||
if (addr == 0xa0) {
|
||||
uint32_t speed = 0;
|
||||
for (int i = 8; i < 15; i+=2) {
|
||||
speed += GET_BYTE(to_push, i) | (GET_BYTE(to_push, i + 1) << 8U);
|
||||
}
|
||||
vehicle_moving = (speed / 4U) > HYUNDAI_HDA2_STANDSTILL_THRSLD;
|
||||
}
|
||||
}
|
||||
|
||||
generic_rx_checks((addr == 0x50) && (bus == 0));
|
||||
|
||||
return valid;
|
||||
}
|
||||
|
||||
static int hyundai_hda2_tx_hook(CANPacket_t *to_send, bool longitudinal_allowed) {
|
||||
UNUSED(longitudinal_allowed);
|
||||
|
||||
int tx = msg_allowed(to_send, HYUNDAI_HDA2_TX_MSGS, sizeof(HYUNDAI_HDA2_TX_MSGS)/sizeof(HYUNDAI_HDA2_TX_MSGS[0]));
|
||||
int addr = GET_ADDR(to_send);
|
||||
int bus = GET_BUS(to_send);
|
||||
|
||||
// steering
|
||||
if ((addr == 0x50) && (bus == 0)) {
|
||||
int desired_torque = ((GET_BYTE(to_send, 6) & 0xFU) << 7U) | (GET_BYTE(to_send, 5) >> 1U);
|
||||
desired_torque -= 1024;
|
||||
uint32_t ts = microsecond_timer_get();
|
||||
bool violation = 0;
|
||||
|
||||
if (controls_allowed) {
|
||||
// *** global torque limit check ***
|
||||
violation |= max_limit_check(desired_torque, HYUNDAI_HDA2_MAX_STEER, -HYUNDAI_HDA2_MAX_STEER);
|
||||
|
||||
// *** torque rate limit check ***
|
||||
violation |= driver_limit_check(desired_torque, desired_torque_last, &torque_driver,
|
||||
HYUNDAI_HDA2_MAX_STEER, HYUNDAI_HDA2_MAX_RATE_UP, HYUNDAI_HDA2_MAX_RATE_DOWN,
|
||||
HYUNDAI_HDA2_DRIVER_TORQUE_ALLOWANCE, HYUNDAI_HDA2_DRIVER_TORQUE_FACTOR);
|
||||
|
||||
// used next time
|
||||
desired_torque_last = desired_torque;
|
||||
|
||||
// *** torque real time rate limit check ***
|
||||
violation |= rt_rate_limit_check(desired_torque, rt_torque_last, HYUNDAI_MAX_RT_DELTA);
|
||||
|
||||
// every RT_INTERVAL set the new limits
|
||||
uint32_t ts_elapsed = get_ts_elapsed(ts, ts_last);
|
||||
if (ts_elapsed > HYUNDAI_RT_INTERVAL) {
|
||||
rt_torque_last = desired_torque;
|
||||
ts_last = ts;
|
||||
}
|
||||
}
|
||||
|
||||
// no torque if controls is not allowed
|
||||
if (!controls_allowed && (desired_torque != 0)) {
|
||||
violation = 1;
|
||||
}
|
||||
|
||||
// reset to 0 if either controls is not allowed or there's a violation
|
||||
if (violation || !controls_allowed) {
|
||||
desired_torque_last = 0;
|
||||
rt_torque_last = 0;
|
||||
ts_last = ts;
|
||||
}
|
||||
|
||||
if (violation) {
|
||||
tx = 0;
|
||||
}
|
||||
}
|
||||
|
||||
// cruise buttons check
|
||||
if ((addr == 0x1cf) && (bus == 1)) {
|
||||
bool is_cancel = GET_BYTE(to_send, 2) == 4U;
|
||||
bool is_resume = GET_BYTE(to_send, 2) == 1U;
|
||||
bool allowed = (is_cancel && cruise_engaged_prev) || (is_resume && controls_allowed);
|
||||
if (!allowed) {
|
||||
tx = 0;
|
||||
}
|
||||
}
|
||||
|
||||
return tx;
|
||||
}
|
||||
|
||||
static int hyundai_hda2_fwd_hook(int bus_num, CANPacket_t *to_fwd) {
|
||||
|
||||
int bus_fwd = -1;
|
||||
int addr = GET_ADDR(to_fwd);
|
||||
|
||||
if (bus_num == 0) {
|
||||
bus_fwd = 2;
|
||||
}
|
||||
if ((bus_num == 2) && (addr != 0x50)) {
|
||||
bus_fwd = 0;
|
||||
}
|
||||
|
||||
return bus_fwd;
|
||||
}
|
||||
|
||||
static const addr_checks* hyundai_hda2_init(uint16_t param) {
|
||||
UNUSED(param);
|
||||
gen_crc_lookup_table_16(0x1021, hyundai_hda2_crc_lut);
|
||||
return &hyundai_hda2_rx_checks;
|
||||
}
|
||||
|
||||
const safety_hooks hyundai_hda2_hooks = {
|
||||
.init = hyundai_hda2_init,
|
||||
.rx = hyundai_hda2_rx_hook,
|
||||
.tx = hyundai_hda2_tx_hook,
|
||||
.tx_lin = nooutput_tx_lin_hook,
|
||||
.fwd = hyundai_hda2_fwd_hook,
|
||||
};
|
||||
|
|
@ -79,7 +79,7 @@ static uint32_t volkswagen_mqb_compute_crc(CANPacket_t *to_push) {
|
|||
static const addr_checks* volkswagen_mqb_init(uint16_t param) {
|
||||
UNUSED(param);
|
||||
|
||||
gen_crc_lookup_table(0x2F, volkswagen_crc8_lut_8h2f);
|
||||
gen_crc_lookup_table_8(0x2F, volkswagen_crc8_lut_8h2f);
|
||||
return &volkswagen_mqb_rx_checks;
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -69,7 +69,8 @@ bool driver_limit_check(int val, int val_last, struct sample_t *val_driver,
|
|||
bool get_longitudinal_allowed(void);
|
||||
bool rt_rate_limit_check(int val, int val_last, const int MAX_RT_DELTA);
|
||||
float interpolate(struct lookup_t xy, float x);
|
||||
void gen_crc_lookup_table(uint8_t poly, uint8_t crc_lut[]);
|
||||
void gen_crc_lookup_table_8(uint8_t poly, uint8_t crc_lut[]);
|
||||
void gen_crc_lookup_table_16(uint16_t poly, uint16_t crc_lut[]);
|
||||
bool msg_allowed(CANPacket_t *to_send, const CanMsg msg_list[], int len);
|
||||
int get_addr_check_index(CANPacket_t *to_push, AddrCheckStruct addr_list[], const int len);
|
||||
void update_counter(AddrCheckStruct addr_list[], int index, uint8_t counter);
|
||||
|
|
|
|||
|
|
@ -149,6 +149,7 @@ class Panda:
|
|||
SAFETY_STELLANTIS = 25
|
||||
SAFETY_FAW = 26
|
||||
SAFETY_BODY = 27
|
||||
SAFETY_HYUNDAI_HDA2 = 28
|
||||
|
||||
SERIAL_DEBUG = 0
|
||||
SERIAL_ESP = 1
|
||||
|
|
|
|||
|
|
@ -22,7 +22,7 @@ misra_f4_output=$( cat /tmp/misra/misra_f4_output.txt | grep -v ": information:
|
|||
|
||||
|
||||
printf "\nPANDA H7 CODE\n"
|
||||
cppcheck -DPANDA -DSTM32H7 -UPEDAL -DUID_BASE \
|
||||
cppcheck -DPANDA -DSTM32H7 -DCANFD -UPEDAL -DUID_BASE \
|
||||
--suppressions-list=suppressions.txt --suppress=*:*inc/* \
|
||||
-I $PANDA_DIR/board/ --dump --enable=all --inline-suppr --force \
|
||||
$PANDA_DIR/board/main.c 2>/tmp/misra/cppcheck_h7_output.txt
|
||||
|
|
|
|||
|
|
@ -3,7 +3,9 @@ import abc
|
|||
import unittest
|
||||
import importlib
|
||||
import numpy as np
|
||||
from collections import defaultdict
|
||||
from typing import Optional, List, Dict
|
||||
|
||||
from opendbc.can.packer import CANPacker # pylint: disable=import-error
|
||||
from panda import ALTERNATIVE_EXPERIENCE, LEN_TO_DLC
|
||||
from panda.tests.safety import libpandasafety_py
|
||||
|
|
@ -25,8 +27,13 @@ def make_msg(bus, addr, length=8):
|
|||
return package_can_msg([addr, 0, b'\x00' * length, bus])
|
||||
|
||||
class CANPackerPanda(CANPacker):
|
||||
def make_can_msg_panda(self, name_or_addr, bus, values, counter=-1, fix_checksum=None):
|
||||
msg = self.make_can_msg(name_or_addr, bus, values, counter=-1)
|
||||
_counters: Dict[str, int] = defaultdict(lambda: -1)
|
||||
|
||||
def make_can_msg_panda(self, name_or_addr, bus, values, counter=False, fix_checksum=None):
|
||||
if counter:
|
||||
self._counters[name_or_addr] += 1
|
||||
|
||||
msg = self.make_can_msg(name_or_addr, bus, values, counter=self._counters[name_or_addr])
|
||||
if fix_checksum is not None:
|
||||
msg = fix_checksum(msg)
|
||||
return package_can_msg(msg)
|
||||
|
|
|
|||
|
|
@ -7,6 +7,8 @@
|
|||
#include "can_definitions.h"
|
||||
#include "utils.h"
|
||||
|
||||
#define CANFD
|
||||
|
||||
typedef struct {
|
||||
uint32_t CNT;
|
||||
} TIM_TypeDef;
|
||||
|
|
|
|||
|
|
@ -0,0 +1,76 @@
|
|||
#!/usr/bin/env python3
|
||||
import unittest
|
||||
from panda import Panda
|
||||
from panda.tests.safety import libpandasafety_py
|
||||
import panda.tests.safety.common as common
|
||||
from panda.tests.safety.common import CANPackerPanda
|
||||
|
||||
|
||||
class TestHyundaiHDA2(common.PandaSafetyTest):
|
||||
|
||||
TX_MSGS = [[0x50, 0], [0x1CF, 1]]
|
||||
STANDSTILL_THRESHOLD = 30 # ~1kph
|
||||
RELAY_MALFUNCTION_ADDR = 0x50
|
||||
RELAY_MALFUNCTION_BUS = 0
|
||||
FWD_BLACKLISTED_ADDRS = {2: [0x50]}
|
||||
FWD_BUS_LOOKUP = {0: 2, 2: 0}
|
||||
|
||||
MAX_RATE_UP = 3
|
||||
MAX_RATE_DOWN = 7
|
||||
MAX_TORQUE = 150
|
||||
|
||||
MAX_RT_DELTA = 112
|
||||
RT_INTERVAL = 250000
|
||||
|
||||
DRIVER_TORQUE_ALLOWANCE = 50
|
||||
DRIVER_TORQUE_FACTOR = 2
|
||||
|
||||
def setUp(self):
|
||||
self.packer = CANPackerPanda("kia_ev6")
|
||||
self.safety = libpandasafety_py.libpandasafety
|
||||
self.safety.set_safety_hooks(Panda.SAFETY_HYUNDAI_HDA2, 0)
|
||||
self.safety.init_tests()
|
||||
|
||||
def _torque_driver_msg(self, torque):
|
||||
values = {"STEERING_COL_TORQUE": torque}
|
||||
return self.packer.make_can_msg_panda("MDPS", 1, values, counter=True)
|
||||
|
||||
def _torque_msg(self, torque, steer_req=1):
|
||||
values = {"TORQUE_REQUEST": torque}
|
||||
return self.packer.make_can_msg_panda("LKAS", 0, values, counter=True)
|
||||
|
||||
def _speed_msg(self, speed):
|
||||
values = {f"WHEEL_SPEED_{i}": speed * 0.03125 for i in range(1, 5)}
|
||||
return self.packer.make_can_msg_panda("WHEEL_SPEEDS", 1, values, counter=True)
|
||||
|
||||
def _user_brake_msg(self, brake):
|
||||
values = {"BRAKE_PRESSED": brake}
|
||||
return self.packer.make_can_msg_panda("BRAKE", 1, values, counter=True)
|
||||
|
||||
def _user_gas_msg(self, gas):
|
||||
values = {"ACCELERATOR_PEDAL": gas}
|
||||
return self.packer.make_can_msg_panda("ACCELERATOR", 1, values, counter=True)
|
||||
|
||||
def _pcm_status_msg(self, enable):
|
||||
values = {"CRUISE_ACTIVE": enable}
|
||||
return self.packer.make_can_msg_panda("SCC1", 1, values, counter=True)
|
||||
|
||||
def _button_msg(self, resume=False, cancel=False):
|
||||
values = {
|
||||
"DISTANCE_BTN": resume,
|
||||
"PAUSE_RESUME_BTN": cancel,
|
||||
}
|
||||
return self.packer.make_can_msg_panda("CRUISE_BUTTONS", 1, values)
|
||||
|
||||
def test_buttons(self):
|
||||
for controls_allowed in (True, False):
|
||||
for cruise_engaged in (True, False):
|
||||
self._rx(self._pcm_status_msg(cruise_engaged))
|
||||
self.safety.set_controls_allowed(controls_allowed)
|
||||
|
||||
self.assertEqual(cruise_engaged, self._tx(self._button_msg(cancel=True)))
|
||||
self.assertEqual(controls_allowed, self._tx(self._button_msg(resume=True)))
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
unittest.main()
|
||||
Loading…
Reference in New Issue