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

safety: keep RX check state only for the active safety config (#3864)

Browse Source
This commit is contained in:
Adeeb Shihadeh
2026-11-02 20:16:41 +00:00
committed by firestar5683
parent 9640b6e049
commit 2645933f91
3 changed files with 60 additions and 55 deletions
Download Patch File Download Diff File Expand all files Collapse all files
opendbc_repo/opendbc/safety/declarations.h
+20 -13
View File
@@ -42,13 +42,16 @@
#define GET_BIT(msg, b) ((bool)!!(((msg)->data[((b) / 8U)] >> ((b) % 8U)) & 0x1U))
#define GET_FLAG(value, mask) (((value) & (mask)) == (mask))
#define BUILD_SAFETY_CFG(rx, tx) ((safety_config){(rx), (sizeof((rx)) / sizeof((rx)[0])), \
// length of an RX check array; fails to compile if it has more checks than rx_status can track
#define RX_CHECKS_LEN(rx) ((sizeof((rx)) / sizeof((rx)[0])) + \
(0U * sizeof(char[((sizeof((rx)) / sizeof((rx)[0])) <= MAX_RX_CHECKS) ? 1 : -1])))
#define BUILD_SAFETY_CFG(rx, tx) ((safety_config){(rx), RX_CHECKS_LEN(rx), \
(tx), (sizeof((tx)) / sizeof((tx)[0])), \
false})
#define SET_RX_CHECKS(rx, config) \
do { \
(config).rx_checks = (rx); \
(config).rx_checks_len = sizeof((rx)) / sizeof((rx)[0]); \
(config).rx_checks_len = RX_CHECKS_LEN(rx); \
(config).disable_forwarding = false; \
} while (0);
@@ -67,6 +70,8 @@ uint64_t GET_BYTES_64_LE(const CANPacket_t *msg, int start, int len);
extern const int MAX_WRONG_COUNTERS;
#define MAX_ADDR_CHECK_MSGS 3U
// max number of RX checks in a safety config; their dynamic state is shared by all safety modes
#define MAX_RX_CHECKS 12U
#define MAX_SAMPLE_VALS 6
// used to represent floating point vehicle speed in a sample_t
#define VEHICLE_SPEED_FACTOR 1000.0
@@ -188,26 +193,28 @@ typedef struct {
const bool ignore_quality_flag; // true if quality flag check is skipped
} CanMsgCheck;
// dynamic state of an RX check, reset on safety mode init
typedef struct {
// dynamic flags, reset on safety mode init
bool msg_seen;
int index; // if multiple messages are allowed to be checked, this stores the index of the first one seen. only msg[msg_index] will be used
bool valid_checksum; // true if and only if checksum check is passed
int wrong_counters; // counter of wrong counters, saturated between 0 and MAX_WRONG_COUNTERS
bool valid_quality_flag; // true if the message's quality/health/status signals are valid
uint8_t last_counter; // last counter value
uint32_t last_timestamp; // micro-s
int wrong_counters; // counter of wrong counters, saturated between 0 and MAX_WRONG_COUNTERS
uint8_t index; // if multiple messages are allowed to be checked, this stores the index of the first one seen. only msg[msg_index] will be used
uint8_t last_counter; // last counter value
bool msg_seen;
bool valid_checksum; // true if and only if checksum check is passed
bool valid_quality_flag; // true if the message's quality/health/status signals are valid
bool lagging; // true if and only if the time between updates is excessive
} RxStatus;
// params and flags about checksum, counter and frequency checks for each monitored address
// params and flags about checksum, counter and frequency checks for each monitored address.
// constant: the state of the current safety config's checks is in rx_status, indexed alike
typedef struct {
CanMsgCheck msg[MAX_ADDR_CHECK_MSGS]; // check either messages (e.g. honda steer)
RxStatus status;
const CanMsgCheck msg[MAX_ADDR_CHECK_MSGS]; // check either messages (e.g. honda steer)
} RxCheck;
extern RxStatus rx_status[MAX_RX_CHECKS];
typedef struct {
RxCheck *rx_checks;
const RxCheck *rx_checks;
int rx_checks_len;
const CanMsg *tx_msgs;
int tx_msgs_len;
opendbc_repo/opendbc/safety/safety.h
+37 -39
View File
@@ -169,14 +169,16 @@ uint16_t current_safety_mode = SAFETY_SILENT;
uint16_t current_safety_param = 0;
static const safety_hooks *current_hooks = &nooutput_hooks;
safety_config current_safety_config;
RxStatus rx_status[MAX_RX_CHECKS]; // dynamic state of current_safety_config.rx_checks
static void generic_rx_checks(void);
static void stock_ecu_check(bool stock_ecu_detected, bool immediate);
static bool is_msg_valid(RxCheck addr_list[], int index) {
static bool is_msg_valid(int index) {
bool valid = true;
if (index != -1) {
if (!addr_list[index].status.valid_checksum || !addr_list[index].status.valid_quality_flag || (addr_list[index].status.wrong_counters >= MAX_WRONG_COUNTERS)) {
const RxStatus *status = &rx_status[index];
if (!status->valid_checksum || !status->valid_quality_flag || (status->wrong_counters >= MAX_WRONG_COUNTERS)) {
valid = false;
controls_allowed = false;
aol_host_axis_mask = 0U;
@@ -188,23 +190,24 @@ static bool is_msg_valid(RxCheck addr_list[], int index) {
return valid;
}
static int get_addr_check_index(const CANPacket_t *msg, RxCheck addr_list[], const int len) {
static int get_addr_check_index(const CANPacket_t *msg, const RxCheck addr_list[], const int len) {
int index = -1;
for (int i = 0; i < len; i++) {
RxStatus *status = &rx_status[i];
// if multiple msgs are allowed, determine which one is present on the bus
if (!addr_list[i].status.msg_seen) {
if (!status->msg_seen) {
for (uint8_t j = 0U; (j < MAX_ADDR_CHECK_MSGS) && (addr_list[i].msg[j].addr != 0); j++) {
if (msg_matches(msg, addr_list[i].msg[j].addr, addr_list[i].msg[j].bus, addr_list[i].msg[j].len)) {
addr_list[i].status.index = j;
addr_list[i].status.msg_seen = true;
status->index = j;
status->msg_seen = true;
break;
}
}
}
if (addr_list[i].status.msg_seen) {
int idx = addr_list[i].status.index;
if (msg_matches(msg, addr_list[i].msg[idx].addr, addr_list[i].msg[idx].bus, addr_list[i].msg[idx].len)) {
if (status->msg_seen) {
const CanMsgCheck *m = &addr_list[i].msg[status->index];
if (msg_matches(msg, m->addr, m->bus, m->len)) {
index = i;
break;
}
@@ -213,20 +216,11 @@ static int get_addr_check_index(const CANPacket_t *msg, RxCheck addr_list[], con
return index;
}
static void update_addr_timestamp(RxCheck addr_list[], int index) {
if (index != -1) {
uint32_t ts = microsecond_timer_get();
addr_list[index].status.last_timestamp = ts;
}
}
static void update_counter(RxCheck addr_list[], int index, uint8_t counter) {
if (index != -1) {
uint8_t expected_counter = (addr_list[index].status.last_counter + 1U) % (addr_list[index].msg[addr_list[index].status.index].max_counter + 1U);
addr_list[index].status.wrong_counters += (expected_counter == counter) ? -1 : 1;
addr_list[index].status.wrong_counters = SAFETY_CLAMP(addr_list[index].status.wrong_counters, 0, MAX_WRONG_COUNTERS);
addr_list[index].status.last_counter = counter;
}
static void update_counter(RxStatus *status, const CanMsgCheck *m, uint8_t counter) {
uint8_t expected_counter = (status->last_counter + 1U) % (m->max_counter + 1U);
status->wrong_counters += (expected_counter == counter) ? -1 : 1;
status->wrong_counters = SAFETY_CLAMP(status->wrong_counters, 0, MAX_WRONG_COUNTERS);
status->last_counter = counter;
}
static bool rx_msg_safety_check(const CANPacket_t *msg,
@@ -234,34 +228,37 @@ static bool rx_msg_safety_check(const CANPacket_t *msg,
const safety_hooks *safety_hooks) {
int index = get_addr_check_index(msg, cfg->rx_checks, cfg->rx_checks_len);
update_addr_timestamp(cfg->rx_checks, index);
if (index != -1) {
RxStatus *status = &rx_status[index];
const CanMsgCheck *m = &cfg->rx_checks[index].msg[status->index];
status->last_timestamp = microsecond_timer_get();
// checksum check
if ((safety_hooks->get_checksum != NULL) && (safety_hooks->compute_checksum != NULL) && !cfg->rx_checks[index].msg[cfg->rx_checks[index].status.index].ignore_checksum) {
if ((safety_hooks->get_checksum != NULL) && (safety_hooks->compute_checksum != NULL) && !m->ignore_checksum) {
uint32_t checksum = safety_hooks->get_checksum(msg);
uint32_t checksum_comp = safety_hooks->compute_checksum(msg);
cfg->rx_checks[index].status.valid_checksum = checksum_comp == checksum;
status->valid_checksum = checksum_comp == checksum;
} else {
cfg->rx_checks[index].status.valid_checksum = cfg->rx_checks[index].msg[cfg->rx_checks[index].status.index].ignore_checksum;
status->valid_checksum = m->ignore_checksum;
}
// counter check
if ((safety_hooks->get_counter != NULL) && (cfg->rx_checks[index].msg[cfg->rx_checks[index].status.index].max_counter > 0U)) {
if ((safety_hooks->get_counter != NULL) && (m->max_counter > 0U)) {
uint8_t counter = safety_hooks->get_counter(msg);
update_counter(cfg->rx_checks, index, counter);
update_counter(status, m, counter);
} else {
cfg->rx_checks[index].status.wrong_counters = cfg->rx_checks[index].msg[cfg->rx_checks[index].status.index].ignore_counter ? 0 : MAX_WRONG_COUNTERS;
status->wrong_counters = m->ignore_counter ? 0 : MAX_WRONG_COUNTERS;
}
// quality flag check
if ((safety_hooks->get_quality_flag_valid != NULL) && !cfg->rx_checks[index].msg[cfg->rx_checks[index].status.index].ignore_quality_flag) {
cfg->rx_checks[index].status.valid_quality_flag = safety_hooks->get_quality_flag_valid(msg);
if ((safety_hooks->get_quality_flag_valid != NULL) && !m->ignore_quality_flag) {
status->valid_quality_flag = safety_hooks->get_quality_flag_valid(msg);
} else {
cfg->rx_checks[index].status.valid_quality_flag = cfg->rx_checks[index].msg[cfg->rx_checks[index].status.index].ignore_quality_flag;
status->valid_quality_flag = m->ignore_quality_flag;
}
}
return is_msg_valid(cfg->rx_checks, index);
return is_msg_valid(index);
}
bool safety_rx_hook(const CANPacket_t *msg) {
@@ -366,18 +363,19 @@ void safety_tick(void) {
bool rx_checks_invalid = false;
uint32_t ts = microsecond_timer_get();
for (int i=0; i < current_safety_config.rx_checks_len; i++) {
uint32_t elapsed_time = safety_get_ts_elapsed(ts, current_safety_config.rx_checks[i].status.last_timestamp);
RxStatus *status = &rx_status[i];
uint32_t elapsed_time = safety_get_ts_elapsed(ts, status->last_timestamp);
// lag threshold is max of: 1s and MAX_MISSED_MSGS * expected timestep.
// Quite conservative to not risk false triggers.
// 2s of lag is worse case, since the function is called at 1Hz
uint32_t frequency = current_safety_config.rx_checks[i].msg[current_safety_config.rx_checks[i].status.index].frequency;
uint32_t frequency = current_safety_config.rx_checks[i].msg[status->index].frequency;
uint32_t timestep = 1000000U / SAFETY_MAX(frequency, 1U); // frequency < 10Hz is invalid below
bool lagging = elapsed_time > SAFETY_MAX(timestep * MAX_MISSED_MSGS, 1000000U);
current_safety_config.rx_checks[i].status.lagging = lagging;
status->lagging = lagging;
// enforce minimum frequency for safety-relevant messages
bool frequency_invalid = frequency < 10U;
if (lagging || frequency_invalid || !is_msg_valid(current_safety_config.rx_checks, i)) {
if (lagging || frequency_invalid || !is_msg_valid(i)) {
rx_checks_invalid = true;
controls_allowed = false;
aol_host_axis_mask = 0U;
@@ -551,7 +549,7 @@ int set_safety_hooks(uint16_t mode, uint16_t param) {
current_safety_config = current_hooks->init(param);
// reset all dynamic fields in addr struct
for (int j = 0; j < current_safety_config.rx_checks_len; j++) {
current_safety_config.rx_checks[j].status = (RxStatus){0};
rx_status[j] = (RxStatus){0};
}
}
return set_status;
opendbc_repo/opendbc/safety/tests/libsafety/safety.c
+3 -3
View File
@@ -57,10 +57,10 @@ bool safety_config_valid() {
}
for (int i = 0; i < current_safety_config.rx_checks_len; i++) {
const RxCheck addr = current_safety_config.rx_checks[i];
bool valid = addr.status.msg_seen && !addr.status.lagging && addr.status.valid_checksum && (addr.status.wrong_counters < MAX_WRONG_COUNTERS) && addr.status.valid_quality_flag;
const RxStatus status = rx_status[i];
bool valid = status.msg_seen && !status.lagging && status.valid_checksum && (status.wrong_counters < MAX_WRONG_COUNTERS) && status.valid_quality_flag;
if (!valid) {
// printf("i %d seen %d lagging %d valid checksum %d wrong counters %d valid quality flag %d\n", i, addr.status.msg_seen, addr.status.lagging, addr.status.valid_checksum, addr.status.wrong_counters, addr.status.valid_quality_flag);
// printf("i %d seen %d lagging %d valid checksum %d wrong counters %d valid quality flag %d\n", i, status.msg_seen, status.lagging, status.valid_checksum, status.wrong_counters, status.valid_quality_flag);
return false;
}
}