Skip to content

Commit

Permalink
matching master version
Browse files Browse the repository at this point in the history
  • Loading branch information
@eFiniLan
eFiniLan committed Feb 4, 2024
1 parent edbd19c commit 1108478
Show file tree
Hide file tree
Showing 22 changed files with 302 additions and 198 deletions.
7 changes: 3 additions & 4 deletions board/main_comms.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -14,10 +14,9 @@ int get_health_pkt(void *dat) {

// Use the GPIO pin to determine ignition or use a CAN based logic
health->ignition_line_pkt = (uint8_t)(current_board->check_ignition());
health->ignition_can_pkt = (uint8_t)(ignition_can);
health->ignition_can_pkt = ignition_can;

health->controls_allowed_pkt = controls_allowed;
health->gas_interceptor_detected_pkt = gas_interceptor_detected;
health->safety_tx_blocked_pkt = safety_tx_blocked;
health->safety_rx_invalid_pkt = safety_rx_invalid;
health->tx_buffer_overflow_pkt = tx_buffer_overflow;
Expand All @@ -27,8 +26,8 @@ int get_health_pkt(void *dat) {
health->safety_mode_pkt = (uint8_t)(current_safety_mode);
health->safety_param_pkt = current_safety_param;
health->alternative_experience_pkt = alternative_experience;
health->power_save_enabled_pkt = (uint8_t)(power_save_status == POWER_SAVE_STATUS_ENABLED);
health->heartbeat_lost_pkt = (uint8_t)(heartbeat_lost);
health->power_save_enabled_pkt = power_save_status == POWER_SAVE_STATUS_ENABLED;
health->heartbeat_lost_pkt = heartbeat_lost;
health->safety_rx_checks_invalid = safety_rx_checks_invalid;

health->spi_checksum_error_count = spi_checksum_error_count;
Expand Down
81 changes: 38 additions & 43 deletions board/safety.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -57,12 +57,10 @@ uint16_t current_safety_param = 0;
const safety_hooks *current_hooks = &nooutput_hooks;
safety_config current_safety_config;

bool safety_rx_hook(CANPacket_t *to_push) {
bool safety_rx_hook(const CANPacket_t *to_push) {
bool controls_allowed_prev = controls_allowed;

bool valid = rx_msg_safety_check(to_push, &current_safety_config, current_hooks->get_checksum,
current_hooks->compute_checksum, current_hooks->get_counter,
current_hooks->get_quality_flag_valid);
bool valid = rx_msg_safety_check(to_push, &current_safety_config, current_hooks);
if (valid) {
current_hooks->rx(to_push);
}
Expand All @@ -85,6 +83,7 @@ bool safety_tx_hook(CANPacket_t *to_send) {
return !relay_malfunction && whitelisted && safety_allowed;
}

// rick - keep it for legacy support
bool safety_tx_lin_hook(int lin_num, uint8_t *data, int len) {
return current_hooks->tx_lin(lin_num, data, len);
}
Expand Down Expand Up @@ -127,7 +126,7 @@ 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) {
bool msg_allowed(const CANPacket_t *to_send, const CanMsg msg_list[], int len) {
int addr = GET_ADDR(to_send);
int bus = GET_BUS(to_send);
int length = GET_LEN(to_send);
Expand All @@ -142,27 +141,27 @@ bool msg_allowed(CANPacket_t *to_send, const CanMsg msg_list[], int len) {
return allowed;
}

int get_addr_check_index(CANPacket_t *to_push, RxCheck addr_list[], const int len) {
int get_addr_check_index(const CANPacket_t *to_push, RxCheck addr_list[], const int len) {
int bus = GET_BUS(to_push);
int addr = GET_ADDR(to_push);
int length = GET_LEN(to_push);

int index = -1;
for (int i = 0; i < len; i++) {
// if multiple msgs are allowed, determine which one is present on the bus
if (!addr_list[i].msg_seen) {
if (!addr_list[i].status.msg_seen) {
for (uint8_t j = 0U; (j < MAX_ADDR_CHECK_MSGS) && (addr_list[i].msg[j].addr != 0); j++) {
if ((addr == addr_list[i].msg[j].addr) && (bus == addr_list[i].msg[j].bus) &&
(length == addr_list[i].msg[j].len)) {
addr_list[i].index = j;
addr_list[i].msg_seen = true;
addr_list[i].status.index = j;
addr_list[i].status.msg_seen = true;
break;
}
}
}

if (addr_list[i].msg_seen) {
int idx = addr_list[i].index;
if (addr_list[i].status.msg_seen) {
int idx = addr_list[i].status.index;
if ((addr == addr_list[i].msg[idx].addr) && (bus == addr_list[i].msg[idx].bus) &&
(length == addr_list[i].msg[idx].len)) {
index = i;
Expand All @@ -179,13 +178,13 @@ void safety_tick(const safety_config *cfg) {
uint32_t ts = microsecond_timer_get();
if (cfg != NULL) {
for (int i=0; i < cfg->rx_checks_len; i++) {
uint32_t elapsed_time = get_ts_elapsed(ts, cfg->rx_checks[i].last_timestamp);
uint32_t elapsed_time = get_ts_elapsed(ts, cfg->rx_checks[i].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 timestep = 1e6 / cfg->rx_checks[i].msg[cfg->rx_checks[i].index].frequency;
uint32_t timestep = 1e6 / cfg->rx_checks[i].msg[cfg->rx_checks[i].status.index].frequency;
bool lagging = elapsed_time > MAX(timestep * MAX_MISSED_MSGS, 1e6);
cfg->rx_checks[i].lagging = lagging;
cfg->rx_checks[i].status.lagging = lagging;
if (lagging) {
controls_allowed = false;
}
Expand All @@ -201,17 +200,17 @@ void safety_tick(const safety_config *cfg) {

void update_counter(RxCheck addr_list[], int index, uint8_t counter) {
if (index != -1) {
uint8_t expected_counter = (addr_list[index].last_counter + 1U) % (addr_list[index].msg[addr_list[index].index].max_counter + 1U);
addr_list[index].wrong_counters += (expected_counter == counter) ? -1 : 1;
addr_list[index].wrong_counters = CLAMP(addr_list[index].wrong_counters, 0, MAX_WRONG_COUNTERS);
addr_list[index].last_counter = counter;
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 = CLAMP(addr_list[index].status.wrong_counters, 0, MAX_WRONG_COUNTERS);
addr_list[index].status.last_counter = counter;
}
}

bool is_msg_valid(RxCheck addr_list[], int index) {
bool valid = true;
if (index != -1) {
if (!addr_list[index].valid_checksum || !addr_list[index].valid_quality_flag || (addr_list[index].wrong_counters >= MAX_WRONG_COUNTERS)) {
if (!addr_list[index].status.valid_checksum || !addr_list[index].status.valid_quality_flag || (addr_list[index].status.wrong_counters >= MAX_WRONG_COUNTERS)) {
valid = false;
controls_allowed = false;
}
Expand All @@ -222,43 +221,40 @@ bool is_msg_valid(RxCheck addr_list[], int index) {
void update_addr_timestamp(RxCheck addr_list[], int index) {
if (index != -1) {
uint32_t ts = microsecond_timer_get();
addr_list[index].last_timestamp = ts;
addr_list[index].status.last_timestamp = ts;
}
}

bool rx_msg_safety_check(CANPacket_t *to_push,
const safety_config *cfg,
const get_checksum_t get_checksum,
const compute_checksum_t compute_checksum,
const get_counter_t get_counter,
const get_quality_flag_valid_t get_quality_flag_valid) {
bool rx_msg_safety_check(const CANPacket_t *to_push,
const safety_config *cfg,
const safety_hooks *safety_hooks) {

int index = get_addr_check_index(to_push, cfg->rx_checks, cfg->rx_checks_len);
update_addr_timestamp(cfg->rx_checks, index);

if (index != -1) {
// checksum check
if ((get_checksum != NULL) && (compute_checksum != NULL) && cfg->rx_checks[index].msg[cfg->rx_checks[index].index].check_checksum) {
uint32_t checksum = get_checksum(to_push);
uint32_t checksum_comp = compute_checksum(to_push);
cfg->rx_checks[index].valid_checksum = checksum_comp == checksum;
if ((safety_hooks->get_checksum != NULL) && (safety_hooks->compute_checksum != NULL) && cfg->rx_checks[index].msg[cfg->rx_checks[index].status.index].check_checksum) {
uint32_t checksum = safety_hooks->get_checksum(to_push);
uint32_t checksum_comp = safety_hooks->compute_checksum(to_push);
cfg->rx_checks[index].status.valid_checksum = checksum_comp == checksum;
} else {
cfg->rx_checks[index].valid_checksum = true;
cfg->rx_checks[index].status.valid_checksum = true;
}

// counter check (max_counter == 0 means skip check)
if ((get_counter != NULL) && (cfg->rx_checks[index].msg[cfg->rx_checks[index].index].max_counter > 0U)) {
uint8_t counter = get_counter(to_push);
if ((safety_hooks->get_counter != NULL) && (cfg->rx_checks[index].msg[cfg->rx_checks[index].status.index].max_counter > 0U)) {
uint8_t counter = safety_hooks->get_counter(to_push);
update_counter(cfg->rx_checks, index, counter);
} else {
cfg->rx_checks[index].wrong_counters = 0U;
cfg->rx_checks[index].status.wrong_counters = 0U;
}

// quality flag check
if ((get_quality_flag_valid != NULL) && cfg->rx_checks[index].msg[cfg->rx_checks[index].index].quality_flag) {
cfg->rx_checks[index].valid_quality_flag = get_quality_flag_valid(to_push);
if ((safety_hooks->get_quality_flag_valid != NULL) && cfg->rx_checks[index].msg[cfg->rx_checks[index].status.index].quality_flag) {
cfg->rx_checks[index].status.valid_quality_flag = safety_hooks->get_quality_flag_valid(to_push);
} else {
cfg->rx_checks[index].valid_quality_flag = true;
cfg->rx_checks[index].status.valid_quality_flag = true;
}
}
return is_msg_valid(cfg->rx_checks, index);
Expand Down Expand Up @@ -336,7 +332,7 @@ int set_safety_hooks(uint16_t mode, uint16_t param) {
// reset state set by safety mode
safety_mode_cnt = 0U;
relay_malfunction = false;
gas_interceptor_detected = false;
enable_gas_interceptor = false;
gas_interceptor_prev = 0;
gas_pressed = false;
gas_pressed_prev = false;
Expand Down Expand Up @@ -388,10 +384,9 @@ int set_safety_hooks(uint16_t mode, uint16_t param) {
current_safety_config.rx_checks_len = cfg.rx_checks_len;
current_safety_config.tx_msgs = cfg.tx_msgs;
current_safety_config.tx_msgs_len = cfg.tx_msgs_len;
// reset message index and seen flags in addr struct
// 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].index = 0;
current_safety_config.rx_checks[j].msg_seen = false;
current_safety_config.rx_checks[j].status = (RxStatus){0};
}
}
return set_status;
Expand Down Expand Up @@ -455,7 +450,7 @@ bool dist_to_meas_check(int val, int val_last, struct sample_t *val_meas,
}

// check that commanded value isn't fighting against driver
bool driver_limit_check(int val, int val_last, struct sample_t *val_driver,
bool driver_limit_check(int val, int val_last, const struct sample_t *val_driver,
const int MAX_VAL, const int MAX_RATE_UP, const int MAX_RATE_DOWN,
const int MAX_ALLOWANCE, const int DRIVER_FACTOR) {

Expand Down Expand Up @@ -552,7 +547,7 @@ bool longitudinal_brake_checks(int desired_brake, const LongitudinalLimits limit
return violation;
}

bool longitudinal_interceptor_checks(CANPacket_t *to_send) {
bool longitudinal_interceptor_checks(const CANPacket_t *to_send) {
return !get_longitudinal_allowed() && (GET_BYTE(to_send, 0) || GET_BYTE(to_send, 1));
}

Expand Down
5 changes: 3 additions & 2 deletions board/safety/safety_body.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -6,7 +6,7 @@ RxCheck body_rx_checks[] = {
{.msg = {{0x201, 0, 8, .check_checksum = false, .max_counter = 0U, .frequency = 100U}, { 0 }, { 0 }}},
};

static void body_rx_hook(CANPacket_t *to_push) {
static void body_rx_hook(const CANPacket_t *to_push) {
// body is never at standstill
vehicle_moving = true;

Expand All @@ -15,7 +15,7 @@ static void body_rx_hook(CANPacket_t *to_push) {
}
}

static bool body_tx_hook(CANPacket_t *to_send) {
static bool body_tx_hook(const CANPacket_t *to_send) {
bool tx = true;
int addr = GET_ADDR(to_send);
int len = GET_LEN(to_send);
Expand All @@ -42,6 +42,7 @@ const safety_hooks body_hooks = {
.init = body_init,
.rx = body_rx_hook,
.tx = body_tx_hook,
// rick - keep it for legacy support
.tx_lin = nooutput_tx_lin_hook,
.fwd = default_fwd_hook,
};
15 changes: 8 additions & 7 deletions board/safety/safety_chrysler.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -120,8 +120,8 @@ const CanMsg CHRYSLER_RAM_HD_TX_MSGS[] = {
RxCheck chrysler_rx_checks[] = {
{.msg = {{CHRYSLER_ADDRS_EPS_2, 0, 8, .check_checksum = true, .max_counter = 15U, .frequency = 100U}, { 0 }, { 0 }}},
{.msg = {{CHRYSLER_ADDRS_ESP_1, 0, 8, .check_checksum = true, .max_counter = 15U, .frequency = 50U}, { 0 }, { 0 }}},
//{.msg = {{ESP_8, 0, 8, .check_checksum = true, .max_counter = 15U, .frequency = 20000U}}},
{.msg = {{514, 0, 8, .check_checksum = false, .max_counter = 0U, .frequency = 10000U}, { 0 }, { 0 }}},
//{.msg = {{ESP_8, 0, 8, .check_checksum = true, .max_counter = 15U, .frequency = 50U}}},
{.msg = {{514, 0, 8, .check_checksum = false, .max_counter = 0U, .frequency = 100U}, { 0 }, { 0 }}},
{.msg = {{CHRYSLER_ADDRS_ECM_5, 0, 8, .check_checksum = true, .max_counter = 15U, .frequency = 50U}, { 0 }, { 0 }}},
{.msg = {{CHRYSLER_ADDRS_DAS_3, 0, 8, .check_checksum = true, .max_counter = 15U, .frequency = 50U}, { 0 }, { 0 }}},
};
Expand Down Expand Up @@ -154,12 +154,12 @@ enum {
} chrysler_platform = CHRYSLER_PACIFICA;
const ChryslerAddrs *chrysler_addrs = &CHRYSLER_ADDRS;

static uint32_t chrysler_get_checksum(CANPacket_t *to_push) {
static uint32_t chrysler_get_checksum(const CANPacket_t *to_push) {
int checksum_byte = GET_LEN(to_push) - 1U;
return (uint8_t)(GET_BYTE(to_push, checksum_byte));
}

static uint32_t chrysler_compute_checksum(CANPacket_t *to_push) {
static uint32_t chrysler_compute_checksum(const CANPacket_t *to_push) {
// TODO: clean this up
// http://illmatics.com/Remote%20Car%20Hacking.pdf
uint8_t checksum = 0xFFU;
Expand Down Expand Up @@ -192,11 +192,11 @@ static uint32_t chrysler_compute_checksum(CANPacket_t *to_push) {
return (uint8_t)(~checksum);
}

static uint8_t chrysler_get_counter(CANPacket_t *to_push) {
static uint8_t chrysler_get_counter(const CANPacket_t *to_push) {
return (uint8_t)(GET_BYTE(to_push, 6) >> 4);
}

static void chrysler_rx_hook(CANPacket_t *to_push) {
static void chrysler_rx_hook(const CANPacket_t *to_push) {
const int bus = GET_BUS(to_push);
const int addr = GET_ADDR(to_push);

Expand Down Expand Up @@ -237,7 +237,7 @@ static void chrysler_rx_hook(CANPacket_t *to_push) {
generic_rx_checks((bus == 0) && (addr == chrysler_addrs->LKAS_COMMAND));
}

static bool chrysler_tx_hook(CANPacket_t *to_send) {
static bool chrysler_tx_hook(const CANPacket_t *to_send) {
bool tx = true;
int addr = GET_ADDR(to_send);

Expand Down Expand Up @@ -310,6 +310,7 @@ const safety_hooks chrysler_hooks = {
.init = chrysler_init,
.rx = chrysler_rx_hook,
.tx = chrysler_tx_hook,
// rick - keep it for legacy support
.tx_lin = nooutput_tx_lin_hook,
.fwd = chrysler_fwd_hook,
.get_counter = chrysler_get_counter,
Expand Down
10 changes: 7 additions & 3 deletions board/safety/safety_defaults.h
View file
Original file line number Diff line number Diff line change
@@ -1,4 +1,4 @@
void default_rx_hook(CANPacket_t *to_push) {
void default_rx_hook(const CANPacket_t *to_push) {
UNUSED(to_push);
}

Expand All @@ -9,11 +9,12 @@ static safety_config nooutput_init(uint16_t param) {
return (safety_config){NULL, 0, NULL, 0};
}

static bool nooutput_tx_hook(CANPacket_t *to_send) {
static bool nooutput_tx_hook(const CANPacket_t *to_send) {
UNUSED(to_send);
return false;
}

// rick - keep it for legacy support
static bool nooutput_tx_lin_hook(int lin_num, uint8_t *data, int len) {
UNUSED(lin_num);
UNUSED(data);
Expand All @@ -31,6 +32,7 @@ const safety_hooks nooutput_hooks = {
.init = nooutput_init,
.rx = default_rx_hook,
.tx = nooutput_tx_hook,
// rick - keep it for legacy support
.tx_lin = nooutput_tx_lin_hook,
.fwd = default_fwd_hook,
};
Expand All @@ -47,11 +49,12 @@ static safety_config alloutput_init(uint16_t param) {
return (safety_config){NULL, 0, NULL, 0};
}

static bool alloutput_tx_hook(CANPacket_t *to_send) {
static bool alloutput_tx_hook(const CANPacket_t *to_send) {
UNUSED(to_send);
return true;
}

// rick - keep it for legacy support
static bool alloutput_tx_lin_hook(int lin_num, uint8_t *data, int len) {
UNUSED(lin_num);
UNUSED(data);
Expand Down Expand Up @@ -79,6 +82,7 @@ const safety_hooks alloutput_hooks = {
.init = alloutput_init,
.rx = default_rx_hook,
.tx = alloutput_tx_hook,
// rick - keep it for legacy support
.tx_lin = alloutput_tx_lin_hook,
.fwd = alloutput_fwd_hook,
};
4 changes: 3 additions & 1 deletion board/safety/safety_elm327.h
View file
Original file line number Diff line number Diff line change
@@ -1,4 +1,4 @@
static bool elm327_tx_hook(CANPacket_t *to_send) {
static bool elm327_tx_hook(const CANPacket_t *to_send) {
bool tx = true;
int addr = GET_ADDR(to_send);
int len = GET_LEN(to_send);
Expand All @@ -17,6 +17,7 @@ static bool elm327_tx_hook(CANPacket_t *to_send) {
return tx;
}

// rick - keep it for legacy support
static bool elm327_tx_lin_hook(int lin_num, uint8_t *data, int len) {
bool tx = true;
if (lin_num != 0) {
Expand All @@ -37,6 +38,7 @@ const safety_hooks elm327_hooks = {
.init = nooutput_init,
.rx = default_rx_hook,
.tx = elm327_tx_hook,
// rick - keep it for legacy support
.tx_lin = elm327_tx_lin_hook,
.fwd = default_fwd_hook,
};
Loading

0 comments on commit 1108478

Please sign in to comment.