sunnypilot v2026.002.000

version: sunnypilot v2026.002.000 (staging)
date: 2026-07-21T07:44:02
master commit: 2c334ede443d7391d27575af8c854a095ba702a8
This commit is contained in:
github-actions[bot] committed 2026-07-21 07:44:02 +00:00
commit de77c89cf6
3995 files changed
+1260688

No files matched your search

+15
View File
@@ -0,0 +1,15 @@
# constants from can.h
DLC_TO_LEN = [0, 1, 2, 3, 4, 5, 6, 7, 8, 12, 16, 20, 24, 32, 48, 64]
LEN_TO_DLC = {length: dlc for (dlc, length) in enumerate(DLC_TO_LEN)}
class ALTERNATIVE_EXPERIENCE:
DEFAULT = 0
DISABLE_STOCK_AEB = 2
RAISE_LONGITUDINAL_LIMITS_TO_ISO_MAX = 8
ALLOW_AEB = 16
# sunnypilot
ENABLE_MADS = 1024
MADS_DISENGAGE_LATERAL_ON_BRAKE = 2048
MADS_PAUSE_LATERAL_ON_BRAKE = 4096
+20
View File
@@ -0,0 +1,20 @@
#pragma once
static const unsigned char dlc_to_len[] = {0U, 1U, 2U, 3U, 4U, 5U, 6U, 7U, 8U, 12U, 16U, 20U, 24U, 32U, 48U, 64U};
#define CANPACKET_HEAD_SIZE 6U // non-data portion of CANPacket_t
#define CANPACKET_DATA_SIZE_MAX 64U
typedef struct {
unsigned char fd : 1;
unsigned char bus : 3;
unsigned char data_len_code : 4; // lookup length with dlc_to_len
unsigned char rejected : 1;
unsigned char returned : 1;
unsigned char extended : 1;
unsigned int addr : 29;
unsigned char checksum;
unsigned char data[CANPACKET_DATA_SIZE_MAX];
} __attribute__((packed, aligned(4))) CANPacket_t;
#define GET_LEN(msg) (dlc_to_len[(msg)->data_len_code])
+370
View File
@@ -0,0 +1,370 @@
#pragma once
#include <stdint.h>
#include <stdbool.h>
// from cereal.car.CarParams.SafetyModel
#define SAFETY_SILENT 0U
#define SAFETY_HONDA_NIDEC 1U
#define SAFETY_TOYOTA 2U
#define SAFETY_ELM327 3U
#define SAFETY_GM 4U
#define SAFETY_HONDA_BOSCH_GIRAFFE 5U
#define SAFETY_FORD 6U
#define SAFETY_HYUNDAI 8U
#define SAFETY_CHRYSLER 9U
#define SAFETY_TESLA 10U
#define SAFETY_SUBARU 11U
#define SAFETY_MAZDA 13U
#define SAFETY_NISSAN 14U
#define SAFETY_VOLKSWAGEN_MQB 15U
#define SAFETY_ALLOUTPUT 17U
#define SAFETY_GM_ASCM 18U
#define SAFETY_NOOUTPUT 19U
#define SAFETY_HONDA_BOSCH 20U
#define SAFETY_VOLKSWAGEN_PQ 21U
#define SAFETY_SUBARU_PREGLOBAL 22U
#define SAFETY_HYUNDAI_LEGACY 23U
#define SAFETY_HYUNDAI_COMMUNITY 24U
#define SAFETY_VOLKSWAGEN_MLB 25U
#define SAFETY_FAW 26U
#define SAFETY_BODY 27U
#define SAFETY_HYUNDAI_CANFD 28U
#define SAFETY_CHRYSLER_CUSW 30U
#define SAFETY_PSA 31U
#define SAFETY_RIVIAN 33U
#define SAFETY_VOLKSWAGEN_MEB 34U
#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])), \
(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).disable_forwarding = false; \
} while (0);
#define SET_TX_MSGS(tx, config) \
do { \
(config).tx_msgs = (tx); \
(config).tx_msgs_len = sizeof((tx)) / sizeof((tx)[0]); \
(config).disable_forwarding = false; \
} while (0);
#define UPDATE_VEHICLE_SPEED(val_ms) (update_sample(&vehicle_speed, ROUND((val_ms) * VEHICLE_SPEED_FACTOR)))
#define UPDATE_VEHICLE_SPEED_2(val_ms) (update_sample(&vehicle_speed_2, ROUND((val_ms) * VEHICLE_SPEED_FACTOR)))
uint32_t GET_BYTES(const CANPacket_t *msg, int start, int len);
extern const int MAX_WRONG_COUNTERS;
#define MAX_ADDR_CHECK_MSGS 3U
#define MAX_SAMPLE_VALS 6
// used to represent floating point vehicle speed in a sample_t
#define VEHICLE_SPEED_FACTOR 1000.0
#define MAX_RT_INTERVAL 250000U
// Conversions
#define KPH_TO_MS (1.0 / 3.6)
// sample struct that keeps 6 samples in memory
struct sample_t {
int values[MAX_SAMPLE_VALS];
int min;
int max;
};
// safety code requires floats
struct lookup_t {
float x[3];
float y[3];
};
typedef struct {
int addr;
unsigned int bus;
int len;
bool check_relay; // if true, trigger relay malfunction if existence on destination bus and block forwarding to destination bus
bool disable_static_blocking; // if true, static blocking is disabled so safety mode can dynamically handle it (e.g. selective AEB pass-through)
} CanMsg;
typedef enum {
TorqueMotorLimited, // torque steering command, limited by EPS output torque
TorqueDriverLimited, // torque steering command, limited by driver's input torque
} SteeringControlType;
typedef struct {
// torque cmd limits
const int max_torque; // this upper limit is always enforced
const bool dynamic_max_torque; // use max_torque_lookup to apply torque limit based on speed
const struct lookup_t max_torque_lookup;
const int max_rate_up;
const int max_rate_down;
const int max_rt_delta; // max change in torque per 250ms interval (MAX_RT_INTERVAL)
const SteeringControlType type;
// driver torque limits
const int driver_torque_allowance;
const int driver_torque_multiplier;
// motor torque limits
const int max_torque_error;
// safety around steer req bit
const int min_valid_request_frames;
const int max_invalid_request_frames;
const uint32_t min_valid_request_rt_interval;
const bool has_steer_req_tolerance;
} TorqueSteeringLimits;
typedef struct {
// angle cmd limits
const int max_angle;
const float angle_deg_to_can;
const struct lookup_t angle_rate_up_lookup;
const struct lookup_t angle_rate_down_lookup;
const uint32_t frequency; // Hz
} AngleSteeringLimits;
typedef struct {
// curvature cmd limits
const int max_curvature; // rad/m * curvature_to_can
const float curvature_to_can; // CAN units per rad/m
const uint32_t frequency; // Hz
const int max_curvature_error; // rad/m * curvature_to_can, max deviation from measured curvature (0 disables)
const float curvature_error_min_speed; // min speed for the curvature error check [m/s]
const int max_steer_power; // max steer power if EPS supports it (0 disables)
const bool inactive_curvature_is_zero; // true resets desired to 0 on violation, false resets to measured curvature
} CurvatureSteeringLimits;
// parameters for lateral accel/jerk angle limiting using a simple vehicle model
typedef struct {
const float slip_factor;
const float steer_ratio;
const float wheelbase;
} AngleSteeringParams;
typedef struct {
// acceleration cmd limits
const int max_accel;
const int min_accel;
const int inactive_accel;
// gas & brake cmd limits
// inactive and min gas are 0 on most safety modes
const int max_gas;
const int min_gas;
const int inactive_gas;
const int max_brake;
// transmission rpm limits
const int max_transmission_rpm;
const int min_transmission_rpm;
const int inactive_transmission_rpm;
// speed cmd limits
const int inactive_speed;
} LongitudinalLimits;
typedef struct {
const int addr;
const unsigned int bus;
const int len;
const uint32_t frequency; // expected frequency of the message [Hz]
const bool ignore_checksum; // checksum check is not performed when set to true
const bool ignore_counter; // counter check is not performed when set to true
const uint8_t max_counter; // maximum value of the counter. 0 means that the counter check is skipped
const bool ignore_quality_flag; // true if quality flag check is skipped
const bool ignore_frequency_check; // true if minimum frequency enforcement is skipped
} CanMsgCheck;
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
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
typedef struct {
const CanMsgCheck msg[MAX_ADDR_CHECK_MSGS]; // check either messages (e.g. honda steer)
RxStatus status;
} RxCheck;
typedef struct {
RxCheck *rx_checks;
int rx_checks_len;
const CanMsg *tx_msgs;
int tx_msgs_len;
bool disable_forwarding;
} safety_config;
typedef uint32_t (*get_checksum_t)(const CANPacket_t *msg);
typedef uint32_t (*compute_checksum_t)(const CANPacket_t *msg);
typedef uint8_t (*get_counter_t)(const CANPacket_t *msg);
typedef bool (*get_quality_flag_valid_t)(const CANPacket_t *msg);
typedef safety_config (*safety_hook_init)(uint16_t param);
typedef void (*rx_hook)(const CANPacket_t *msg);
typedef bool (*tx_hook)(const CANPacket_t *msg); // returns true if the message is allowed
typedef bool (*fwd_hook)(int bus_num, int addr); // returns true if the message should be blocked from forwarding
typedef struct {
safety_hook_init init;
rx_hook rx;
tx_hook tx;
fwd_hook fwd;
get_checksum_t get_checksum;
compute_checksum_t compute_checksum;
get_counter_t get_counter;
get_quality_flag_valid_t get_quality_flag_valid;
} safety_hooks;
bool safety_rx_hook(const CANPacket_t *msg);
bool safety_tx_hook(CANPacket_t *msg);
int to_signed(int d, int bits);
void update_sample(struct sample_t *sample, int sample_new);
bool get_longitudinal_allowed(void);
int ROUND(float val);
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 steer_torque_cmd_checks(int desired_torque, int steer_req, const TorqueSteeringLimits limits);
bool steer_angle_cmd_checks(int desired_angle, bool steer_control_enabled, const AngleSteeringLimits limits);
bool steer_angle_cmd_checks_vm(int desired_angle, bool steer_control_enabled, const AngleSteeringLimits limits,
const AngleSteeringParams params);
bool steer_curvature_cmd_checks(int desired_curvature, int steer_power, bool steer_control_enabled, const CurvatureSteeringLimits limits);
bool longitudinal_accel_checks(int desired_accel, const LongitudinalLimits limits);
bool longitudinal_speed_checks(int desired_speed, const LongitudinalLimits limits);
bool longitudinal_gas_checks(int desired_gas, const LongitudinalLimits limits);
bool longitudinal_transmission_rpm_checks(int desired_transmission_rpm, const LongitudinalLimits limits);
bool longitudinal_brake_checks(int desired_brake, const LongitudinalLimits limits);
bool longitudinal_interceptor_checks(const CANPacket_t *msg); // gas interceptor
void pcm_cruise_check(bool cruise_engaged);
void speed_mismatch_check(const float speed_2);
void safety_tick(const safety_config *safety_config);
// This can be set by the safety hooks
extern bool controls_allowed;
extern bool relay_malfunction;
extern bool gas_pressed;
extern bool gas_pressed_prev;
extern bool brake_pressed;
extern bool brake_pressed_prev;
extern bool regen_braking;
extern bool regen_braking_prev;
extern bool steering_disengage;
extern bool steering_disengage_prev;
extern bool cruise_engaged_prev;
extern struct sample_t vehicle_speed;
extern struct sample_t vehicle_speed_2;
extern bool vehicle_moving;
extern bool acc_main_on; // referred to as "ACC off" in ISO 15622:2018
extern int cruise_button_prev;
extern bool safety_rx_checks_invalid;
extern bool enable_gas_interceptor;
extern int gas_interceptor_prev;
// for safety modes with torque steering control
extern int desired_torque_last; // last desired steer torque
extern int rt_torque_last; // last desired torque for real time check
extern int valid_steer_req_count; // counter for steer request bit matching non-zero torque
extern int invalid_steer_req_count; // counter to allow multiple frames of mismatching torque request bit
extern struct sample_t torque_meas; // last 6 motor torques produced by the eps
extern struct sample_t torque_driver; // last 6 driver torques measured
extern uint32_t ts_torque_check_last;
extern uint32_t ts_steer_req_mismatch_last; // last timestamp steer req was mismatched with torque
// state for controls_allowed timeout logic
extern bool heartbeat_engaged; // openpilot enabled, passed in heartbeat USB command
extern uint32_t heartbeat_engaged_mismatches; // count of mismatches between heartbeat_engaged and controls_allowed
// for safety modes with angle steering control
extern uint32_t rt_angle_msgs;
extern uint32_t ts_angle_check_last;
extern int desired_angle_last;
extern struct sample_t angle_meas; // last 6 steer angles
// for safety modes with curvature steering control
typedef struct {
int desired_last;
uint32_t rt_msgs;
uint32_t rt_msgs_prev;
uint32_t ts_check_last;
int steer_power_last;
struct sample_t meas; // last 6 steer curvatures
} CurvatureSteeringState;
extern CurvatureSteeringState curvature_state;
// Alt experiences can be set with a USB command
// It enables features that allow alternative experiences, like not disengaging on gas press
// It is only either 0 or 1 on mainline comma.ai openpilot
//#define ALT_EXP_DISABLE_DISENGAGE_ON_GAS 1 // not used anymore, but reserved
// If using this flag, make sure to communicate to your users that a stock safety feature is now disabled.
#define ALT_EXP_DISABLE_STOCK_AEB 2
// If using this flag, be aware that harder braking is more likely to lead to rear endings,
// and that alone this flag doesn't make braking compliant because there's also a time element.
// Setting this flag is used for allowing the full -5.0 to +4.0 m/s^2 at lower speeds
// See ISO 15622:2018 for more information.
#define ALT_EXP_RAISE_LONGITUDINAL_LIMITS_TO_ISO_MAX 8
// This flag allows AEB to be commanded from openpilot.
#define ALT_EXP_ALLOW_AEB 16
extern int alternative_experience;
// time since safety mode has been changed
extern uint32_t safety_mode_cnt;
typedef struct {
uint16_t id;
const safety_hooks *hooks;
} safety_hook_config;
extern uint16_t current_safety_mode;
extern uint16_t current_safety_param;
extern uint16_t current_safety_param_sp;
extern safety_config current_safety_config;
int safety_fwd_hook(int bus_num, int addr);
int set_safety_hooks(uint16_t mode, uint16_t param);
extern const safety_hooks body_hooks;
extern const safety_hooks chrysler_hooks;
extern const safety_hooks chrysler_cusw_hooks;
extern const safety_hooks elm327_hooks;
extern const safety_hooks nooutput_hooks;
extern const safety_hooks alloutput_hooks;
extern const safety_hooks ford_hooks;
extern const safety_hooks gm_hooks;
extern const safety_hooks honda_nidec_hooks;
extern const safety_hooks honda_bosch_hooks;
extern const safety_hooks hyundai_canfd_hooks;
extern const safety_hooks hyundai_hooks;
extern const safety_hooks hyundai_legacy_hooks;
extern const safety_hooks mazda_hooks;
extern const safety_hooks nissan_hooks;
extern const safety_hooks subaru_hooks;
extern const safety_hooks subaru_preglobal_hooks;
extern const safety_hooks tesla_hooks;
extern const safety_hooks toyota_hooks;
extern const safety_hooks volkswagen_mlb_hooks;
extern const safety_hooks volkswagen_mqb_hooks;
extern const safety_hooks volkswagen_meb_hooks;
extern const safety_hooks volkswagen_pq_hooks;
extern const safety_hooks rivian_hooks;
extern const safety_hooks psa_hooks;
+72
View File
@@ -0,0 +1,72 @@
#include "opendbc/safety/declarations.h"
// cppcheck-suppress-macro misra-c2012-1.2; allow __typeof__ extension
// cppcheck-suppress-macro misra-c2012-17.3; suppress false implicit declaration alert on typeof extension
#define SAFETY_MIN(a, b) ({ \
__typeof__(a) _a = (a); \
__typeof__(b) _b = (b); \
(_a < _b) ? _a : _b; \
})
// cppcheck-suppress-macro misra-c2012-1.2; allow __typeof__ extension
// cppcheck-suppress-macro misra-c2012-17.3; suppress false implicit declaration alert on typeof extension
#define SAFETY_MAX(a, b) ({ \
__typeof__(a) _a = (a); \
__typeof__(b) _b = (b); \
(_a > _b) ? _a : _b; \
})
// cppcheck-suppress-macro misra-c2012-1.2; allow __typeof__ extension
// cppcheck-suppress-macro misra-c2012-17.3; suppress false implicit declaration alert on typeof extension
#define SAFETY_CLAMP(x, low, high) ({ \
__typeof__(x) __x = (x); \
__typeof__(low) __low = (low);\
__typeof__(high) __high = (high);\
(__x > __high) ? __high : ((__x < __low) ? __low : __x); \
})
// cppcheck-suppress-macro misra-c2012-1.2; allow __typeof__ extension
// cppcheck-suppress-macro misra-c2012-17.3; suppress false implicit declaration alert on typeof extension
#define SAFETY_ABS(a) ({ \
__typeof__(a) _a = (a); \
(_a > 0) ? _a : (-_a); \
})
#define SAFETY_UNUSED(x) ((void)(x))
// compute the time elapsed (in microseconds) from 2 counter samples
// case where ts < ts_last is ok: overflow is properly re-casted into uint32_t
static inline uint32_t safety_get_ts_elapsed(uint32_t ts, uint32_t ts_last) {
return ts - ts_last;
}
static bool safety_max_limit_check(int val, const int MAX_VAL, const int MIN_VAL) {
return (val > MAX_VAL) || (val < MIN_VAL);
}
// interp function that holds extreme values
static float safety_interpolate(struct lookup_t xy, float x) {
int size = sizeof(xy.x) / sizeof(xy.x[0]);
float ret = xy.y[size - 1]; // default output is last point
// x is lower than the first point in the x array. Return the first point
if (x <= xy.x[0]) {
ret = xy.y[0];
} else {
// find the index such that (xy.x[i] <= x < xy.x[i+1]) and linearly interp
for (int i=0; i < (size - 1); i++) {
if (x < xy.x[i+1]) {
float x0 = xy.x[i];
float y0 = xy.y[i];
float dx = xy.x[i+1] - x0;
float dy = xy.y[i+1] - y0;
// dx should not be zero as xy.x is supposed to be monotonic
dx = SAFETY_MAX(dx, 0.0001);
ret = (dy * (x - x0) / dx) + y0;
break;
}
}
}
return ret;
}
+77
View File
@@ -0,0 +1,77 @@
#pragma once
#include <stdbool.h>
#include <stdint.h>
#include "opendbc/safety/can.h"
bool ignition_can = false;
uint32_t ignition_can_cnt = 0U;
void ignition_can_hook(const CANPacket_t *msg) {
if (msg->bus == 0U) {
int len = GET_LEN(msg);
// GM exception
if ((msg->addr == 0x1F1U) && (len == 8)) {
// SystemPowerMode (2=Run, 3=Crank Request)
ignition_can = (msg->data[0] & 0x2U) != 0U;
ignition_can_cnt = 0U;
}
// Rivian R1S/T GEN1 exception
if ((msg->addr == 0x152U) && (len == 8)) {
// 0x152 overlaps with Subaru pre-global which has this bit as the high beam
int counter = msg->data[1] & 0xFU; // max is only 14
static int prev_counter_rivian = -1;
if ((counter == ((prev_counter_rivian + 1) % 15)) && (prev_counter_rivian != -1)) {
// VDM_OutputSignals->VDM_EpasPowerMode
ignition_can = ((msg->data[7] >> 4U) & 0x3U) == 1U; // VDM_EpasPowerMode_Drive_On=1
ignition_can_cnt = 0U;
}
prev_counter_rivian = counter;
}
// Tesla Model 3/Y exception
if ((msg->addr == 0x221U) && (len == 8)) {
// 0x221 overlaps with Rivian which has random data on byte 0
int counter = msg->data[6] >> 4;
static int prev_counter_tesla = -1;
if ((counter == ((prev_counter_tesla + 1) % 16)) && (prev_counter_tesla != -1)) {
// VCFRONT_LVPowerState->VCFRONT_vehiclePowerState
int power_state = (msg->data[0] >> 5U) & 0x3U;
ignition_can = power_state == 0x3; // VEHICLE_POWER_STATE_DRIVE=3
ignition_can_cnt = 0U;
}
prev_counter_tesla = counter;
}
// Mazda exception
if ((msg->addr == 0x9EU) && (len == 8)) {
ignition_can = (msg->data[0] >> 5) == 0x6U;
ignition_can_cnt = 0U;
}
// Volkswagen MEB exception
if ((msg->addr == 0x3C0U) && (len == 4)) {
int counter = msg->data[1] & 0xFU;
static int prev_counter_vw_meb = -1;
if ((counter == ((prev_counter_vw_meb + 1) % 16)) && (prev_counter_vw_meb != -1)) {
// Klemmen_Status_01->ZAS_Kl_15
ignition_can = ((msg->data[2] >> 1) & 1U) != 0U;
ignition_can_cnt = 0U;
}
prev_counter_vw_meb = counter;
}
}
// TODO: this is too loose, Teslas have 0x222
// body v2 exception
// if (((msg->bus == 0U) || (msg->bus == 2U)) && (msg->addr == 0x222U)) {
// ignition_can = true;
// ignition_can_cnt = 0U;
// }
}
+380
View File
@@ -0,0 +1,380 @@
#include "opendbc/safety/sunnypilot/mads.h"
#include "opendbc/safety/declarations.h"
// ISO 11270
static const float ISO_LATERAL_ACCEL = 3.0; // m/s^2
static const float EARTH_G = 9.81;
static const float AVERAGE_ROAD_ROLL = 0.06; // ~3.4 degrees, 6% superelevation
// check that commanded torque value isn't too far from measured
static bool dist_to_meas_check(int val, int val_last, struct sample_t *val_meas,
const int MAX_RATE_UP, const int MAX_RATE_DOWN, const int MAX_ERROR) {
// *** val rate limit check ***
int highest_allowed_rl = SAFETY_MAX(val_last, 0) + MAX_RATE_UP;
int lowest_allowed_rl = SAFETY_MIN(val_last, 0) - MAX_RATE_UP;
// if we've exceeded the meas val, we must start moving toward 0
int highest_allowed = SAFETY_MIN(highest_allowed_rl, SAFETY_MAX(val_last - MAX_RATE_DOWN, SAFETY_MAX(val_meas->max, 0) + MAX_ERROR));
int lowest_allowed = SAFETY_MAX(lowest_allowed_rl, SAFETY_MIN(val_last + MAX_RATE_DOWN, SAFETY_MIN(val_meas->min, 0) - MAX_ERROR));
// check for violation
return safety_max_limit_check(val, highest_allowed, lowest_allowed);
}
// check that commanded value isn't fighting against driver
static 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) {
// torque delta/rate limits
int highest_allowed_rl = SAFETY_MAX(val_last, 0) + MAX_RATE_UP;
int lowest_allowed_rl = SAFETY_MIN(val_last, 0) - MAX_RATE_UP;
// driver
int driver_max_limit = MAX_VAL + (MAX_ALLOWANCE + val_driver->max) * DRIVER_FACTOR;
int driver_min_limit = -MAX_VAL + (-MAX_ALLOWANCE + val_driver->min) * DRIVER_FACTOR;
// if we've exceeded the applied torque, we must start moving toward 0
int highest_allowed = SAFETY_MIN(highest_allowed_rl, SAFETY_MAX(val_last - MAX_RATE_DOWN,
SAFETY_MAX(driver_max_limit, 0)));
int lowest_allowed = SAFETY_MAX(lowest_allowed_rl, SAFETY_MIN(val_last + MAX_RATE_DOWN,
SAFETY_MIN(driver_min_limit, 0)));
// check for violation
return safety_max_limit_check(val, highest_allowed, lowest_allowed);
}
// real time check, mainly used for steer torque rate limiter
static bool rt_torque_rate_limit_check(int val, int val_last, const int MAX_RT_DELTA) {
// *** torque real time rate limit check ***
int highest_val = SAFETY_MAX(val_last, 0) + MAX_RT_DELTA;
int lowest_val = SAFETY_MIN(val_last, 0) - MAX_RT_DELTA;
// check for violation
return safety_max_limit_check(val, highest_val, lowest_val);
}
// Safety checks for torque-based steering commands
bool steer_torque_cmd_checks(int desired_torque, int steer_req, const TorqueSteeringLimits limits) {
bool violation = false;
uint32_t ts = microsecond_timer_get();
if (controls_allowed || controls_allowed_lateral) {
// Some safety models support variable torque limit based on vehicle speed
int max_torque = limits.max_torque;
if (limits.dynamic_max_torque) {
const float fudged_speed = (vehicle_speed.min / VEHICLE_SPEED_FACTOR) - 1.;
max_torque = safety_interpolate(limits.max_torque_lookup, fudged_speed) + 1;
max_torque = SAFETY_CLAMP(max_torque, -limits.max_torque, limits.max_torque);
}
// *** global torque limit check ***
violation |= safety_max_limit_check(desired_torque, max_torque, -max_torque);
// *** torque rate limit check ***
if (limits.type == TorqueDriverLimited) {
violation |= driver_limit_check(desired_torque, desired_torque_last, &torque_driver,
max_torque, limits.max_rate_up, limits.max_rate_down,
limits.driver_torque_allowance, limits.driver_torque_multiplier);
} else {
violation |= dist_to_meas_check(desired_torque, desired_torque_last, &torque_meas,
limits.max_rate_up, limits.max_rate_down, limits.max_torque_error);
}
desired_torque_last = desired_torque;
// *** torque real time rate limit check ***
violation |= rt_torque_rate_limit_check(desired_torque, rt_torque_last, limits.max_rt_delta);
// every RT_INTERVAL set the new limits
uint32_t ts_elapsed = safety_get_ts_elapsed(ts, ts_torque_check_last);
if (ts_elapsed > MAX_RT_INTERVAL) {
rt_torque_last = desired_torque;
ts_torque_check_last = ts;
}
}
// no torque if controls is not allowed
if (!(controls_allowed || controls_allowed_lateral) && (desired_torque != 0)) {
violation = true;
}
// certain safety modes set their steer request bit low for one or more frame at a
// predefined max frequency to avoid steering faults in certain situations
bool steer_req_mismatch = (steer_req == 0) && (desired_torque != 0);
if (!limits.has_steer_req_tolerance) {
if (steer_req_mismatch) {
violation = true;
}
} else {
if (steer_req_mismatch) {
if (invalid_steer_req_count == 0) {
// disallow torque cut if not enough recent matching steer_req messages
if (valid_steer_req_count < limits.min_valid_request_frames) {
violation = true;
}
// or we've cut torque too recently in time
uint32_t ts_elapsed = safety_get_ts_elapsed(ts, ts_steer_req_mismatch_last);
if (ts_elapsed < limits.min_valid_request_rt_interval) {
violation = true;
}
} else {
// or we're cutting more frames consecutively than allowed
if (invalid_steer_req_count >= limits.max_invalid_request_frames) {
violation = true;
}
}
valid_steer_req_count = 0;
ts_steer_req_mismatch_last = ts;
invalid_steer_req_count = SAFETY_MIN(invalid_steer_req_count + 1, limits.max_invalid_request_frames);
} else {
valid_steer_req_count = SAFETY_MIN(valid_steer_req_count + 1, limits.min_valid_request_frames);
invalid_steer_req_count = 0;
}
}
// reset to 0 if either controls is not allowed or there's a violation
if (violation || !(controls_allowed || controls_allowed_lateral)) {
valid_steer_req_count = 0;
invalid_steer_req_count = 0;
desired_torque_last = 0;
rt_torque_last = 0;
ts_torque_check_last = ts;
ts_steer_req_mismatch_last = ts;
}
return violation;
}
static bool rt_angle_rate_limit_check(AngleSteeringLimits limits) {
bool violation = false;
uint32_t ts = microsecond_timer_get();
// *** angle real time rate limit check ***
int max_rt_msgs = ((float)limits.frequency * MAX_RT_INTERVAL / 1e6 * 1.2) + 1; // 1.2x buffer
if ((int)rt_angle_msgs > max_rt_msgs) {
violation = true;
}
rt_angle_msgs += 1U;
// every RT_INTERVAL reset message counter
uint32_t ts_elapsed = safety_get_ts_elapsed(ts, ts_angle_check_last);
if (ts_elapsed >= MAX_RT_INTERVAL) {
rt_angle_msgs = 0;
ts_angle_check_last = ts;
}
return violation;
}
static bool rt_curvature_rate_limit_check(CurvatureSteeringLimits limits) {
bool violation = false;
uint32_t ts = microsecond_timer_get();
// *** curvature real time rate limit check ***
int max_rt_msgs = ((float)limits.frequency * MAX_RT_INTERVAL / 1e6 * 1.2) + 1; // 1.2x buffer
uint32_t rt_msgs = curvature_state.rt_msgs + curvature_state.rt_msgs_prev;
if ((int)rt_msgs > max_rt_msgs) {
violation = true;
}
curvature_state.rt_msgs += 1U;
//roll the window every half interval
if (safety_get_ts_elapsed(ts, curvature_state.ts_check_last) >= (MAX_RT_INTERVAL / 2U)) {
curvature_state.rt_msgs_prev = curvature_state.rt_msgs;
curvature_state.rt_msgs = 0U;
curvature_state.ts_check_last = ts;
}
return violation;
}
// Safety checks for angle-based steering commands
bool steer_angle_cmd_checks(int desired_angle, bool steer_control_enabled, const AngleSteeringLimits limits) {
bool violation = false;
if ((controls_allowed || controls_allowed_lateral) && steer_control_enabled) {
// convert floating point angle rate limits to integers in the scale of the desired angle on CAN,
// add 1 to not false trigger the violation. also fudge the speed by 1 m/s so rate limits are
// always slightly above openpilot's in case we read an updated speed in between angle commands
// TODO: this speed fudge can be much lower, look at data to determine the lowest reasonable offset
const float fudged_speed = (vehicle_speed.min / VEHICLE_SPEED_FACTOR) - 1.;
int delta_angle_up = (safety_interpolate(limits.angle_rate_up_lookup, fudged_speed) * limits.angle_deg_to_can) + 1.;
int delta_angle_down = (safety_interpolate(limits.angle_rate_down_lookup, fudged_speed) * limits.angle_deg_to_can) + 1.;
// allow down limits at zero since small floats from openpilot will be rounded to 0
// TODO: openpilot should be cognizant of this and not send small floats
int highest_desired_angle = desired_angle_last + ((desired_angle_last > 0) ? delta_angle_up : delta_angle_down);
int lowest_desired_angle = desired_angle_last - ((desired_angle_last >= 0) ? delta_angle_down : delta_angle_up);
// check for violation;
violation |= safety_max_limit_check(desired_angle, highest_desired_angle, lowest_desired_angle);
}
desired_angle_last = desired_angle;
// Angle should be close to current angle while not steering
if (!steer_control_enabled) {
const int max_inactive_angle = SAFETY_CLAMP(angle_meas.max, -limits.max_angle, limits.max_angle) + 1;
const int min_inactive_angle = SAFETY_CLAMP(angle_meas.min, -limits.max_angle, limits.max_angle) - 1;
violation |= safety_max_limit_check(desired_angle, max_inactive_angle, min_inactive_angle);
}
// No angle control allowed when controls are not allowed
if (!(controls_allowed || controls_allowed_lateral)) {
violation |= steer_control_enabled;
}
// reset to current angle if either controls is not allowed or there's a violation
if (violation || !(controls_allowed || controls_allowed_lateral)) {
desired_angle_last = SAFETY_CLAMP(angle_meas.values[0], -limits.max_angle, limits.max_angle);
}
return violation;
}
// Safety checks for curvature-based steering commands
bool steer_curvature_cmd_checks(int desired_curvature, int steer_power, bool steer_control_enabled, const CurvatureSteeringLimits limits) {
// Highway curves are rolled in the direction of the turn, add tolerance to compensate
static const float MAX_LATERAL_ACCEL = ISO_LATERAL_ACCEL + (EARTH_G * AVERAGE_ROAD_ROLL); // ~3.6 m/s^2
// Lower than ISO 11270 lateral jerk limit, which is 5.0 m/s^3
static const float MAX_LATERAL_JERK = 3.0 + (EARTH_G * AVERAGE_ROAD_ROLL); // ~3.6 m/s^3
const float fudged_speed = SAFETY_MAX((vehicle_speed.min / VEHICLE_SPEED_FACTOR) - 1.0, 1.0);
bool violation = false;
speed_mismatch_check((float)vehicle_speed_2.values[0] / VEHICLE_SPEED_FACTOR);
if ((controls_allowed || controls_allowed_lateral) && steer_control_enabled) {
// *** absolute curvature cap ***
violation |= safety_max_limit_check(desired_curvature, limits.max_curvature, -limits.max_curvature);
// *** ISO lateral jerk limit ***
const float max_curvature_rate_sec = MAX_LATERAL_JERK / (fudged_speed * fudged_speed);
const float max_curvature_delta = max_curvature_rate_sec / (float)limits.frequency;
const int max_curvature_delta_can = (max_curvature_delta * limits.curvature_to_can) + 1.;
const int highest_desired_curvature = curvature_state.desired_last + max_curvature_delta_can;
const int lowest_desired_curvature = curvature_state.desired_last - max_curvature_delta_can;
violation |= safety_max_limit_check(desired_curvature, highest_desired_curvature, lowest_desired_curvature);
// *** ISO lateral accel limit ***
const float max_curvature = MAX_LATERAL_ACCEL / (fudged_speed * fudged_speed);
const int max_curvature_can = (max_curvature * limits.curvature_to_can) + 1.;
violation |= safety_max_limit_check(desired_curvature, max_curvature_can, -max_curvature_can);
// *** curvature error from measured ***
if (limits.max_curvature_error && ((vehicle_speed.values[0] / VEHICLE_SPEED_FACTOR) > limits.curvature_error_min_speed)) {
const int lowest_desired_curvature_error = curvature_state.meas.min - limits.max_curvature_error - 1;
const int highest_desired_curvature_error = curvature_state.meas.max + limits.max_curvature_error + 1;
violation |= safety_max_limit_check(desired_curvature, highest_desired_curvature_error, lowest_desired_curvature_error);
}
// *** real time rate limit check ***
violation |= rt_curvature_rate_limit_check(limits);
}
curvature_state.desired_last = desired_curvature;
// Curvature must be 0 while not steering
if (!steer_control_enabled) {
violation |= desired_curvature != 0;
}
// *** steer power check ***
// allow power winddown after disengage to prevent EPS fault
if (limits.max_steer_power != 0) {
violation |= safety_max_limit_check(steer_power, limits.max_steer_power, 0);
violation |= (steer_power != 0) && !steer_control_enabled;
violation |= !(controls_allowed || controls_allowed_lateral) && (steer_power != 0) && (steer_power >= curvature_state.steer_power_last);
curvature_state.steer_power_last = steer_power;
} else {
// No curvature control allowed when controls are not allowed
violation |= !(controls_allowed || controls_allowed_lateral) && steer_control_enabled;
}
// reset to zero or measured curvature depending on EPS expectation
if (violation || !(controls_allowed || controls_allowed_lateral)) {
curvature_state.desired_last = limits.inactive_curvature_is_zero ? 0 : curvature_state.meas.values[0];
}
return violation;
}
static float get_curvature_factor(const float speed, const AngleSteeringParams params) {
// Matches VehicleModel.curvature_factor()
return 1. / (1. - (params.slip_factor * (speed * speed))) / params.wheelbase;
}
static float get_angle_from_curvature(const float curvature, const float curvature_factor, const AngleSteeringParams params) {
// Matches VehicleModel.get_steer_from_curvature()
static const float RAD_TO_DEG = 57.29577951308232;
return curvature * params.steer_ratio / curvature_factor * RAD_TO_DEG;
}
bool steer_angle_cmd_checks_vm(int desired_angle, bool steer_control_enabled, const AngleSteeringLimits limits,
const AngleSteeringParams params) {
// This check uses a simple vehicle model to allow for constant lateral acceleration and jerk limits across all speeds.
// TODO: remove the inaccurate breakpoint angle limiting function above and always use this one
// Highway curves are rolled in the direction of the turn, add tolerance to compensate
static const float MAX_LATERAL_ACCEL = ISO_LATERAL_ACCEL + (EARTH_G * AVERAGE_ROAD_ROLL); // ~3.6 m/s^2
// Lower than ISO 11270 lateral jerk limit, which is 5.0 m/s^3
static const float MAX_LATERAL_JERK = 3.0 + (EARTH_G * AVERAGE_ROAD_ROLL); // ~3.6 m/s^3
const float fudged_speed = SAFETY_MAX((vehicle_speed.min / VEHICLE_SPEED_FACTOR) - 1.0, 1.0);
const float curvature_factor = get_curvature_factor(fudged_speed, params);
bool violation = false;
if ((controls_allowed || controls_allowed_lateral) && steer_control_enabled) {
// *** ISO lateral jerk limit ***
// calculate maximum angle rate per second
const float max_curvature_rate_sec = MAX_LATERAL_JERK / (fudged_speed * fudged_speed);
const float max_angle_rate_sec = get_angle_from_curvature(max_curvature_rate_sec, curvature_factor, params);
// finally get max angle delta per frame
const float max_angle_delta = max_angle_rate_sec / (float)limits.frequency;
const int max_angle_delta_can = (max_angle_delta * limits.angle_deg_to_can) + 1.;
// NOTE: symmetric up and down limits
const int highest_desired_angle = desired_angle_last + max_angle_delta_can;
const int lowest_desired_angle = desired_angle_last - max_angle_delta_can;
violation |= safety_max_limit_check(desired_angle, highest_desired_angle, lowest_desired_angle);
// *** ISO lateral accel limit ***
const float max_curvature = MAX_LATERAL_ACCEL / (fudged_speed * fudged_speed);
const float max_angle = get_angle_from_curvature(max_curvature, curvature_factor, params);
const int max_angle_can = (max_angle * limits.angle_deg_to_can) + 1.;
violation |= safety_max_limit_check(desired_angle, max_angle_can, -max_angle_can);
// *** angle real time rate limit check ***
violation |= rt_angle_rate_limit_check(limits);
}
desired_angle_last = desired_angle;
// Angle should either be 0 or same as current angle while not steering
if (!steer_control_enabled) {
const int max_inactive_angle = SAFETY_CLAMP(angle_meas.max, -limits.max_angle, limits.max_angle) + 1;
const int min_inactive_angle = SAFETY_CLAMP(angle_meas.min, -limits.max_angle, limits.max_angle) - 1;
violation |= safety_max_limit_check(desired_angle, max_inactive_angle, min_inactive_angle);
}
// No angle control allowed when controls are not allowed
if (!(controls_allowed || controls_allowed_lateral)) {
violation |= steer_control_enabled;
}
// reset to current angle if either controls is not allowed or there's a violation
if (violation || !(controls_allowed || controls_allowed_lateral)) {
desired_angle_last = SAFETY_CLAMP(angle_meas.values[0], -limits.max_angle, limits.max_angle);
}
return violation;
}
@@ -0,0 +1,39 @@
#include "opendbc/safety/declarations.h"
bool get_longitudinal_allowed(void) {
return controls_allowed && !gas_pressed_prev;
}
// Safety checks for longitudinal actuation
bool longitudinal_accel_checks(int desired_accel, const LongitudinalLimits limits) {
bool accel_valid = get_longitudinal_allowed() && !safety_max_limit_check(desired_accel, limits.max_accel, limits.min_accel);
bool accel_inactive = desired_accel == limits.inactive_accel;
return !(accel_valid || accel_inactive);
}
bool longitudinal_speed_checks(int desired_speed, const LongitudinalLimits limits) {
return !get_longitudinal_allowed() && (desired_speed != limits.inactive_speed);
}
bool longitudinal_transmission_rpm_checks(int desired_transmission_rpm, const LongitudinalLimits limits) {
bool transmission_rpm_valid = get_longitudinal_allowed() && !safety_max_limit_check(desired_transmission_rpm, limits.max_transmission_rpm, limits.min_transmission_rpm);
bool transmission_rpm_inactive = desired_transmission_rpm == limits.inactive_transmission_rpm;
return !(transmission_rpm_valid || transmission_rpm_inactive);
}
bool longitudinal_gas_checks(int desired_gas, const LongitudinalLimits limits) {
bool gas_valid = get_longitudinal_allowed() && !safety_max_limit_check(desired_gas, limits.max_gas, limits.min_gas);
bool gas_inactive = desired_gas == limits.inactive_gas;
return !(gas_valid || gas_inactive);
}
bool longitudinal_brake_checks(int desired_brake, const LongitudinalLimits limits) {
bool violation = false;
violation |= !get_longitudinal_allowed() && (desired_brake != 0);
violation |= desired_brake > limits.max_brake;
return violation;
}
bool longitudinal_interceptor_checks(const CANPacket_t *msg) {
return !get_longitudinal_allowed() && (msg->data[0] || msg->data[1]);
}
+45
View File
@@ -0,0 +1,45 @@
#pragma once
#include "opendbc/safety/declarations.h"
static void body_rx_hook(const CANPacket_t *msg) {
if (msg->addr == 0x201U) {
controls_allowed = true;
}
}
static bool body_tx_hook(const CANPacket_t *msg) {
bool tx = true;
if (!controls_allowed && (msg->addr != 0x1U)) {
tx = false;
}
// Allow going into CAN flashing mode even if controls are not allowed
bool flash_msg = (msg->addr == 0x250U) && (GET_LEN(msg) == 8U);
if (!controls_allowed && (GET_BYTES(msg, 0, 4) == 0xdeadfaceU) && (GET_BYTES(msg, 4, 4) == 0x0ab00b1eU) && flash_msg) {
tx = true;
}
return tx;
}
static safety_config body_init(uint16_t param) {
static RxCheck body_rx_checks[] = {
{.msg = {{0x201, 0, 8, 100U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}},
};
static const CanMsg BODY_TX_MSGS[] = {{0x250, 0, 8, .check_relay = false}, {0x250, 0, 6, .check_relay = false}, {0x251, 0, 5, .check_relay = false}, // body
{0x1, 0, 8, .check_relay = false}}; // CAN flasher
SAFETY_UNUSED(param);
safety_config ret = BUILD_SAFETY_CFG(body_rx_checks, BODY_TX_MSGS);
ret.disable_forwarding = true;
return ret;
}
const safety_hooks body_hooks = {
.init = body_init,
.rx = body_rx_hook,
.tx = body_tx_hook,
};
@@ -0,0 +1,247 @@
#pragma once
#include "opendbc/safety/declarations.h"
#include "opendbc/safety/modes/chrysler_common.h"
// Chrysler Pacifica/Jeep addresses
#define CHRYSLER_EPS_2 0x220 // EPS driver input torque
#define CHRYSLER_ESP_1 0x140 // Brake pedal and vehicle speed
#define CHRYSLER_ESP_8 0x11C // Brake pedal and vehicle speed
#define CHRYSLER_ECM_5 0x22F // Throttle position sensor
#define CHRYSLER_DAS_3 0x1F4 // ACC engagement states from DASM
#define CHRYSLER_DAS_6 0x2A6 // LKAS HUD and auto headlight control from DASM
#define CHRYSLER_LKAS_COMMAND 0x292 // LKAS controls from DASM
#define CHRYSLER_CRUISE_BUTTONS 0x23B // Cruise control buttons
#define CHRYSLER_LKAS_HEARTBIT 0x2D9 // LKAS HEARTBIT from DASM
#define CHRYSLER_TRACTION_BUTTON 0x330 // Traction control button
#define CHRYSLER_Center_Stack_2 0x000 // Placeholder, does not exist
// RAM DT addresses
#define CHRYSLER_RAM_DT_EPS_2 0x31
#define CHRYSLER_RAM_DT_ESP_1 0x83
#define CHRYSLER_RAM_DT_ESP_8 0x79
#define CHRYSLER_RAM_DT_ECM_5 0x9D
#define CHRYSLER_RAM_DT_DAS_3 0x99
#define CHRYSLER_RAM_DT_DAS_6 0xFA
#define CHRYSLER_RAM_DT_LKAS_COMMAND 0xA6
#define CHRYSLER_RAM_DT_CRUISE_BUTTONS 0xB1
#define CHRYSLER_RAM_DT_LKAS_HEARTBIT 0x00 // Placeholder, does not exist
#define CHRYSLER_RAM_DT_TRACTION_BUTTON 0x00 // Placeholder, does not exist
#define CHRYSLER_RAM_DT_Center_Stack_2 0x28A
// RAM HD addresses
#define CHRYSLER_RAM_HD_EPS_2 0x220
#define CHRYSLER_RAM_HD_ESP_1 0x140
#define CHRYSLER_RAM_HD_ESP_8 0x11C
#define CHRYSLER_RAM_HD_ECM_5 0x22F
#define CHRYSLER_RAM_HD_DAS_3 0x1F4
#define CHRYSLER_RAM_HD_DAS_6 0x275
#define CHRYSLER_RAM_HD_LKAS_COMMAND 0x276
#define CHRYSLER_RAM_HD_CRUISE_BUTTONS 0x23A
#define CHRYSLER_RAM_HD_LKAS_HEARTBIT 0x00 // Placeholder, does not exist
#define CHRYSLER_RAM_HD_TRACTION_BUTTON 0x00 // Placeholder, does not exist
#define CHRYSLER_RAM_HD_Center_Stack_2 0x28A
typedef enum {
CHRYSLER_RAM_DT,
CHRYSLER_RAM_HD,
CHRYSLER_PACIFICA, // plus Jeep
} ChryslerPlatform;
static ChryslerPlatform chrysler_platform;
#define CHRYSLER_ADDR(name) ((uint32_t)((chrysler_platform == CHRYSLER_RAM_DT) ? CHRYSLER_RAM_DT_##name : \
((chrysler_platform == CHRYSLER_RAM_HD) ? CHRYSLER_RAM_HD_##name : CHRYSLER_##name)))
static uint8_t chrysler_get_counter(const CANPacket_t *msg) {
return (uint8_t)(msg->data[6] >> 4);
}
static void chrysler_rx_hook(const CANPacket_t *msg) {
// Measured EPS torque
if ((msg->bus == 0U) && (msg->addr == CHRYSLER_ADDR(EPS_2))) {
int torque_meas_new = ((msg->data[4] & 0x7U) << 8) + msg->data[5] - 1024U;
update_sample(&torque_meas, torque_meas_new);
}
// enter controls on rising edge of ACC, exit controls on ACC off
const unsigned int das_3_bus = (chrysler_platform == CHRYSLER_PACIFICA) ? 0U : 2U;
if ((msg->bus == das_3_bus) && (msg->addr == CHRYSLER_ADDR(DAS_3))) {
bool cruise_engaged = GET_BIT(msg, 21U);
pcm_cruise_check(cruise_engaged);
acc_main_on = GET_BIT(msg, 20U);
}
// TODO: use the same message for both
// update vehicle moving
if ((chrysler_platform != CHRYSLER_PACIFICA) && (msg->bus == 0U) && (msg->addr == CHRYSLER_ADDR(ESP_8))) {
vehicle_moving = ((msg->data[4] << 8) + msg->data[5]) != 0U;
}
if ((chrysler_platform == CHRYSLER_PACIFICA) && (msg->bus == 0U) && (msg->addr == 514U)) {
int speed_l = (msg->data[0] << 4) + (msg->data[1] >> 4);
int speed_r = (msg->data[2] << 4) + (msg->data[3] >> 4);
vehicle_moving = (speed_l != 0) || (speed_r != 0);
}
// exit controls on rising edge of gas press
if ((msg->bus == 0U) && (msg->addr == CHRYSLER_ADDR(ECM_5))) {
gas_pressed = msg->data[0U] != 0U;
}
// exit controls on rising edge of brake press
if ((msg->bus == 0U) && (msg->addr == CHRYSLER_ADDR(ESP_1))) {
brake_pressed = ((msg->data[0U] & 0xFU) >> 2U) == 1U;
}
if ((chrysler_platform == CHRYSLER_PACIFICA) && (msg->bus == 0U) && (msg->addr == CHRYSLER_ADDR(TRACTION_BUTTON))) {
mads_button_press = GET_BIT(msg, 53U) ? MADS_BUTTON_PRESSED : MADS_BUTTON_NOT_PRESSED;
}
if ((chrysler_platform != CHRYSLER_PACIFICA) && (msg->bus == 0U)) {
if (msg->addr == CHRYSLER_ADDR(Center_Stack_2)) {
mads_button_press = GET_BIT(msg, 57U) ? MADS_BUTTON_PRESSED : MADS_BUTTON_NOT_PRESSED;
}
}
}
static bool chrysler_tx_hook(const CANPacket_t *msg) {
const TorqueSteeringLimits CHRYSLER_STEERING_LIMITS = {
.max_torque = 261,
.max_rt_delta = 112,
.max_rate_up = 3,
.max_rate_down = 3,
.max_torque_error = 80,
.type = TorqueMotorLimited,
};
const TorqueSteeringLimits CHRYSLER_RAM_DT_STEERING_LIMITS = {
.max_torque = 350,
.max_rt_delta = 112,
.max_rate_up = 6,
.max_rate_down = 6,
.max_torque_error = 80,
.type = TorqueMotorLimited,
};
const TorqueSteeringLimits CHRYSLER_RAM_HD_STEERING_LIMITS = {
.max_torque = 361,
.max_rt_delta = 182,
.max_rate_up = 14,
.max_rate_down = 14,
.max_torque_error = 80,
.type = TorqueMotorLimited,
};
bool tx = true;
// STEERING
if (msg->addr == CHRYSLER_ADDR(LKAS_COMMAND)) {
int start_byte = (chrysler_platform == CHRYSLER_PACIFICA) ? 0 : 1;
int desired_torque = ((msg->data[start_byte] & 0x7U) << 8) | msg->data[start_byte + 1];
desired_torque -= 1024;
const TorqueSteeringLimits limits = (chrysler_platform == CHRYSLER_PACIFICA) ? CHRYSLER_STEERING_LIMITS :
(chrysler_platform == CHRYSLER_RAM_DT) ? CHRYSLER_RAM_DT_STEERING_LIMITS : CHRYSLER_RAM_HD_STEERING_LIMITS;
bool steer_req = (chrysler_platform == CHRYSLER_PACIFICA) ? GET_BIT(msg, 4U) : (msg->data[3] & 0x7U) == 2U;
if (steer_torque_cmd_checks(desired_torque, steer_req, limits)) {
tx = false;
}
}
// FORCE CANCEL: only the cancel button press is allowed
if (msg->addr == CHRYSLER_ADDR(CRUISE_BUTTONS)) {
const bool is_cancel = msg->data[0] == 1U;
const bool is_accel = msg->data[0] == 0x04U;
const bool is_decel = msg->data[0] == 0x08U;
const bool is_resume = msg->data[0] == 0x10U;
const bool allowed = is_cancel || ((is_resume || is_accel || is_decel) && controls_allowed);
if (!allowed) {
tx = false;
}
}
return tx;
}
static safety_config chrysler_init(uint16_t param) {
const uint32_t CHRYSLER_PARAM_RAM_DT = 1U; // set for Ram DT platform
static RxCheck chrysler_ram_dt_rx_checks[] = {
{.msg = {{CHRYSLER_RAM_DT_EPS_2, 0, 8, 100U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}},
{.msg = {{CHRYSLER_RAM_DT_ESP_1, 0, 8, 50U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}},
{.msg = {{CHRYSLER_RAM_DT_ESP_8, 0, 8, 50U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}},
{.msg = {{CHRYSLER_RAM_DT_ECM_5, 0, 8, 50U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}},
{.msg = {{CHRYSLER_RAM_DT_DAS_3, 2, 8, 50U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}},
{.msg = {{CHRYSLER_RAM_DT_Center_Stack_2, 0, 8, 1U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true, .ignore_frequency_check = true}, { 0 }, { 0 }}},
};
static RxCheck chrysler_rx_checks[] = {
{.msg = {{CHRYSLER_EPS_2, 0, 8, 100U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}},
{.msg = {{CHRYSLER_ESP_1, 0, 8, 50U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}},
{.msg = {{514, 0, 8, 100U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}},
{.msg = {{CHRYSLER_ECM_5, 0, 8, 50U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}},
{.msg = {{CHRYSLER_DAS_3, 0, 8, 50U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}},
{.msg = {{CHRYSLER_TRACTION_BUTTON, 0, 8, 1U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true, .ignore_frequency_check = true}, { 0 }, { 0 }}},
};
static const CanMsg CHRYSLER_TX_MSGS[] = {
{CHRYSLER_CRUISE_BUTTONS, 0, 3, .check_relay = false},
{CHRYSLER_LKAS_COMMAND, 0, 6, .check_relay = true},
{CHRYSLER_DAS_6, 0, 8, .check_relay = true},
{CHRYSLER_LKAS_HEARTBIT, 0, 5, .check_relay = true},
};
static const CanMsg CHRYSLER_RAM_DT_TX_MSGS[] = {
{CHRYSLER_RAM_DT_CRUISE_BUTTONS, 2, 3, .check_relay = false},
{CHRYSLER_RAM_DT_LKAS_COMMAND, 0, 8, .check_relay = true},
{CHRYSLER_RAM_DT_DAS_6, 0, 8, .check_relay = true},
};
#ifdef ALLOW_DEBUG
static RxCheck chrysler_ram_hd_rx_checks[] = {
{.msg = {{CHRYSLER_RAM_HD_EPS_2, 0, 8, 100U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}},
{.msg = {{CHRYSLER_RAM_HD_ESP_1, 0, 8, 50U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}},
{.msg = {{CHRYSLER_RAM_HD_ESP_8, 0, 8, 50U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}},
{.msg = {{CHRYSLER_RAM_HD_ECM_5, 0, 8, 50U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}},
{.msg = {{CHRYSLER_RAM_HD_DAS_3, 2, 8, 50U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}},
{.msg = {{CHRYSLER_RAM_HD_Center_Stack_2, 0, 8, 1U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true, .ignore_frequency_check = true}, { 0 }, { 0 }}},
};
static const CanMsg CHRYSLER_RAM_HD_TX_MSGS[] = {
{CHRYSLER_RAM_HD_CRUISE_BUTTONS, 2, 3, .check_relay = false},
{CHRYSLER_RAM_HD_LKAS_COMMAND, 0, 8, .check_relay = true},
{CHRYSLER_RAM_HD_DAS_6, 0, 8, .check_relay = true},
};
const uint32_t CHRYSLER_PARAM_RAM_HD = 2U; // set for Ram HD platform
bool enable_ram_hd = GET_FLAG(param, CHRYSLER_PARAM_RAM_HD);
#endif
safety_config ret;
bool enable_ram_dt = GET_FLAG(param, CHRYSLER_PARAM_RAM_DT);
if (enable_ram_dt) {
chrysler_platform = CHRYSLER_RAM_DT;
ret = BUILD_SAFETY_CFG(chrysler_ram_dt_rx_checks, CHRYSLER_RAM_DT_TX_MSGS);
#ifdef ALLOW_DEBUG
} else if (enable_ram_hd) {
chrysler_platform = CHRYSLER_RAM_HD;
ret = BUILD_SAFETY_CFG(chrysler_ram_hd_rx_checks, CHRYSLER_RAM_HD_TX_MSGS);
#endif
} else {
chrysler_platform = CHRYSLER_PACIFICA;
ret = BUILD_SAFETY_CFG(chrysler_rx_checks, CHRYSLER_TX_MSGS);
}
return ret;
}
const safety_hooks chrysler_hooks = {
.init = chrysler_init,
.rx = chrysler_rx_hook,
.tx = chrysler_tx_hook,
.get_counter = chrysler_get_counter,
.get_checksum = chrysler_get_checksum,
.compute_checksum = chrysler_compute_checksum,
};
@@ -0,0 +1,39 @@
#pragma once
static uint32_t chrysler_get_checksum(const CANPacket_t *msg) {
int checksum_byte = GET_LEN(msg) - 1U;
return (uint8_t)(msg->data[checksum_byte]);
}
static uint32_t chrysler_compute_checksum(const CANPacket_t *msg) {
// TODO: clean this up
// http://illmatics.com/Remote%20Car%20Hacking.pdf
uint8_t checksum = 0xFFU;
int len = GET_LEN(msg);
for (int j = 0; j < (len - 1); j++) {
uint8_t shift = 0x80U;
uint8_t curr = (uint8_t)msg->data[j];
for (int i=0; i<8; i++) {
uint8_t bit_sum = curr & shift;
uint8_t temp_chk = checksum & 0x80U;
if (bit_sum != 0U) {
bit_sum = 0x1C;
if (temp_chk != 0U) {
bit_sum = 1;
}
checksum = checksum << 1;
temp_chk = checksum | 1U;
bit_sum ^= temp_chk;
} else {
if (temp_chk != 0U) {
bit_sum = 0x1D;
}
checksum = checksum << 1;
bit_sum ^= checksum;
}
checksum = bit_sum;
shift = shift >> 1;
}
}
return (uint8_t)(~checksum);
}
@@ -0,0 +1,107 @@
#include "opendbc/safety/modes/chrysler_common.h"
static safety_config chrysler_cusw_init(uint16_t param) {
SAFETY_UNUSED(param);
static const CanMsg CHRYSLER_CUSW_TX_MSGS[] = {
{0x1F6U, 0, 4, .check_relay = true},
{0x5DCU, 0, 4, .check_relay = true},
{0x2FAU, 0, 3, .check_relay = false},
};
static RxCheck chrysler_cusw_rx_checks[] = {
{.msg = {{0x1E4U, 0, 8, 50U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}},
{.msg = {{0x1E8U, 0, 5, 50U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}},
{.msg = {{0x1ECU, 0, 8, 100U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}},
{.msg = {{0x1FEU, 0, 5, 50U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}},
{.msg = {{0x2ECU, 0, 8, 50U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}},
};
return BUILD_SAFETY_CFG(chrysler_cusw_rx_checks, CHRYSLER_CUSW_TX_MSGS);
}
static void chrysler_cusw_rx_hook(const CANPacket_t *msg) {
if (msg->bus == 0U) {
if (msg->addr == 0x1ECU) {
// Signal: EPS_STATUS.TORQUE_MOTOR
int torque_meas_new = ((msg->data[3] & 0xFU) << 8) + msg->data[4] - 2048U;
update_sample(&torque_meas, torque_meas_new);
}
if (msg->addr == 0x2ECU) {
// Signal: ACC_CONTROL.ACC_ACTIVE
bool cruise_engaged = GET_BIT(msg, 7U);
pcm_cruise_check(cruise_engaged);
}
if (msg->addr == 0x1E4U) {
// Signal: BRAKE_1.VEHICLE_SPEED
vehicle_moving = (((msg->data[4] & 0x7U) << 8) + msg->data[5]) != 0U;
}
if (msg->addr == 0x1FEU) {
// Signal: ACCEL_GAS.GAS_HUMAN
gas_pressed = msg->data[1] != 0U;
}
if (msg->addr == 0x1E8U) {
// Signal: BRAKE_3.DRIVER_BRAKE_SWITCH
brake_pressed = GET_BIT(msg, 18U);
}
}
}
static bool chrysler_cusw_tx_hook(const CANPacket_t *msg) {
const TorqueSteeringLimits CHRYSLER_CUSW_STEERING_LIMITS = {
.max_torque = 250, // TODO: Find the actual cap, think we're faulting before 261
.max_rt_delta = 150,
.max_rate_up = 4,
.max_rate_down = 4,
.max_torque_error = 80,
.type = TorqueMotorLimited,
};
bool tx = true;
if (msg->addr == 0x1F6U) {
// Signal: LKAS_COMMAND.STEERING_TORQUE
int desired_torque = ((msg->data[0]) << 3) | ((msg->data[1] & 0xE0U) >> 5);
desired_torque -= 1024;
// Signal: LKAS_COMMAND.LKAS_CONTROL_BIT
const bool steer_req = GET_BIT(msg, 12U);
if (steer_torque_cmd_checks(desired_torque, steer_req, CHRYSLER_CUSW_STEERING_LIMITS)) {
tx = false;
// FIXME: too many problems here right now, hotwire things while investigating
// tx = true;
}
}
if (msg->addr == 0x2FAU) {
// Signal: CRUISE_BUTTONS.ACC_Cancel
// Signal: CRUISE_BUTTONS.ACC_Resume
const bool is_cancel = GET_BIT(msg, 0U);
const bool is_resume = GET_BIT(msg, 4U);
const bool allowed = is_cancel || (is_resume && controls_allowed);
if (!allowed) {
tx = false;
}
}
return tx;
}
static uint8_t chrysler_cusw_get_counter(const CANPacket_t *msg) {
int counter_byte = GET_LEN(msg) - 2U;
return (uint8_t)(msg->data[counter_byte] & 0xFU);
}
const safety_hooks chrysler_cusw_hooks = {
.init = chrysler_cusw_init,
.rx = chrysler_cusw_rx_hook,
.tx = chrysler_cusw_tx_hook,
.get_counter = chrysler_cusw_get_counter,
.get_checksum = chrysler_get_checksum,
.compute_checksum = chrysler_compute_checksum,
};
@@ -0,0 +1,51 @@
#pragma once
#include "opendbc/safety/declarations.h"
// GCOV_EXCL_START
// Unreachable by design (doesn't define any rx msgs)
void default_rx_hook(const CANPacket_t *msg) {
SAFETY_UNUSED(msg);
}
// GCOV_EXCL_STOP
// *** no output safety mode ***
static safety_config nooutput_init(uint16_t param) {
SAFETY_UNUSED(param);
return (safety_config){NULL, 0, NULL, 0, true}; // NOLINT(readability/braces)
}
// GCOV_EXCL_START
// Unreachable by design (doesn't define any tx msgs)
static bool nooutput_tx_hook(const CANPacket_t *msg) {
SAFETY_UNUSED(msg);
return false;
}
// GCOV_EXCL_STOP
const safety_hooks nooutput_hooks = {
.init = nooutput_init,
.rx = default_rx_hook,
.tx = nooutput_tx_hook,
};
// *** all output safety mode ***
static safety_config alloutput_init(uint16_t param) {
// Enables passthrough mode where relay is open and bus 0 gets forwarded to bus 2 and vice versa
const uint16_t ALLOUTPUT_PARAM_PASSTHROUGH = 1;
controls_allowed = true;
bool alloutput_passthrough = GET_FLAG(param, ALLOUTPUT_PARAM_PASSTHROUGH);
return (safety_config){NULL, 0, NULL, 0, !alloutput_passthrough}; // NOLINT(readability/braces)
}
static bool alloutput_tx_hook(const CANPacket_t *msg) {
SAFETY_UNUSED(msg);
return true;
}
const safety_hooks alloutput_hooks = {
.init = alloutput_init,
.rx = default_rx_hook,
.tx = alloutput_tx_hook,
};
@@ -0,0 +1,40 @@
#pragma once
#include "opendbc/safety/declarations.h"
#include "opendbc/safety/modes/defaults.h"
static bool elm327_tx_hook(const CANPacket_t *msg) {
const unsigned int GM_CAMERA_DIAG_ADDR = 0x24BU;
bool tx = true;
int len = GET_LEN(msg);
// All ISO 15765-4 messages must be 8 bytes long
if (len != 8) {
tx = false;
}
// Check valid 29 bit send addresses for ISO 15765-4
// Check valid 11 bit send addresses for ISO 15765-4
if ((msg->addr != 0x18DB33F1U) && ((msg->addr & 0x1FFF00FFU) != 0x18DA00F1U) &&
((msg->addr & 0x1FFFFF00U) != 0x600U) && ((msg->addr & 0x1FFFFF00U) != 0x700U) &&
(msg->addr != GM_CAMERA_DIAG_ADDR)) {
tx = false;
}
// GM camera uses non-standard diagnostic address, this has no control message address collisions
if ((msg->addr == GM_CAMERA_DIAG_ADDR) && (len == 8)) {
// Only allow known frame types for ISO 15765-2
if ((msg->data[0] & 0xF0U) > 0x30U) {
tx = false;
}
}
return tx;
}
// If safety_param == 0, bus 1 is multiplexed to the OBD-II port
const safety_hooks elm327_hooks = {
.init = nooutput_init,
.rx = default_rx_hook,
.tx = elm327_tx_hook,
};
+343
View File
@@ -0,0 +1,343 @@
#pragma once
#include "opendbc/safety/declarations.h"
// Safety-relevant CAN messages for Ford vehicles.
#define FORD_EngBrakeData 0x165U // RX from PCM, for driver brake pedal and cruise state
#define FORD_EngVehicleSpThrottle 0x204U // RX from PCM, for driver throttle input
#define FORD_DesiredTorqBrk 0x213U // RX from ABS, for standstill state
#define FORD_BrakeSysFeatures 0x415U // RX from ABS, for vehicle speed
#define FORD_EngVehicleSpThrottle2 0x202U // RX from PCM, for second vehicle speed
#define FORD_Yaw_Data_FD1 0x91U // RX from RCM, for yaw rate
#define FORD_Steering_Data_FD1 0x083U // TX by OP, various driver switches and LKAS/CC buttons
#define FORD_ACCDATA 0x186U // TX by OP, ACC controls
#define FORD_ACCDATA_3 0x18AU // TX by OP, ACC/TJA user interface
#define FORD_Lane_Assist_Data1 0x3CAU // TX by OP, Lane Keep Assist
#define FORD_LateralMotionControl 0x3D3U // TX by OP, Lateral Control message
#define FORD_LateralMotionControl2 0x3D6U // TX by OP, alternate Lateral Control message
#define FORD_IPMA_Data 0x3D8U // TX by OP, IPMA and LKAS user interface
// CAN bus numbers.
#define FORD_MAIN_BUS 0U
#define FORD_CAM_BUS 2U
static uint8_t ford_get_counter(const CANPacket_t *msg) {
uint8_t cnt = 0;
if (msg->addr == FORD_BrakeSysFeatures) {
// Signal: VehVActlBrk_No_Cnt
cnt = (msg->data[2] >> 2) & 0xFU;
} else if (msg->addr == FORD_Yaw_Data_FD1) {
// Signal: VehRollYaw_No_Cnt
cnt = msg->data[5];
} else {
}
return cnt;
}
static uint32_t ford_get_checksum(const CANPacket_t *msg) {
uint8_t chksum = 0;
if (msg->addr == FORD_BrakeSysFeatures) {
// Signal: VehVActlBrk_No_Cs
chksum = msg->data[3];
} else if (msg->addr == FORD_Yaw_Data_FD1) {
// Signal: VehRollYawW_No_Cs
chksum = msg->data[4];
} else {
}
return chksum;
}
static uint32_t ford_compute_checksum(const CANPacket_t *msg) {
uint8_t chksum = 0;
if (msg->addr == FORD_BrakeSysFeatures) {
chksum += msg->data[0] + msg->data[1]; // Veh_V_ActlBrk
chksum += msg->data[2] >> 6; // VehVActlBrk_D_Qf
chksum += (msg->data[2] >> 2) & 0xFU; // VehVActlBrk_No_Cnt
chksum = 0xFFU - chksum;
} else if (msg->addr == FORD_Yaw_Data_FD1) {
chksum += msg->data[0] + msg->data[1]; // VehRol_W_Actl
chksum += msg->data[2] + msg->data[3]; // VehYaw_W_Actl
chksum += msg->data[5]; // VehRollYaw_No_Cnt
chksum += msg->data[6] >> 6; // VehRolWActl_D_Qf
chksum += (msg->data[6] >> 4) & 0x3U; // VehYawWActl_D_Qf
chksum = 0xFFU - chksum;
} else {
}
return chksum;
}
static bool ford_get_quality_flag_valid(const CANPacket_t *msg) {
bool valid = false;
if (msg->addr == FORD_BrakeSysFeatures) {
valid = (msg->data[2] >> 6) == 0x3U; // VehVActlBrk_D_Qf
} else if (msg->addr == FORD_EngVehicleSpThrottle2) {
valid = ((msg->data[4] >> 5) & 0x3U) == 0x3U; // VehVActlEng_D_Qf
} else if (msg->addr == FORD_Yaw_Data_FD1) {
valid = ((msg->data[6] >> 4) & 0x3U) == 0x3U; // VehYawWActl_D_Qf
} else {
}
return valid;
}
#define FORD_INACTIVE_CURVATURE 1000U
#define FORD_INACTIVE_CURVATURE_RATE 4096U
#define FORD_INACTIVE_PATH_OFFSET 512U
#define FORD_INACTIVE_PATH_ANGLE 1000U
#define FORD_CANFD_INACTIVE_CURVATURE_RATE 1024U
static const CurvatureSteeringLimits FORD_STEERING_LIMITS = {
.max_curvature = 1000, // 0.02 rad/m * curvature_to_can
.curvature_to_can = 50000, // CAN units per rad/m
.frequency = 20, // Hz
.max_curvature_error = 100, // 0.002 rad/m * curvature_to_can
.curvature_error_min_speed = 10.0, // m/s
.max_steer_power = 0, // disabled, Ford has no steed power signal
.inactive_curvature_is_zero = true, // Ford EPS expects curvature=0 when inactive
};
static void ford_rx_hook(const CANPacket_t *msg) {
if (msg->bus == FORD_MAIN_BUS) {
// Update in motion state from standstill signal
if (msg->addr == FORD_DesiredTorqBrk) {
// Signal: VehStop_D_Stat
vehicle_moving = ((msg->data[3] >> 3) & 0x3U) != 1U;
}
// Update vehicle speed
if (msg->addr == FORD_BrakeSysFeatures) {
// Signal: Veh_V_ActlBrk
UPDATE_VEHICLE_SPEED(((msg->data[0] << 8) | msg->data[1]) * 0.01 * KPH_TO_MS);
}
// Check vehicle speed against a second source
if (msg->addr == FORD_EngVehicleSpThrottle2) {
// Disable controls if speeds from ABS and PCM ECUs are too far apart.
// Signal: Veh_V_ActlEng
float filtered_pcm_speed = ((msg->data[6] << 8) | msg->data[7]) * 0.01 * KPH_TO_MS;
UPDATE_VEHICLE_SPEED_2(filtered_pcm_speed);
}
// Update vehicle yaw rate
if (msg->addr == FORD_Yaw_Data_FD1) {
// Signal: VehYaw_W_Actl
// TODO: we should use the speed which results in the closest angle measurement to the desired angle
float ford_yaw_rate = (((msg->data[2] << 8U) | msg->data[3]) * 0.0002) - 6.5;
float current_curvature = ford_yaw_rate / SAFETY_MAX(vehicle_speed.values[0] / VEHICLE_SPEED_FACTOR, 0.1);
// convert current curvature into units on CAN for comparison with desired curvature
update_sample(&curvature_state.meas, ROUND(current_curvature * FORD_STEERING_LIMITS.curvature_to_can));
}
// Update gas pedal
if (msg->addr == FORD_EngVehicleSpThrottle) {
// Pedal position: (0.1 * val) in percent
// Signal: ApedPos_Pc_ActlArb
gas_pressed = (((msg->data[0] & 0x03U) << 8) | msg->data[1]) > 0U;
}
// Update brake pedal and cruise state
if (msg->addr == FORD_EngBrakeData) {
// Signal: BpedDrvAppl_D_Actl
brake_pressed = ((msg->data[0] >> 4) & 0x3U) == 2U;
// Signal: CcStat_D_Actl
unsigned int cruise_state = msg->data[1] & 0x07U;
bool cruise_engaged = (cruise_state == 4U) || (cruise_state == 5U);
pcm_cruise_check(cruise_engaged);
acc_main_on = (cruise_state == 3U) || cruise_engaged;
}
if (msg->addr == FORD_Steering_Data_FD1) {
mads_button_press = GET_BIT(msg, 40U) ? MADS_BUTTON_PRESSED : MADS_BUTTON_NOT_PRESSED;
}
}
}
static bool ford_tx_hook(const CANPacket_t *msg) {
const LongitudinalLimits FORD_LONG_LIMITS = {
// acceleration cmd limits (used for brakes)
// Signal: AccBrkTot_A_Rq
.max_accel = 5641, // 1.9999 m/s^s
.min_accel = 4231, // -3.4991 m/s^2
.inactive_accel = 5128, // -0.0008 m/s^2
// gas cmd limits
// Signal: AccPrpl_A_Rq & AccPrpl_A_Pred
.max_gas = 700, // 2.0 m/s^2
.min_gas = 450, // -0.5 m/s^2
.inactive_gas = 0, // -5.0 m/s^2
};
bool tx = true;
// Safety check for ACCDATA accel and brake requests
if (msg->addr == FORD_ACCDATA) {
// Signal: AccPrpl_A_Rq
int gas = ((msg->data[6] & 0x3U) << 8) | msg->data[7];
// Signal: AccPrpl_A_Pred
int gas_pred = ((msg->data[2] & 0x3U) << 8) | msg->data[3];
// Signal: AccBrkTot_A_Rq
int accel = ((msg->data[0] & 0x1FU) << 8) | msg->data[1];
// Signal: CmbbDeny_B_Actl
bool cmbb_deny = (msg->data[4] >> 5) & 1U;
// Signal: AccBrkPrchg_B_Rq & AccBrkDecel_B_Rq
bool brake_actuation = ((msg->data[6] >> 6) & 1U) || ((msg->data[6] >> 7) & 1U);
bool violation = false;
violation |= longitudinal_accel_checks(accel, FORD_LONG_LIMITS);
violation |= longitudinal_gas_checks(gas, FORD_LONG_LIMITS);
violation |= longitudinal_gas_checks(gas_pred, FORD_LONG_LIMITS);
// Safety check for stock AEB
violation |= cmbb_deny; // do not prevent stock AEB actuation
violation |= !get_longitudinal_allowed() && brake_actuation;
if (violation) {
tx = false;
}
}
// Safety check for Steering_Data_FD1 button signals
// Note: Many other signals in this message are not relevant to safety (e.g. blinkers, wiper switches, high beam)
// which we passthru in OP.
if (msg->addr == FORD_Steering_Data_FD1) {
// Violation if resume button is pressed while controls not allowed, or
// if cancel button is pressed when cruise isn't engaged.
bool violation = false;
violation |= ((msg->data[1] >> 0) & 1U) && !cruise_engaged_prev; // Signal: CcAslButtnCnclPress (cancel)
violation |= ((msg->data[3] >> 1) & 1U) && !controls_allowed; // Signal: CcAsllButtnResPress (resume)
if (violation) {
tx = false;
}
}
// Safety check for Lane_Assist_Data1 action
if (msg->addr == FORD_Lane_Assist_Data1) {
// Do not allow steering using Lane_Assist_Data1 (Lane-Departure Aid).
// This message must be sent for Lane Centering to work, and can include
// values such as the steering angle or lane curvature for debugging,
// but the action (LkaActvStats_D2_Req) must be set to zero.
unsigned int action = msg->data[0] >> 5;
if (action != 0U) {
tx = false;
}
}
// Safety check for LateralMotionControl action
if (msg->addr == FORD_LateralMotionControl) {
// Signal: LatCtl_D_Rq
bool steer_control_enabled = ((msg->data[4] >> 2) & 0x7U) != 0U;
unsigned int raw_curvature = (msg->data[0] << 3) | (msg->data[1] >> 5);
unsigned int raw_curvature_rate = ((msg->data[1] & 0x1FU) << 8) | msg->data[2];
unsigned int raw_path_angle = (msg->data[3] << 3) | (msg->data[4] >> 5);
unsigned int raw_path_offset = (msg->data[5] << 2) | (msg->data[6] >> 6);
// These signals are not yet tested with the current safety limits
bool violation = (raw_curvature_rate != FORD_INACTIVE_CURVATURE_RATE) || (raw_path_angle != FORD_INACTIVE_PATH_ANGLE) || (raw_path_offset != FORD_INACTIVE_PATH_OFFSET);
// Check angle error and steer_control_enabled
int desired_curvature = raw_curvature - FORD_INACTIVE_CURVATURE; // /FORD_STEERING_LIMITS.curvature_to_can to get real curvature
violation |= steer_curvature_cmd_checks(desired_curvature, 0, steer_control_enabled, FORD_STEERING_LIMITS);
if (violation) {
tx = false;
}
}
// Safety check for LateralMotionControl2 action
if (msg->addr == FORD_LateralMotionControl2) {
// Signal: LatCtl_D2_Rq
bool steer_control_enabled = ((msg->data[0] >> 4) & 0x7U) != 0U;
unsigned int raw_curvature = (msg->data[2] << 3) | (msg->data[3] >> 5);
unsigned int raw_curvature_rate = (msg->data[6] << 3) | (msg->data[7] >> 5);
unsigned int raw_path_angle = ((msg->data[3] & 0x1FU) << 6) | (msg->data[4] >> 2);
unsigned int raw_path_offset = ((msg->data[4] & 0x3U) << 8) | msg->data[5];
// These signals are not yet tested with the current safety limits
bool violation = (raw_curvature_rate != FORD_CANFD_INACTIVE_CURVATURE_RATE) || (raw_path_angle != FORD_INACTIVE_PATH_ANGLE) || (raw_path_offset != FORD_INACTIVE_PATH_OFFSET);
// Check angle error and steer_control_enabled
int desired_curvature = raw_curvature - FORD_INACTIVE_CURVATURE; // /FORD_STEERING_LIMITS.curvature_to_can to get real curvature
violation |= steer_curvature_cmd_checks(desired_curvature, 0, steer_control_enabled, FORD_STEERING_LIMITS);
if (violation) {
tx = false;
}
}
return tx;
}
static safety_config ford_init(uint16_t param) {
// warning: quality flags are not yet checked in openpilot's CAN parser,
// this may be the cause of blocked messages
static RxCheck ford_rx_checks[] = {
{.msg = {{FORD_BrakeSysFeatures, 0, 8, 50U, .max_counter = 15U}, { 0 }, { 0 }}},
// FORD_EngVehicleSpThrottle2 has a counter that either randomly skips or by 2, likely ECU bug
// Some hybrid models also experience a bug where this checksum mismatches for one or two frames under heavy acceleration with ACC
// It has been confirmed that the Bronco Sport's camera only disallows ACC for bad quality flags, not counters or checksums, so we match that
{.msg = {{FORD_EngVehicleSpThrottle2, 0, 8, 50U, .ignore_checksum = true, .ignore_counter = true}, { 0 }, { 0 }}},
{.msg = {{FORD_Yaw_Data_FD1, 0, 8, 100U, .max_counter = 255U}, { 0 }, { 0 }}},
// These messages have no counter or checksum
{.msg = {{FORD_EngBrakeData, 0, 8, 10U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}},
{.msg = {{FORD_EngVehicleSpThrottle, 0, 8, 100U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}},
{.msg = {{FORD_DesiredTorqBrk, 0, 8, 50U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}},
{.msg = {{FORD_Steering_Data_FD1, 0, 8, 10U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}},
};
#define FORD_COMMON_TX_MSGS \
{FORD_Steering_Data_FD1, 0, 8, .check_relay = false}, \
{FORD_Steering_Data_FD1, 2, 8, .check_relay = false}, \
{FORD_ACCDATA_3, 0, 8, .check_relay = true}, \
{FORD_Lane_Assist_Data1, 0, 8, .check_relay = true}, \
{FORD_IPMA_Data, 0, 8, .check_relay = true}, \
#ifdef ALLOW_DEBUG
static const CanMsg FORD_CANFD_LONG_TX_MSGS[] = {
FORD_COMMON_TX_MSGS
{FORD_ACCDATA, 0, 8, .check_relay = true},
{FORD_LateralMotionControl2, 0, 8, .check_relay = true},
};
#endif
static const CanMsg FORD_CANFD_STOCK_TX_MSGS[] = {
FORD_COMMON_TX_MSGS
{FORD_LateralMotionControl2, 0, 8, .check_relay = true},
};
static const CanMsg FORD_LONG_TX_MSGS[] = {
FORD_COMMON_TX_MSGS
{FORD_ACCDATA, 0, 8, .check_relay = true},
{FORD_LateralMotionControl, 0, 8, .check_relay = true},
};
const uint16_t FORD_PARAM_CANFD = 2;
const bool ford_canfd = GET_FLAG(param, FORD_PARAM_CANFD);
safety_config ret;
if (ford_canfd) {
ret = BUILD_SAFETY_CFG(ford_rx_checks, FORD_CANFD_STOCK_TX_MSGS);
#ifdef ALLOW_DEBUG
const uint16_t FORD_PARAM_LONGITUDINAL = 1;
if (GET_FLAG(param, FORD_PARAM_LONGITUDINAL)) {
ret = BUILD_SAFETY_CFG(ford_rx_checks, FORD_CANFD_LONG_TX_MSGS);
}
#endif
} else {
ret = BUILD_SAFETY_CFG(ford_rx_checks, FORD_LONG_TX_MSGS);
}
return ret;
}
const safety_hooks ford_hooks = {
.init = ford_init,
.rx = ford_rx_hook,
.tx = ford_tx_hook,
.get_counter = ford_get_counter,
.get_checksum = ford_get_checksum,
.compute_checksum = ford_compute_checksum,
.get_quality_flag_valid = ford_get_quality_flag_valid,
};
+283
View File
@@ -0,0 +1,283 @@
#pragma once
#include "opendbc/safety/declarations.h"
// TODO: do checksum and counter checks. Add correct timestep, 0.1s for now.
#define GM_COMMON_RX_CHECKS \
{.msg = {{0x184, 0, 8, 10U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \
{.msg = {{0x34A, 0, 5, 10U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \
{.msg = {{0x1E1, 0, 7, 10U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \
{.msg = {{0xBE, 0, 6, 10U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, /* Volt, Silverado, Acadia Denali */ \
{0xBE, 0, 7, 10U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, /* Bolt EUV */ \
{0xBE, 0, 8, 10U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}}}, /* Escalade */ \
{.msg = {{0x1C4, 0, 8, 10U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \
{.msg = {{0xC9, 0, 8, 10U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \
#define GM_EV_COMMON_ADDR_CHECK \
{.msg = {{0xBD, 0, 7, 40U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \
#define GM_NON_ACC_ADDR_CHECK \
{.msg = {{0x3D1, 0, 8, 10U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \
static const LongitudinalLimits *gm_long_limits;
enum {
GM_BTN_UNPRESS = 1,
GM_BTN_RESUME = 2,
GM_BTN_SET = 3,
GM_BTN_CANCEL = 6,
};
typedef enum {
GM_ASCM,
GM_CAM
} GmHardware;
static GmHardware gm_hw = GM_ASCM;
static bool gm_pcm_cruise = false;
static bool gm_non_acc = false;
static void gm_rx_hook(const CANPacket_t *msg) {
const int GM_STANDSTILL_THRSLD = 10; // 0.311kph
if (msg->bus == 0U) {
if (msg->addr == 0x184U) {
int torque_driver_new = ((msg->data[6] & 0x7U) << 8) | msg->data[7];
torque_driver_new = to_signed(torque_driver_new, 11);
// update array of samples
update_sample(&torque_driver, torque_driver_new);
}
// sample rear wheel speeds
if (msg->addr == 0x34AU) {
int left_rear_speed = (msg->data[0] << 8) | msg->data[1];
int right_rear_speed = (msg->data[2] << 8) | msg->data[3];
vehicle_moving = (left_rear_speed > GM_STANDSTILL_THRSLD) || (right_rear_speed > GM_STANDSTILL_THRSLD);
}
// ACC steering wheel buttons (GM_CAM is tied to the PCM)
if ((msg->addr == 0x1E1U) && !gm_pcm_cruise) {
int button = (msg->data[5] & 0x70U) >> 4;
// enter controls on falling edge of set or rising edge of resume (avoids fault)
bool set = (button != GM_BTN_SET) && (cruise_button_prev == GM_BTN_SET);
bool res = (button == GM_BTN_RESUME) && (cruise_button_prev != GM_BTN_RESUME);
if (set || res) {
controls_allowed = true;
}
// exit controls on cancel press
if (button == GM_BTN_CANCEL) {
controls_allowed = false;
}
cruise_button_prev = button;
}
// Reference for brake pressed signals:
// https://github.com/commaai/openpilot/blob/master/selfdrive/car/gm/carstate.py
if ((msg->addr == 0xBEU) && (gm_hw == GM_ASCM)) {
brake_pressed = msg->data[1] >= 8U;
}
if ((msg->addr == 0xC9U) && (gm_hw == GM_CAM)) {
brake_pressed = GET_BIT(msg, 40U);
}
if (msg->addr == 0x1C4U) {
gas_pressed = msg->data[5] != 0U;
// enter controls on rising edge of ACC, exit controls when ACC off
if (gm_pcm_cruise && !gm_non_acc) {
bool cruise_engaged = (msg->data[1] >> 5) != 0U;
pcm_cruise_check(cruise_engaged);
}
}
if (msg->addr == 0xBDU) {
regen_braking = (msg->data[0] >> 4) != 0U;
}
if (msg->addr == 0xC9U) {
acc_main_on = GET_BIT(msg, 29U);
}
if (msg->addr == 0x3D1U) {
bool cruise_engaged = GET_BIT(msg, 39U);
pcm_cruise_check(cruise_engaged);
}
}
}
static bool gm_tx_hook(const CANPacket_t *msg) {
const TorqueSteeringLimits GM_STEERING_LIMITS = {
.max_torque = 300,
.max_rate_up = 10,
.max_rate_down = 15,
.driver_torque_allowance = 65,
.driver_torque_multiplier = 4,
.max_rt_delta = 128,
.type = TorqueDriverLimited,
};
bool tx = true;
// BRAKE: safety check
if (msg->addr == 0x315U) {
int brake = ((msg->data[0] & 0xFU) << 8) + msg->data[1];
brake = (0x1000 - brake) & 0xFFF;
if (longitudinal_brake_checks(brake, *gm_long_limits)) {
tx = false;
}
}
// LKA STEER: safety check
if (msg->addr == 0x180U) {
int desired_torque = ((msg->data[0] & 0x7U) << 8) + msg->data[1];
desired_torque = to_signed(desired_torque, 11);
bool steer_req = GET_BIT(msg, 3U);
if (steer_torque_cmd_checks(desired_torque, steer_req, GM_STEERING_LIMITS)) {
tx = false;
}
}
// GAS/REGEN: safety check
if (msg->addr == 0x2CBU) {
bool apply = GET_BIT(msg, 0U);
// convert float CAN signal to an int for gas checks: 22534 / 0.125 = 180272
int gas_regen = (((msg->data[1] & 0x7U) << 16) | (msg->data[2] << 8) | msg->data[3]) - 180272U;
bool violation = false;
// Allow apply bit in pre-enabled and overriding states
violation |= !controls_allowed && apply;
violation |= longitudinal_gas_checks(gas_regen, *gm_long_limits);
if (violation) {
tx = false;
}
}
// BUTTONS: used for resume spamming and cruise cancellation with stock longitudinal
if ((msg->addr == 0x1E1U) && gm_pcm_cruise) {
int button = (msg->data[5] >> 4) & 0x7U;
bool allowed_cancel = (button == 6) && cruise_engaged_prev;
if (!allowed_cancel) {
tx = false;
}
}
return tx;
}
static safety_config gm_init(uint16_t param) {
const uint16_t GM_PARAM_HW_CAM = 1;
const uint16_t GM_PARAM_EV = 4;
// common safety checks assume unscaled integer values
static const int GM_GAS_TO_CAN = 8; // 1 / 0.125
static const LongitudinalLimits GM_ASCM_LONG_LIMITS = {
.max_gas = 1018 * GM_GAS_TO_CAN,
.min_gas = -650 * GM_GAS_TO_CAN,
.inactive_gas = -650 * GM_GAS_TO_CAN,
.max_brake = 400,
};
static const CanMsg GM_ASCM_TX_MSGS[] = {{0x180, 0, 4, .check_relay = true}, {0x409, 0, 7, .check_relay = false}, {0x40A, 0, 7, .check_relay = false}, {0x2CB, 0, 8, .check_relay = true}, {0x370, 0, 6, .check_relay = false}, // pt bus
{0xA1, 1, 7, .check_relay = false}, {0x306, 1, 8, .check_relay = false}, {0x308, 1, 7, .check_relay = false}, {0x310, 1, 2, .check_relay = false}, // obs bus
{0x315, 2, 5, .check_relay = false}}; // ch bus
static const LongitudinalLimits GM_CAM_LONG_LIMITS = {
.max_gas = 1346 * GM_GAS_TO_CAN,
.min_gas = -540 * GM_GAS_TO_CAN,
.inactive_gas = -500 * GM_GAS_TO_CAN,
.max_brake = 400,
};
#ifdef ALLOW_DEBUG
// block PSCMStatus (0x184); forwarded through openpilot to hide an alert from the camera
static const CanMsg GM_CAM_LONG_TX_MSGS[] = {{0x180, 0, 4, .check_relay = true}, {0x315, 0, 5, .check_relay = true}, {0x2CB, 0, 8, .check_relay = true}, {0x370, 0, 6, .check_relay = true}, // pt bus
{0x184, 2, 8, .check_relay = true}}; // camera bus
#endif
static RxCheck gm_rx_checks[] = {
GM_COMMON_RX_CHECKS
};
static RxCheck gm_ev_rx_checks[] = {
GM_COMMON_RX_CHECKS
GM_EV_COMMON_ADDR_CHECK
};
static RxCheck gm_non_acc_rx_checks[] = {
GM_COMMON_RX_CHECKS
GM_NON_ACC_ADDR_CHECK
};
static RxCheck gm_non_acc_ev_rx_checks[] = {
GM_COMMON_RX_CHECKS
GM_EV_COMMON_ADDR_CHECK
GM_NON_ACC_ADDR_CHECK
};
static const CanMsg GM_CAM_TX_MSGS[] = {{0x180, 0, 4, .check_relay = true}, // pt bus
{0x1E1, 2, 7, .check_relay = false}, {0x184, 2, 8, .check_relay = true}}; // camera bus
if (GET_FLAG(param, GM_PARAM_HW_CAM)) {
gm_hw = GM_CAM;
gm_long_limits = &GM_CAM_LONG_LIMITS;
} else {
gm_hw = GM_ASCM;
gm_long_limits = &GM_ASCM_LONG_LIMITS;
}
gm_pcm_cruise = (gm_hw == GM_CAM);
const uint16_t GM_PARAM_SP_NON_ACC = 1;
gm_non_acc = GET_FLAG(current_safety_param_sp, GM_PARAM_SP_NON_ACC);
safety_config ret;
if (gm_hw == GM_CAM) {
ret = BUILD_SAFETY_CFG(gm_rx_checks, GM_CAM_TX_MSGS);
#ifdef ALLOW_DEBUG
const uint16_t GM_PARAM_HW_CAM_LONG = 2;
const bool gm_cam_long = GET_FLAG(param, GM_PARAM_HW_CAM_LONG);
gm_pcm_cruise = !gm_cam_long;
if (gm_cam_long) {
ret = BUILD_SAFETY_CFG(gm_rx_checks, GM_CAM_LONG_TX_MSGS);
}
#endif
} else {
ret = BUILD_SAFETY_CFG(gm_rx_checks, GM_ASCM_TX_MSGS);
}
const bool gm_ev = GET_FLAG(param, GM_PARAM_EV);
if (gm_ev) {
SET_RX_CHECKS(gm_ev_rx_checks, ret);
}
if (gm_non_acc) {
SET_TX_MSGS(GM_CAM_TX_MSGS, ret);
if (gm_ev) {
SET_RX_CHECKS(gm_non_acc_ev_rx_checks, ret);
} else {
SET_RX_CHECKS(gm_non_acc_rx_checks, ret);
}
}
// ASCM does not forward any messages
if (gm_hw == GM_ASCM) {
ret.disable_forwarding = true;
}
return ret;
}
const safety_hooks gm_hooks = {
.init = gm_init,
.rx = gm_rx_hook,
.tx = gm_tx_hook,
};
+533
View File
@@ -0,0 +1,533 @@
#pragma once
#include "opendbc/safety/declarations.h"
// All common address checks except SCM_BUTTONS which isn't on one Nidec safety configuration
#define HONDA_COMMON_NO_SCM_FEEDBACK_RX_CHECKS(pt_bus) \
{.msg = {{0x1A6, (pt_bus), 8, 25U, .max_counter = 3U, .ignore_quality_flag = true}, /* SCM_BUTTONS */ \
{0x296, (pt_bus), 4, 25U, .max_counter = 3U, .ignore_quality_flag = true}, { 0 }}}, \
{.msg = {{0x158, (pt_bus), 8, 100U, .max_counter = 3U, .ignore_quality_flag = true}, { 0 }, { 0 }}}, /* ENGINE_DATA */ \
{.msg = {{0x17C, (pt_bus), 8, 100U, .max_counter = 3U, .ignore_quality_flag = true}, { 0 }, { 0 }}}, /* POWERTRAIN_DATA */ \
#define HONDA_COMMON_RX_CHECKS(pt_bus) \
HONDA_COMMON_NO_SCM_FEEDBACK_RX_CHECKS(pt_bus) \
{.msg = {{0x326, (pt_bus), 8, 10U, .max_counter = 3U, .ignore_quality_flag = true}, { 0 }, { 0 }}}, /* SCM_FEEDBACK */ \
// Alternate brake message is used on some Honda Bosch, and Honda Bosch radarless (where PT bus is 0)
#define HONDA_ALT_BRAKE_ADDR_CHECK(pt_bus) \
{.msg = {{0x1BE, (pt_bus), 3, 50U, .max_counter = 3U, .ignore_quality_flag = true}, { 0 }, { 0 }}}, /* BRAKE_MODULE */ \
#define HONDA_GAS_INTERCEPTOR_ADDR_CHECK \
{.msg = {{0x201, 0, 6, 50U, .max_counter = 15U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \
#define HONDA_N_COMMON_TX_MSGS \
{0xE4, 0, 5, .check_relay = true}, \
{0x194, 0, 4, .check_relay = true}, \
{0x1FA, 0, 8, .check_relay = false}, \
{0x30C, 0, 8, .check_relay = true}, \
{0x33D, 0, 5, .check_relay = true}, \
enum {
HONDA_BTN_NONE = 0,
HONDA_BTN_MAIN = 1,
HONDA_BTN_CANCEL = 2,
HONDA_BTN_SET = 3,
HONDA_BTN_RESUME = 4,
};
static int honda_brake = 0;
static bool honda_brake_switch_prev = false;
static bool honda_alt_brake_msg = false;
static bool honda_fwd_brake = false;
static bool honda_bosch_long = false;
static bool honda_bosch_radarless = false;
static bool honda_bosch_canfd = false;
static bool honda_nidec_hybrid = false;
typedef enum {HONDA_NIDEC, HONDA_BOSCH} HondaHw;
static HondaHw honda_hw = HONDA_NIDEC;
static unsigned int honda_get_pt_bus(void) {
return ((honda_hw == HONDA_BOSCH) && !honda_bosch_radarless && !honda_bosch_canfd) ? 1U : 0U;
}
static uint32_t honda_get_checksum(const CANPacket_t *msg) {
int checksum_byte = GET_LEN(msg) - 1U;
return (uint8_t)(msg->data[checksum_byte]) & 0xFU;
}
static uint32_t honda_compute_checksum(const CANPacket_t *msg) {
int len = GET_LEN(msg);
uint8_t checksum = 0U;
unsigned int addr = msg->addr;
while (addr > 0U) {
checksum += (uint8_t)(addr & 0xFU); addr >>= 4;
}
for (int j = 0; j < len; j++) {
uint8_t byte = msg->data[j];
checksum += (uint8_t)(byte & 0xFU) + (byte >> 4U);
if (j == (len - 1)) {
checksum -= (byte & 0xFU); // remove checksum in message
}
}
return (uint8_t)((8U - checksum) & 0xFU);
}
static uint8_t honda_get_counter(const CANPacket_t *msg) {
uint8_t cnt = 0U;
if (msg->addr == 0x201U) {
// Signal: PEDAL_COUNTER
cnt = msg->data[4] & 0x0FU;
} else {
int counter_byte = GET_LEN(msg) - 1U;
cnt = (msg->data[counter_byte] >> 4U) & 0x3U;
}
return cnt;
}
static int HONDA_GET_INTERCEPTOR(const CANPacket_t *msg) {
uint16_t val1 = (uint16_t)((uint16_t)msg->data[0] << 8U) | (uint16_t)msg->data[1];
uint16_t val2 = (uint16_t)((uint16_t)msg->data[2] << 8U) | (uint16_t)msg->data[3];
uint16_t avg = (uint16_t)((val1 + val2) / 2U);
return (int)avg;
}
static void honda_rx_hook(const CANPacket_t *msg) {
const bool pcm_cruise = ((honda_hw == HONDA_BOSCH) && !honda_bosch_long) || ((honda_hw == HONDA_NIDEC) && !enable_gas_interceptor);
unsigned int pt_bus = honda_get_pt_bus();
// sample speed
if (msg->addr == 0x158U) {
vehicle_moving = msg->data[0] | msg->data[1];
}
// check ACC main state
// 0x326 for all Bosch and some Nidec, 0x1A6 for some Nidec
if ((msg->addr == 0x326U) || (msg->addr == 0x1A6U)) {
acc_main_on = GET_BIT(msg, ((msg->addr == 0x326U) ? 28U : 47U));
if (!acc_main_on) {
controls_allowed = false;
}
}
// enter controls when PCM enters cruise state
if (pcm_cruise && (msg->addr == 0x17CU)) {
const bool cruise_engaged = GET_BIT(msg, 38U);
// engage on rising edge
if (cruise_engaged && !cruise_engaged_prev) {
controls_allowed = true;
}
// Since some Nidec cars can brake down to 0 after the PCM disengages,
// we don't disengage when the PCM does.
if (!cruise_engaged && (honda_hw != HONDA_NIDEC)) {
controls_allowed = false;
}
cruise_engaged_prev = cruise_engaged;
}
// state machine to enter and exit controls for button enabling
// 0x1A6 for the ILX, 0x296 for the Civic Touring
if (((msg->addr == 0x1A6U) || (msg->addr == 0x296U)) && (msg->bus == pt_bus)) {
int button = (msg->data[0] & 0xE0U) >> 5;
int cruise_setting = (msg->data[(msg->addr == 0x296U) ? 0U : 5U] & 0x0CU) >> 2U;
if (cruise_setting == 1) {
mads_button_press = MADS_BUTTON_PRESSED;
} else if (cruise_setting == 0) {
mads_button_press = MADS_BUTTON_NOT_PRESSED;
} else {
}
// enter controls on the falling edge of set or resume
bool set = (button != HONDA_BTN_SET) && (cruise_button_prev == HONDA_BTN_SET);
bool res = (button != HONDA_BTN_RESUME) && (cruise_button_prev == HONDA_BTN_RESUME);
if (acc_main_on && !pcm_cruise && (set || res)) {
controls_allowed = true;
}
// exit controls once main or cancel are pressed
if ((button == HONDA_BTN_MAIN) || (button == HONDA_BTN_CANCEL)) {
controls_allowed = false;
}
cruise_button_prev = button;
}
// user brake signal on 0x17C reports applied brake from computer brake on accord
// and crv, which prevents the usual brake safety from working correctly. these
// cars have a signal on 0x1BE which only detects user's brake being applied so
// in these cases, this is used instead.
// most hondas: 0x17C
// accord, crv: 0x1BE
if (honda_alt_brake_msg) {
if (msg->addr == 0x1BEU) {
brake_pressed = GET_BIT(msg, 4U);
}
} else {
if (msg->addr == 0x17CU) {
// also if brake switch is 1 for two CAN frames, as brake pressed is delayed
const bool brake_switch = GET_BIT(msg, 32U);
brake_pressed = (GET_BIT(msg, 53U)) || (brake_switch && honda_brake_switch_prev);
honda_brake_switch_prev = brake_switch;
}
}
// length check because bosch hardware also uses this id (0x201 w/ len = 8)
if (msg->addr == 0x201U) {
// 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_thrsld = 492;
int gas_interceptor = HONDA_GET_INTERCEPTOR(msg);
gas_pressed = gas_interceptor > honda_gas_interceptor_thrsld;
gas_interceptor_prev = gas_interceptor;
}
if (!enable_gas_interceptor) {
if (msg->addr == 0x17CU) {
gas_pressed = msg->data[0] != 0U;
}
}
// disable stock Honda AEB in alternative experience
if (!(alternative_experience & ALT_EXP_DISABLE_STOCK_AEB)) {
if ((msg->bus == 2U) && (msg->addr == 0x1FAU)) {
bool honda_stock_aeb = GET_BIT(msg, 29U);
int honda_stock_brake = (msg->data[0] << 2) | (msg->data[1] >> 6);
if (honda_nidec_hybrid) {
honda_stock_brake = (msg->data[6] << 2) | (msg->data[7] >> 6);
}
// Forward AEB when stock braking is higher than openpilot braking
// only stop forwarding when AEB event is over
if (!honda_stock_aeb) {
honda_fwd_brake = false;
} else if (honda_stock_brake >= honda_brake) {
honda_fwd_brake = true;
} else {
// Leave Honda forward brake as is
}
}
}
}
static bool honda_tx_hook(const CANPacket_t *msg) {
const LongitudinalLimits HONDA_BOSCH_LONG_LIMITS = {
.max_accel = 200, // accel is used for brakes
.min_accel = -350,
.max_gas = 2000,
.inactive_gas = -30000,
};
const LongitudinalLimits HONDA_NIDEC_LONG_LIMITS = {
.max_gas = 198, // 0xc6
.max_brake = 255,
.inactive_speed = 0,
};
bool tx = true;
unsigned int bus_pt = honda_get_pt_bus();
unsigned int bus_buttons = (honda_bosch_radarless) ? 2U : bus_pt; // the camera controls ACC on radarless Bosch cars
// ACC_HUD: safety check (nidec w/o pedal)
if ((msg->addr == 0x30CU) && (msg->bus == bus_pt)) {
int pcm_speed = (msg->data[0] << 8) | msg->data[1];
int pcm_gas = msg->data[2];
bool violation = false;
violation |= longitudinal_speed_checks(pcm_speed, HONDA_NIDEC_LONG_LIMITS);
violation |= longitudinal_gas_checks(pcm_gas, HONDA_NIDEC_LONG_LIMITS);
if (violation) {
tx = false;
}
}
// BRAKE: safety check (nidec)
if ((msg->addr == 0x1FAU) && (msg->bus == bus_pt)) {
honda_brake = (msg->data[0] << 2) + ((msg->data[1] >> 6) & 0x3U);
if (honda_nidec_hybrid) {
honda_brake = (msg->data[6] << 2) + ((msg->data[7] >> 6) & 0x3U);
}
if (longitudinal_brake_checks(honda_brake, HONDA_NIDEC_LONG_LIMITS)) {
tx = false;
}
if (honda_fwd_brake) {
tx = false;
}
}
// BRAKE/GAS: safety check (bosch)
if ((msg->addr == 0x1DFU) && (msg->bus == bus_pt)) {
int accel = (msg->data[3] << 3) | ((msg->data[4] >> 5) & 0x7U);
accel = to_signed(accel, 11);
int gas = (msg->data[0] << 8) | msg->data[1];
gas = to_signed(gas, 16);
bool violation = false;
violation |= longitudinal_accel_checks(accel, HONDA_BOSCH_LONG_LIMITS);
violation |= longitudinal_gas_checks(gas, HONDA_BOSCH_LONG_LIMITS);
if (violation) {
tx = false;
}
}
// ACCEL: safety check (radarless)
if ((msg->addr == 0x1C8U) && (msg->bus == bus_pt)) {
int accel = (msg->data[0] << 4) | (msg->data[1] >> 4);
accel = to_signed(accel, 12);
bool violation = false;
violation |= longitudinal_accel_checks(accel, HONDA_BOSCH_LONG_LIMITS);
if (violation) {
tx = false;
}
}
// STEER: safety check
if ((msg->addr == 0xE4U) || (msg->addr == 0x194U)) {
if (!(controls_allowed || controls_allowed_lateral)) {
bool steer_applied = msg->data[0] | msg->data[1];
if (steer_applied) {
tx = false;
}
}
}
// Bosch supplemental control check
if (msg->addr == 0xE5U) {
if ((GET_BYTES(msg, 0, 4) != 0x10800004U) || ((GET_BYTES(msg, 4, 4) & 0x00FFFFFFU) != 0x0U)) {
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
if ((msg->addr == 0x296U) && !controls_allowed && (msg->bus == bus_buttons)) {
if (((msg->data[0] >> 5) & 0x7U) != 2U) {
tx = false;
}
}
// Only tester present ("\x02\x3E\x80\x00\x00\x00\x00\x00") allowed on diagnostics address
if (msg->addr == 0x18DAB0F1U) {
if ((GET_BYTES(msg, 0, 4) != 0x00803E02U) || (GET_BYTES(msg, 4, 4) != 0x0U)) {
tx = false;
}
}
// GAS: safety check (interceptor)
if (msg->addr == 0x200U) {
if (longitudinal_interceptor_checks(msg)) {
tx = false;
}
}
return tx;
}
static safety_config honda_nidec_init(uint16_t param) {
// 0x1FA is dynamically forwarded based on stock AEB
// 0xE4 is steering on all cars except CRV and RDX, 0x194 for CRV and RDX,
// 0x1FA is brake control, 0x30C is acc hud, 0x33D is lkas hud
static CanMsg HONDA_N_TX_MSGS[] = {HONDA_N_COMMON_TX_MSGS};
static CanMsg HONDA_N_INTERCEPTOR_TX_MSGS[] = {
HONDA_N_COMMON_TX_MSGS
{0x200, 0, 6, .check_relay = false},
};
const uint16_t HONDA_PARAM_NIDEC_ALT = 4;
const uint16_t HONDA_PARAM_SP_NIDEC_HYBRID = 1;
const uint16_t HONDA_PARAM_GAS_INTERCEPTOR = 2;
honda_hw = HONDA_NIDEC;
honda_brake = 0;
honda_brake_switch_prev = false;
honda_fwd_brake = false;
honda_alt_brake_msg = false;
honda_bosch_long = false;
honda_bosch_radarless = false;
honda_bosch_canfd = false;
safety_config ret;
bool enable_nidec_alt = GET_FLAG(param, HONDA_PARAM_NIDEC_ALT);
honda_nidec_hybrid = GET_FLAG(current_safety_param_sp, HONDA_PARAM_SP_NIDEC_HYBRID);
enable_gas_interceptor = GET_FLAG(current_safety_param_sp, HONDA_PARAM_GAS_INTERCEPTOR);
if (enable_nidec_alt) {
// For Nidecs with main on signal on an alternate msg (missing 0x326)
static RxCheck honda_nidec_alt_rx_checks[] = {
HONDA_COMMON_NO_SCM_FEEDBACK_RX_CHECKS(0)
{.msg = {{0x1FA, 2, 8, 50U, .max_counter = 3U, .ignore_quality_flag = true}, { 0 }, { 0 }}}, // BRAKE_COMMAND
};
SET_RX_CHECKS(honda_nidec_alt_rx_checks, ret);
} else {
// Nidec includes BRAKE_COMMAND
static RxCheck honda_nidec_common_rx_checks[] = {
HONDA_COMMON_RX_CHECKS(0)
{.msg = {{0x1FA, 2, 8, 50U, .max_counter = 3U, .ignore_quality_flag = true}, { 0 }, { 0 }}}, // BRAKE_COMMAND
};
SET_RX_CHECKS(honda_nidec_common_rx_checks, ret);
}
SET_TX_MSGS(HONDA_N_TX_MSGS, ret);
if (enable_gas_interceptor) {
if (enable_nidec_alt) {
static RxCheck honda_nidec_alt_interceptor_rx_checks[] = {
HONDA_COMMON_NO_SCM_FEEDBACK_RX_CHECKS(0)
{.msg = {{0x1FA, 2, 8, 50U, .max_counter = 3U, .ignore_quality_flag = true}, { 0 }, { 0 }}}, // BRAKE_COMMAND
HONDA_GAS_INTERCEPTOR_ADDR_CHECK
};
SET_RX_CHECKS(honda_nidec_alt_interceptor_rx_checks, ret);
} else {
static RxCheck honda_nidec_common_interceptor_rx_checks[] = {
HONDA_COMMON_RX_CHECKS(0)
{.msg = {{0x1FA, 2, 8, 50U, .max_counter = 3U, .ignore_quality_flag = true}, { 0 }, { 0 }}}, // BRAKE_COMMAND
HONDA_GAS_INTERCEPTOR_ADDR_CHECK
};
SET_RX_CHECKS(honda_nidec_common_interceptor_rx_checks, ret);
}
SET_TX_MSGS(HONDA_N_INTERCEPTOR_TX_MSGS, ret);
}
return ret;
}
static safety_config honda_bosch_init(uint16_t param) {
static CanMsg HONDA_BOSCH_TX_MSGS[] = {{0xE4, 0, 5, .check_relay = true}, {0xE5, 0, 8, .check_relay = true}, {0x296, 1, 4, .check_relay = false},
{0x33D, 0, 5, .check_relay = true}, {0x33D, 0, 8, .check_relay = true}, {0x33DA, 0, 5, .check_relay = true}, {0x33DB, 0, 8, .check_relay = true}}; // Bosch
static CanMsg HONDA_BOSCH_LONG_TX_MSGS[] = {{0xE4, 1, 5, .check_relay = true}, {0x1DF, 1, 8, .check_relay = true}, {0x1EF, 1, 8, .check_relay = false},
{0x1FA, 1, 8, .check_relay = false}, {0x30C, 1, 8, .check_relay = false}, {0x33D, 1, 5, .check_relay = true}, {0x33D, 1, 8, .check_relay = true},
{0x33DA, 1, 5, .check_relay = true}, {0x33DB, 1, 8, .check_relay = true}, {0x39F, 1, 8, .check_relay = false},
{0x18DAB0F1, 1, 8, .check_relay = false}}; // Bosch w/ gas and brakes
static CanMsg HONDA_RADARLESS_TX_MSGS[] = {{0xE4, 0, 5, .check_relay = true}, {0x296, 2, 4, .check_relay = false}, {0x33D, 0, 8, .check_relay = true}}; // Bosch radarless
static CanMsg HONDA_RADARLESS_LONG_TX_MSGS[] = {{0xE4, 0, 5, .check_relay = true}, {0x33D, 0, 8, .check_relay = true}, {0x1C8, 0, 8, .check_relay = true},
{0x30C, 0, 8, .check_relay = true}}; // Bosch radarless w/ gas and brakes
static CanMsg HONDA_CANFD_TX_MSGS[] = {{0xE4, 0, 5, .check_relay = true}, {0x296, 0, 4, .check_relay = false}, {0x33D, 0, 8, .check_relay = true}};
const uint16_t HONDA_PARAM_ALT_BRAKE = 1;
const uint16_t HONDA_PARAM_RADARLESS = 8;
const uint16_t HONDA_PARAM_BOSCH_CANFD = 16;
// Bosch radarless has the powertrain bus on bus 0
static RxCheck honda_bosch_pt0_rx_checks[] = {
HONDA_COMMON_RX_CHECKS(0)
};
static RxCheck honda_bosch_pt0_alt_brake_rx_checks[] = {
HONDA_COMMON_RX_CHECKS(0)
HONDA_ALT_BRAKE_ADDR_CHECK(0)
};
// Bosch has powertrain on bus 1, verified 0x1A6 does not exist
static RxCheck honda_bosch_pt1_rx_checks[] = {
HONDA_COMMON_RX_CHECKS(1)
};
static RxCheck honda_bosch_pt1_alt_brake_rx_checks[] = {
HONDA_COMMON_RX_CHECKS(1)
HONDA_ALT_BRAKE_ADDR_CHECK(1)
};
honda_hw = HONDA_BOSCH;
honda_brake_switch_prev = false;
honda_bosch_radarless = GET_FLAG(param, HONDA_PARAM_RADARLESS);
honda_bosch_canfd = GET_FLAG(param, HONDA_PARAM_BOSCH_CANFD);
// Checking for alternate brake override from safety parameter
honda_alt_brake_msg = GET_FLAG(param, HONDA_PARAM_ALT_BRAKE);
// radar disabled so allow gas/brakes
#ifdef ALLOW_DEBUG
const uint16_t HONDA_PARAM_BOSCH_LONG = 2;
honda_bosch_long = GET_FLAG(param, HONDA_PARAM_BOSCH_LONG);
#endif
safety_config ret;
if (honda_bosch_radarless || honda_bosch_canfd) {
if (honda_alt_brake_msg) {
SET_RX_CHECKS(honda_bosch_pt0_alt_brake_rx_checks, ret);
} else {
SET_RX_CHECKS(honda_bosch_pt0_rx_checks, ret);
}
} else {
if (honda_alt_brake_msg) {
SET_RX_CHECKS(honda_bosch_pt1_alt_brake_rx_checks, ret);
} else {
SET_RX_CHECKS(honda_bosch_pt1_rx_checks, ret);
}
}
if (honda_bosch_radarless) {
if (honda_bosch_long) {
SET_TX_MSGS(HONDA_RADARLESS_LONG_TX_MSGS, ret);
} else {
SET_TX_MSGS(HONDA_RADARLESS_TX_MSGS, ret);
}
} else if (honda_bosch_canfd) {
SET_TX_MSGS(HONDA_CANFD_TX_MSGS, ret);
} else {
if (honda_bosch_long) {
SET_TX_MSGS(HONDA_BOSCH_LONG_TX_MSGS, ret);
} else {
SET_TX_MSGS(HONDA_BOSCH_TX_MSGS, ret);
}
}
return ret;
}
static bool honda_nidec_fwd_hook(int bus_num, int addr) {
bool block_msg = false;
if (bus_num == 2) {
// forwarded if stock AEB is active
bool is_brake_msg = addr == 0x1FA;
block_msg = is_brake_msg && !honda_fwd_brake;
}
return block_msg;
}
const safety_hooks honda_nidec_hooks = {
.init = honda_nidec_init,
.rx = honda_rx_hook,
.tx = honda_tx_hook,
.fwd = honda_nidec_fwd_hook,
.get_counter = honda_get_counter,
.get_checksum = honda_get_checksum,
.compute_checksum = honda_compute_checksum,
};
const safety_hooks honda_bosch_hooks = {
.init = honda_bosch_init,
.rx = honda_rx_hook,
.tx = honda_tx_hook,
.get_counter = honda_get_counter,
.get_checksum = honda_get_checksum,
.compute_checksum = honda_compute_checksum,
};
+508
View File
@@ -0,0 +1,508 @@
#pragma once
#include "opendbc/safety/declarations.h"
#include "opendbc/safety/modes/hyundai_common.h"
#define HYUNDAI_LIMITS(steer, rate_up, rate_down) { \
.max_torque = (steer), \
.max_rate_up = (rate_up), \
.max_rate_down = (rate_down), \
.max_rt_delta = 112, \
.driver_torque_allowance = 50, \
.driver_torque_multiplier = 2, \
.type = TorqueDriverLimited, \
/* the EPS faults when the steering angle is above a certain threshold for too long. to prevent this, */ \
/* we allow setting CF_Lkas_ActToi bit to 0 while maintaining the requested torque value for two consecutive frames */ \
.min_valid_request_frames = 89, \
.max_invalid_request_frames = 2, \
.min_valid_request_rt_interval = 810000, /* 810ms; a ~10% buffer on cutting every 90 frames */ \
.has_steer_req_tolerance = true, \
}
extern const LongitudinalLimits HYUNDAI_LONG_LIMITS;
const LongitudinalLimits HYUNDAI_LONG_LIMITS = {
.max_accel = 200, // 1/100 m/s2
.min_accel = -350, // 1/100 m/s2
};
#define HYUNDAI_COMMON_TX_MSGS(scc_bus) \
{0x340, 0, 8, .check_relay = true}, /* LKAS11 Bus 0 */ \
{0x4F1, scc_bus, 4, .check_relay = false}, /* CLU11 Bus 0 (radar-SCC) or 2 (camera-SCC) */ \
{0x485, 0, 4, .check_relay = true}, /* LFAHDA_MFC Bus 0 */ \
#define HYUNDAI_LONG_COMMON_TX_MSGS(scc_bus) \
HYUNDAI_COMMON_TX_MSGS(scc_bus) \
{0x420, 0, 8, .check_relay = true}, /* SCC11 Bus 0 */ \
{0x421, 0, 8, .check_relay = true}, /* SCC12 Bus 0 */ \
{0x50A, 0, 8, .check_relay = true}, /* SCC13 Bus 0 */ \
{0x389, 0, 8, .check_relay = true}, /* SCC14 Bus 0 */ \
{0x4A2, 0, 2, .check_relay = false}, /* FRT_RADAR11 Bus 0 */ \
#define HYUNDAI_COMMON_RX_CHECKS(legacy) \
{.msg = {{0x260, 0, 8, 100U, .max_counter = 3U, .ignore_quality_flag = true}, \
{0x371, 0, 8, 100U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }}}, \
{.msg = {{0x386, 0, 8, 100U, .ignore_checksum = (legacy), .ignore_counter = (legacy), .max_counter = (legacy) ? 0U : 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \
{.msg = {{0x394, 0, 8, 100U, .ignore_checksum = (legacy), .ignore_counter = (legacy), .max_counter = (legacy) ? 0U : 7U, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \
{.msg = {{0x251, 0, 8, 50U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \
{.msg = {{0x4F1, 0, 4, 50U, .ignore_checksum = true, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \
#define HYUNDAI_SCC11_ADDR_CHECK(scc_bus) \
{.msg = {{0x420, (scc_bus), 8, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true, .frequency = 50U}, { 0 }, { 0 }}}, \
#define HYUNDAI_SCC12_ADDR_CHECK(scc_bus) \
{.msg = {{0x421, (scc_bus), 8, 50U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \
#define HYUNDAI_FCEV_GAS_ADDR_CHECK \
{.msg = {{0x91, 0, 8, 100U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \
#define HYUNDAI_LDA_BUTTON_ADDR_CHECK \
{.msg = {{0x391, 0, 8, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true, .frequency = 50U}, { 0 }, { 0 }}}, \
#define HYUNDAI_NON_SCC_HEV_ADDR_CHECK \
{.msg = {{0x595U, 0, 8, 10U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \
#define HYUNDAI_NON_SCC_EV_ADDR_CHECK \
{.msg = {{0x592U, 0, 8, 10U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \
static const CanMsg HYUNDAI_TX_MSGS[] = {
HYUNDAI_COMMON_TX_MSGS(0)
};
static bool hyundai_legacy = false;
static uint8_t hyundai_get_counter(const CANPacket_t *msg) {
uint8_t cnt = 0;
if (msg->addr == 0x260U) {
cnt = (msg->data[7] >> 4) & 0x3U;
} else if (msg->addr == 0x386U) {
cnt = ((msg->data[3] >> 6) << 2) | (msg->data[1] >> 6);
} else if (msg->addr == 0x394U) {
cnt = (msg->data[1] >> 5) & 0x7U;
} else if (msg->addr == 0x421U) {
cnt = msg->data[7] & 0xFU;
} else if (msg->addr == 0x4F1U) {
cnt = (msg->data[3] >> 4) & 0xFU;
} else {
}
return cnt;
}
static uint32_t hyundai_get_checksum(const CANPacket_t *msg) {
uint8_t chksum = 0;
if (msg->addr == 0x260U) {
chksum = msg->data[7] & 0xFU;
} else if (msg->addr == 0x386U) {
chksum = ((msg->data[7] >> 6) << 2) | (msg->data[5] >> 6);
} else if (msg->addr == 0x394U) {
chksum = msg->data[6] & 0xFU;
} else if (msg->addr == 0x421U) {
chksum = msg->data[7] >> 4;
} else {
}
return chksum;
}
static uint32_t hyundai_compute_checksum(const CANPacket_t *msg) {
uint8_t chksum = 0;
if (msg->addr == 0x386U) {
// count the bits
for (int i = 0; i < 8; i++) {
uint8_t b = msg->data[i];
for (int j = 0; j < 8; j++) {
uint8_t bit = 0;
// exclude checksum and counter
if (((i != 1) || (j < 6)) && ((i != 3) || (j < 6)) && ((i != 5) || (j < 6)) && ((i != 7) || (j < 6))) {
bit = (b >> (uint8_t)j) & 1U;
}
chksum += bit;
}
}
chksum = (chksum ^ 9U) & 15U;
} else {
// sum of nibbles
for (int i = 0; i < 8; i++) {
if ((msg->addr == 0x394U) && (i == 7)) {
continue; // exclude
}
uint8_t b = msg->data[i];
if (((msg->addr == 0x260U) && (i == 7)) || ((msg->addr == 0x394U) && (i == 6)) || ((msg->addr == 0x421U) && (i == 7))) {
b &= (msg->addr == 0x421U) ? 0x0FU : 0xF0U; // remove checksum
}
chksum += (b % 16U) + (b / 16U);
}
chksum = (16U - (chksum % 16U)) % 16U;
}
return chksum;
}
static void hyundai_rx_hook(const CANPacket_t *msg) {
// SCC12 is on bus 2 for camera-based SCC cars, bus 0 on all others
if (msg->addr == 0x421U) {
if (((msg->bus == 0U) && !hyundai_camera_scc) || ((msg->bus == 2U) && hyundai_camera_scc)) {
// 2 bits: 13-14
int cruise_engaged = (GET_BYTES(msg, 0, 4) >> 13) & 0x3U;
hyundai_common_cruise_state_check(cruise_engaged);
}
}
if (msg->addr == 0x420U) {
if (((msg->bus == 0U) && !hyundai_camera_scc) || ((msg->bus == 2U) && hyundai_camera_scc)) {
if (!hyundai_longitudinal) {
acc_main_on = GET_BIT(msg, 0U);
}
}
}
if (msg->bus == 0U) {
if (msg->addr == 0x251U) {
int torque_driver_new = (GET_BYTES(msg, 0, 2) & 0x7ffU) - 1024U;
// update array of samples
update_sample(&torque_driver, torque_driver_new);
}
if (msg->addr == 0x391U) {
mads_button_press = GET_BIT(msg, 4U) ? MADS_BUTTON_PRESSED : MADS_BUTTON_NOT_PRESSED;
}
// ACC steering wheel buttons
if (msg->addr == 0x4F1U) {
int cruise_button = msg->data[0] & 0x7U;
bool main_button = GET_BIT(msg, 3U);
hyundai_common_cruise_buttons_check(cruise_button, main_button);
}
// gas press, different for EV, hybrid, and ICE models
if ((msg->addr == 0x371U) && hyundai_ev_gas_signal) {
gas_pressed = (((msg->data[4] & 0x7FU) << 1) | (msg->data[3] >> 7)) != 0U;
} else if ((msg->addr == 0x371U) && hyundai_hybrid_gas_signal) {
gas_pressed = msg->data[7] != 0U;
} else if ((msg->addr == 0x91U) && hyundai_fcev_gas_signal) {
gas_pressed = msg->data[6] != 0U;
} else if ((msg->addr == 0x260U) && !hyundai_ev_gas_signal && !hyundai_hybrid_gas_signal) {
gas_pressed = (msg->data[7] >> 6) != 0U;
} else {
}
// sample wheel speed, averaging opposite corners
if (msg->addr == 0x386U) {
uint32_t front_left_speed = GET_BYTES(msg, 0, 2) & 0x3FFFU;
uint32_t rear_right_speed = GET_BYTES(msg, 6, 2) & 0x3FFFU;
vehicle_moving = (front_left_speed > HYUNDAI_STANDSTILL_THRSLD) || (rear_right_speed > HYUNDAI_STANDSTILL_THRSLD);
}
if (msg->addr == 0x394U) {
brake_pressed = ((msg->data[5] >> 5U) & 0x3U) == 0x2U;
}
if (msg->addr == 0x592U) {
acc_main_on = GET_BIT(msg, 34U);
bool cruise_engaged = GET_BIT(msg, 35U);
hyundai_common_cruise_state_check(cruise_engaged);
}
if (msg->addr == 0x595U) {
acc_main_on = GET_BIT(msg, 50U);
bool cruise_engaged = GET_BIT(msg, 51U);
hyundai_common_cruise_state_check(cruise_engaged);
}
if ((msg->addr == 0x260U) && hyundai_non_scc && !hyundai_ev_gas_signal && !hyundai_hybrid_gas_signal) {
acc_main_on = GET_BIT(msg, 25U);
bool cruise_engaged = GET_BIT(msg, 26U);
hyundai_common_cruise_state_check(cruise_engaged);
}
}
hyundai_common_reset_acc_main_on_mismatches();
}
static bool hyundai_tx_hook(const CANPacket_t *msg) {
const TorqueSteeringLimits HYUNDAI_STEERING_LIMITS = HYUNDAI_LIMITS(384, 3, 7);
const TorqueSteeringLimits HYUNDAI_STEERING_LIMITS_ALT = HYUNDAI_LIMITS(270, 2, 3);
const TorqueSteeringLimits HYUNDAI_STEERING_LIMITS_ALT_2 = HYUNDAI_LIMITS(170, 2, 3);
bool tx = true;
// FCA11: Block any potential actuation
if (msg->addr == 0x38DU) {
int CR_VSM_DecCmd = msg->data[1];
bool FCA_CmdAct = GET_BIT(msg, 20U);
bool CF_VSM_DecCmdAct = GET_BIT(msg, 31U);
if ((CR_VSM_DecCmd != 0) || FCA_CmdAct || CF_VSM_DecCmdAct) {
tx = false;
}
}
if (msg->addr == 0x420U) {
acc_main_on_tx = GET_BIT(msg, 0U);
hyundai_common_acc_main_on_sync();
}
// ACCEL: safety check
if (msg->addr == 0x421U) {
int desired_accel_raw = (((msg->data[4] & 0x7U) << 8) | msg->data[3]) - 1023U;
int desired_accel_val = ((msg->data[5] << 3) | (msg->data[4] >> 5)) - 1023U;
int aeb_decel_cmd = msg->data[2];
bool aeb_req = GET_BIT(msg, 54U);
bool violation = false;
violation |= longitudinal_accel_checks(desired_accel_raw, HYUNDAI_LONG_LIMITS);
violation |= longitudinal_accel_checks(desired_accel_val, HYUNDAI_LONG_LIMITS);
if (!hyundai_escc) {
violation |= (aeb_decel_cmd != 0);
violation |= aeb_req;
}
if (violation) {
tx = false;
}
}
// LKA STEER: safety check
if (msg->addr == 0x340U) {
int desired_torque = ((GET_BYTES(msg, 0, 4) >> 16) & 0x7ffU) - 1024U;
bool steer_req = GET_BIT(msg, 27U);
const TorqueSteeringLimits limits = hyundai_alt_limits_2 ? HYUNDAI_STEERING_LIMITS_ALT_2 :
hyundai_alt_limits ? HYUNDAI_STEERING_LIMITS_ALT : HYUNDAI_STEERING_LIMITS;
if (steer_torque_cmd_checks(desired_torque, steer_req, limits)) {
tx = false;
}
}
// UDS: Only tester present ("\x02\x3E\x80\x00\x00\x00\x00\x00") allowed on diagnostics address
if (msg->addr == 0x7D0U) {
if ((GET_BYTES(msg, 0, 4) != 0x00803E02U) || (GET_BYTES(msg, 4, 4) != 0x0U)) {
tx = false;
}
}
// BUTTONS: used for resume spamming and cruise cancellation
if ((msg->addr == 0x4F1U) && !hyundai_longitudinal) {
int button = msg->data[0] & 0x7U;
bool allowed_resume = (button == 1) && controls_allowed;
bool allowed_set = (button == 2) && controls_allowed;
bool allowed_cancel = (button == 4) && cruise_engaged_prev;
if (!(allowed_resume || allowed_set || allowed_cancel)) {
tx = false;
}
}
return tx;
}
static safety_config hyundai_init(uint16_t param) {
static const CanMsg HYUNDAI_LONG_TX_MSGS[] = {
HYUNDAI_LONG_COMMON_TX_MSGS(0)
{0x38D, 0, 8, .check_relay = false}, // FCA11 Bus 0
{0x483, 0, 8, .check_relay = false}, // FCA12 Bus 0
{0x7D0, 0, 8, .check_relay = false}, // radar UDS TX addr Bus 0 (for radar disable)
};
static const CanMsg HYUNDAI_CAMERA_SCC_TX_MSGS[] = {
HYUNDAI_COMMON_TX_MSGS(2)
};
static const CanMsg HYUNDAI_CAMERA_SCC_LONG_TX_MSGS[] = {
HYUNDAI_LONG_COMMON_TX_MSGS(2)
};
static const CanMsg HYUNDAI_LONG_ESCC_TX_MSGS[] = {
HYUNDAI_LONG_COMMON_TX_MSGS(0)
};
hyundai_common_init(param);
hyundai_legacy = false;
safety_config ret;
if (hyundai_longitudinal) {
// Use CLU11 (buttons) to manage controls allowed instead of SCC cruise state
static RxCheck hyundai_long_rx_checks[] = {
HYUNDAI_COMMON_RX_CHECKS(false)
};
static RxCheck hyundai_lda_button_long_rx_checks[] = {
HYUNDAI_COMMON_RX_CHECKS(false)
HYUNDAI_LDA_BUTTON_ADDR_CHECK
};
static RxCheck hyundai_fcev_long_rx_checks[] = {
HYUNDAI_COMMON_RX_CHECKS(false)
HYUNDAI_FCEV_GAS_ADDR_CHECK
};
static RxCheck hyundai_fcev_lda_button_long_rx_checks[] = {
HYUNDAI_COMMON_RX_CHECKS(false)
HYUNDAI_FCEV_GAS_ADDR_CHECK
HYUNDAI_LDA_BUTTON_ADDR_CHECK
};
if (hyundai_fcev_gas_signal) {
if (hyundai_has_lda_button) {
SET_RX_CHECKS(hyundai_fcev_lda_button_long_rx_checks, ret);
} else {
SET_RX_CHECKS(hyundai_fcev_long_rx_checks, ret);
}
} else {
if (hyundai_has_lda_button) {
SET_RX_CHECKS(hyundai_lda_button_long_rx_checks, ret);
} else {
SET_RX_CHECKS(hyundai_long_rx_checks, ret);
}
}
if (hyundai_escc) {
SET_TX_MSGS(HYUNDAI_LONG_ESCC_TX_MSGS, ret);
} else if (hyundai_camera_scc) {
SET_TX_MSGS(HYUNDAI_CAMERA_SCC_LONG_TX_MSGS, ret);
} else {
SET_TX_MSGS(HYUNDAI_LONG_TX_MSGS, ret);
}
} else if (hyundai_camera_scc) {
static RxCheck hyundai_cam_scc_rx_checks[] = {
HYUNDAI_COMMON_RX_CHECKS(false)
HYUNDAI_SCC12_ADDR_CHECK(2)
HYUNDAI_SCC11_ADDR_CHECK(2)
HYUNDAI_LDA_BUTTON_ADDR_CHECK
};
ret = BUILD_SAFETY_CFG(hyundai_cam_scc_rx_checks, HYUNDAI_CAMERA_SCC_TX_MSGS);
} else {
static RxCheck hyundai_rx_checks[] = {
HYUNDAI_COMMON_RX_CHECKS(false)
HYUNDAI_SCC12_ADDR_CHECK(0)
HYUNDAI_SCC11_ADDR_CHECK(0)
};
static RxCheck hyundai_lda_button_rx_checks[] = {
HYUNDAI_COMMON_RX_CHECKS(false)
HYUNDAI_SCC12_ADDR_CHECK(0)
HYUNDAI_SCC11_ADDR_CHECK(0)
HYUNDAI_LDA_BUTTON_ADDR_CHECK
};
static RxCheck hyundai_fcev_rx_checks[] = {
HYUNDAI_COMMON_RX_CHECKS(false)
HYUNDAI_SCC12_ADDR_CHECK(0)
HYUNDAI_SCC11_ADDR_CHECK(0)
HYUNDAI_FCEV_GAS_ADDR_CHECK
};
static RxCheck hyundai_fcev_lda_button_rx_checks[] = {
HYUNDAI_COMMON_RX_CHECKS(false)
HYUNDAI_SCC12_ADDR_CHECK(0)
HYUNDAI_SCC11_ADDR_CHECK(0)
HYUNDAI_FCEV_GAS_ADDR_CHECK
HYUNDAI_LDA_BUTTON_ADDR_CHECK
};
static RxCheck hyundai_non_scc_addr_checks[] = {
HYUNDAI_COMMON_RX_CHECKS(false)
};
static RxCheck hyundai_non_scc_lda_button_addr_checks[] = {
HYUNDAI_COMMON_RX_CHECKS(false)
HYUNDAI_LDA_BUTTON_ADDR_CHECK
};
static RxCheck hyundai_hev_non_scc_addr_checks[] = {
HYUNDAI_COMMON_RX_CHECKS(false)
HYUNDAI_NON_SCC_HEV_ADDR_CHECK
};
static RxCheck hyundai_hev_non_scc_lda_button_addr_checks[] = {
HYUNDAI_COMMON_RX_CHECKS(false)
HYUNDAI_NON_SCC_HEV_ADDR_CHECK
HYUNDAI_LDA_BUTTON_ADDR_CHECK
};
static RxCheck hyundai_ev_non_scc_addr_checks[] = {
HYUNDAI_COMMON_RX_CHECKS(false)
HYUNDAI_NON_SCC_EV_ADDR_CHECK
};
static RxCheck hyundai_ev_non_scc_lda_button_addr_checks[] = {
HYUNDAI_COMMON_RX_CHECKS(false)
HYUNDAI_NON_SCC_EV_ADDR_CHECK
HYUNDAI_LDA_BUTTON_ADDR_CHECK
};
SET_TX_MSGS(HYUNDAI_TX_MSGS, ret);
if (hyundai_fcev_gas_signal) {
if (hyundai_has_lda_button) {
SET_RX_CHECKS(hyundai_fcev_lda_button_rx_checks, ret);
} else {
SET_RX_CHECKS(hyundai_fcev_rx_checks, ret);
}
} else if (hyundai_non_scc) {
if (hyundai_ev_gas_signal) {
if (hyundai_has_lda_button) {
SET_RX_CHECKS(hyundai_ev_non_scc_lda_button_addr_checks, ret);
} else {
SET_RX_CHECKS(hyundai_ev_non_scc_addr_checks, ret);
}
} else if (hyundai_hybrid_gas_signal) {
if (hyundai_has_lda_button) {
SET_RX_CHECKS(hyundai_hev_non_scc_lda_button_addr_checks, ret);
} else {
SET_RX_CHECKS(hyundai_hev_non_scc_addr_checks, ret);
}
} else {
if (hyundai_has_lda_button) {
SET_RX_CHECKS(hyundai_non_scc_lda_button_addr_checks, ret);
} else {
SET_RX_CHECKS(hyundai_non_scc_addr_checks, ret);
}
}
} else {
if (hyundai_has_lda_button) {
SET_RX_CHECKS(hyundai_lda_button_rx_checks, ret);
} else {
SET_RX_CHECKS(hyundai_rx_checks, ret);
}
}
}
return ret;
}
static safety_config hyundai_legacy_init(uint16_t param) {
// older hyundai models have less checks due to missing counters and checksums
static RxCheck hyundai_legacy_rx_checks[] = {
HYUNDAI_COMMON_RX_CHECKS(true)
HYUNDAI_SCC12_ADDR_CHECK(0)
HYUNDAI_SCC11_ADDR_CHECK(0)
};
hyundai_common_init(param);
hyundai_legacy = true;
hyundai_longitudinal = false;
hyundai_camera_scc = false;
return BUILD_SAFETY_CFG(hyundai_legacy_rx_checks, HYUNDAI_TX_MSGS);
}
const safety_hooks hyundai_hooks = {
.init = hyundai_init,
.rx = hyundai_rx_hook,
.tx = hyundai_tx_hook,
.get_counter = hyundai_get_counter,
.get_checksum = hyundai_get_checksum,
.compute_checksum = hyundai_compute_checksum,
};
const safety_hooks hyundai_legacy_hooks = {
.init = hyundai_legacy_init,
.rx = hyundai_rx_hook,
.tx = hyundai_tx_hook,
.get_counter = hyundai_get_counter,
.get_checksum = hyundai_get_checksum,
.compute_checksum = hyundai_compute_checksum,
};
@@ -0,0 +1,391 @@
#pragma once
#include "opendbc/safety/declarations.h"
#include "opendbc/safety/modes/hyundai_common.h"
#define HYUNDAI_CANFD_CRUISE_BUTTON_TX_MSGS(bus) \
{0x1CF, bus, 8, .check_relay = false}, /* CRUISE_BUTTON */ \
#define HYUNDAI_CANFD_LKA_STEER_MSG_COMMON_TX_MSGS(a_can, e_can) \
HYUNDAI_CANFD_CRUISE_BUTTON_TX_MSGS(e_can) \
{0x50, a_can, 16, .check_relay = (a_can) == 0}, /* LKAS */ \
{0x2A4, a_can, 24, .check_relay = (a_can) == 0}, /* CAM_0x2A4 */ \
#define HYUNDAI_CANFD_LKA_STEER_MSG_ALT_COMMON_TX_MSGS(a_can, e_can) \
HYUNDAI_CANFD_CRUISE_BUTTON_TX_MSGS(e_can) \
{0x110, a_can, 32, .check_relay = (a_can) == 0}, /* LKAS_ALT */ \
{0x362, a_can, 32, .check_relay = (a_can) == 0}, /* CAM_0x362 */ \
#define HYUNDAI_CANFD_LFA_STEERING_COMMON_TX_MSGS(e_can) \
{0x12A, e_can, 16, .check_relay = (e_can) == 0}, /* LFA */ \
{0x1E0, e_can, 16, .check_relay = (e_can) == 0}, /* LFAHDA_CLUSTER */ \
#define HYUNDAI_CANFD_SCC_CONTROL_COMMON_TX_MSGS(e_can, longitudinal) \
{0x1A0, e_can, 32, .check_relay = (longitudinal)}, /* SCC_CONTROL */ \
// *** Addresses checked in rx hook ***
// EV, ICE, HYBRID: ACCELERATOR (0x35), ACCELERATOR_BRAKE_ALT (0x100), ACCELERATOR_ALT (0x105)
#define HYUNDAI_CANFD_COMMON_RX_CHECKS(pt_bus) \
{.msg = {{0x35, (pt_bus), 32, 100U, .max_counter = 0xffU, .ignore_quality_flag = true}, \
{0x100, (pt_bus), 32, 100U, .max_counter = 0xffU, .ignore_quality_flag = true}, \
{0x105, (pt_bus), 32, 100U, .max_counter = 0xffU, .ignore_quality_flag = true}}}, \
{.msg = {{0x175, (pt_bus), 24, 50U, .max_counter = 0xffU, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \
{.msg = {{0xa0, (pt_bus), 24, 100U, .max_counter = 0xffU, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \
{.msg = {{0xea, (pt_bus), 24, 100U, .max_counter = 0xffU, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \
#define HYUNDAI_CANFD_STD_BUTTONS_RX_CHECKS(pt_bus) \
HYUNDAI_CANFD_COMMON_RX_CHECKS(pt_bus) \
{.msg = {{0x1cf, (pt_bus), 8, 50U, .ignore_checksum = true, .max_counter = 0xfU, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \
#define HYUNDAI_CANFD_ALT_BUTTONS_RX_CHECKS(pt_bus) \
HYUNDAI_CANFD_COMMON_RX_CHECKS(pt_bus) \
{.msg = {{0x1aa, (pt_bus), 16, 50U, .ignore_checksum = true, .max_counter = 0xffU, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \
// SCC_CONTROL (from ADAS unit or camera)
#define HYUNDAI_CANFD_SCC_ADDR_CHECK(scc_bus) \
{.msg = {{0x1a0, (scc_bus), 32, 50U, .max_counter = 0xffU, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \
static bool hyundai_canfd_alt_buttons = false;
static bool hyundai_canfd_lka_steer_msg_alt = false;
static unsigned int hyundai_canfd_get_lka_addr(void) {
return hyundai_canfd_lka_steer_msg_alt ? 0x110U : 0x50U;
}
static uint8_t hyundai_canfd_get_counter(const CANPacket_t *msg) {
uint8_t ret = 0;
if (GET_LEN(msg) == 8U) {
ret = msg->data[1] >> 4;
} else {
ret = msg->data[2];
}
return ret;
}
static uint32_t hyundai_canfd_get_checksum(const CANPacket_t *msg) {
uint32_t chksum = msg->data[0] | (msg->data[1] << 8);
return chksum;
}
static void hyundai_canfd_rx_hook(const CANPacket_t *msg) {
const unsigned pt_bus = hyundai_canfd_lka_steer_msg ? 1U : 0U;
const unsigned int scc_bus = hyundai_camera_scc ? 2U : pt_bus;
if (msg->bus == pt_bus) {
// driver torque
if (msg->addr == 0xeaU) {
int torque_driver_new = ((msg->data[11] & 0x1fU) << 8U) | msg->data[10];
torque_driver_new -= 4095;
update_sample(&torque_driver, torque_driver_new);
}
// cruise buttons
const unsigned int button_addr = hyundai_canfd_alt_buttons ? 0x1aaU : 0x1cfU;
if (msg->addr == button_addr) {
bool main_button = false;
int cruise_button = 0;
if (msg->addr == 0x1cfU) {
cruise_button = msg->data[2] & 0x7U;
main_button = GET_BIT(msg, 19U);
mads_button_press = GET_BIT(msg, 23U) ? MADS_BUTTON_PRESSED : MADS_BUTTON_NOT_PRESSED;
} else {
cruise_button = (msg->data[4] >> 4) & 0x7U;
main_button = GET_BIT(msg, 34U);
mads_button_press = GET_BIT(msg, 39U) ? MADS_BUTTON_PRESSED : MADS_BUTTON_NOT_PRESSED;
}
hyundai_common_cruise_buttons_check(cruise_button, main_button);
}
// gas press, different for EV, hybrid, and ICE models
if ((msg->addr == 0x35U) && hyundai_ev_gas_signal) {
gas_pressed = msg->data[5] != 0U;
} else if ((msg->addr == 0x105U) && hyundai_hybrid_gas_signal) {
gas_pressed = GET_BIT(msg, 103U) || (msg->data[13] != 0U) || GET_BIT(msg, 112U);
} else if ((msg->addr == 0x100U) && !hyundai_ev_gas_signal && !hyundai_hybrid_gas_signal) {
gas_pressed = GET_BIT(msg, 176U);
} else {
}
// brake press
if (msg->addr == 0x175U) {
brake_pressed = GET_BIT(msg, 81U);
}
// vehicle moving
if (msg->addr == 0xa0U) {
uint32_t fl = (GET_BYTES(msg, 8, 2)) & 0x3FFFU;
uint32_t fr = (GET_BYTES(msg, 10, 2)) & 0x3FFFU;
uint32_t rl = (GET_BYTES(msg, 12, 2)) & 0x3FFFU;
uint32_t rr = (GET_BYTES(msg, 14, 2)) & 0x3FFFU;
vehicle_moving = (fl > HYUNDAI_STANDSTILL_THRSLD) || (fr > HYUNDAI_STANDSTILL_THRSLD) ||
(rl > HYUNDAI_STANDSTILL_THRSLD) || (rr > HYUNDAI_STANDSTILL_THRSLD);
// average of all 4 wheel speeds. Conversion: raw * 0.03125 / 3.6 = m/s
UPDATE_VEHICLE_SPEED((fr + rr + rl + fl) / 4.0 * 0.03125 * KPH_TO_MS);
}
}
if (msg->bus == scc_bus) {
// cruise state
if ((msg->addr == 0x1a0U) && !hyundai_longitudinal) {
// 1=enabled, 2=driver override
int cruise_status = ((msg->data[8] >> 4) & 0x7U);
bool cruise_engaged = (cruise_status == 1) || (cruise_status == 2);
hyundai_common_cruise_state_check(cruise_engaged);
acc_main_on = GET_BIT(msg, 66U);
}
}
hyundai_common_reset_acc_main_on_mismatches();
}
static bool hyundai_canfd_tx_hook(const CANPacket_t *msg) {
const TorqueSteeringLimits HYUNDAI_CANFD_STEERING_LIMITS = {
.max_torque = 270,
.max_rt_delta = 112,
.max_rate_up = 2,
.max_rate_down = 3,
.driver_torque_allowance = 250,
.driver_torque_multiplier = 2,
.type = TorqueDriverLimited,
// the EPS faults when the steering angle is above a certain threshold for too long. to prevent this,
// we allow setting torque actuation bit to 0 while maintaining the requested torque value for two consecutive frames
.min_valid_request_frames = 89,
.max_invalid_request_frames = 2,
.min_valid_request_rt_interval = 810000, // 810ms; a ~10% buffer on cutting every 90 frames
.has_steer_req_tolerance = true,
};
bool tx = true;
// steering
const unsigned int steer_addr = (hyundai_canfd_lka_steer_msg && !hyundai_longitudinal) ? hyundai_canfd_get_lka_addr() : 0x12aU;
if (msg->addr == steer_addr) {
int desired_torque = (((msg->data[6] & 0xFU) << 7U) | (msg->data[5] >> 1U)) - 1024U;
bool steer_req = GET_BIT(msg, 52U);
if (steer_torque_cmd_checks(desired_torque, steer_req, HYUNDAI_CANFD_STEERING_LIMITS)) {
tx = false;
}
}
// cruise buttons check
if (msg->addr == 0x1cfU) {
int button = msg->data[2] & 0x7U;
bool is_cancel = (button == HYUNDAI_BTN_CANCEL);
bool is_resume = (button == HYUNDAI_BTN_RESUME);
bool is_set = (button == HYUNDAI_BTN_SET);
bool allowed = (is_cancel && cruise_engaged_prev) || ((is_resume || is_set) && controls_allowed);
if (!allowed) {
tx = false;
}
}
// UDS: only tester present ("\x02\x3E\x80\x00\x00\x00\x00\x00") allowed on diagnostics address
if (((msg->addr == 0x730U) && hyundai_canfd_lka_steer_msg) || ((msg->addr == 0x7D0U) && !hyundai_camera_scc)) {
if ((GET_BYTES(msg, 0, 4) != 0x00803E02U) || (GET_BYTES(msg, 4, 4) != 0x0U)) {
tx = false;
}
}
// ACCEL: safety check
if (msg->addr == 0x1a0U) {
int desired_accel_raw = (((msg->data[17] & 0x7U) << 8) | msg->data[16]) - 1023U;
int desired_accel_val = ((msg->data[18] << 4) | (msg->data[17] >> 4)) - 1023U;
bool violation = false;
if (hyundai_longitudinal) {
violation |= longitudinal_accel_checks(desired_accel_raw, HYUNDAI_LONG_LIMITS);
violation |= longitudinal_accel_checks(desired_accel_val, HYUNDAI_LONG_LIMITS);
} else {
// only used to cancel on here
const int acc_mode = (msg->data[8] >> 4) & 0x7U;
if (acc_mode != 4) {
violation = true;
}
if ((desired_accel_raw != 0) || (desired_accel_val != 0)) {
violation = true;
}
}
if (violation) {
tx = false;
}
acc_main_on_tx = GET_BIT(msg, 66U);
hyundai_common_acc_main_on_sync();
}
return tx;
}
static safety_config hyundai_canfd_init(uint16_t param) {
const uint16_t HYUNDAI_PARAM_CANFD_LKA_STEER_MSG_ALT = 128;
const uint16_t HYUNDAI_PARAM_CANFD_ALT_BUTTONS = 32;
static const CanMsg HYUNDAI_CANFD_LKA_STEER_MSG_TX_MSGS[] = {
HYUNDAI_CANFD_LKA_STEER_MSG_COMMON_TX_MSGS(0, 1)
};
static const CanMsg HYUNDAI_CANFD_LKA_STEER_MSG_ALT_TX_MSGS[] = {
HYUNDAI_CANFD_LKA_STEER_MSG_ALT_COMMON_TX_MSGS(0, 1)
};
static const CanMsg HYUNDAI_CANFD_LKA_STEER_MSG_LONG_TX_MSGS[] = {
HYUNDAI_CANFD_LKA_STEER_MSG_COMMON_TX_MSGS(0, 1)
HYUNDAI_CANFD_LFA_STEERING_COMMON_TX_MSGS(1)
HYUNDAI_CANFD_SCC_CONTROL_COMMON_TX_MSGS(1, true)
{0x51, 0, 32, .check_relay = false}, // ADRV_0x51
{0x730, 1, 8, .check_relay = false}, // tester present for ADAS ECU disable
{0x160, 1, 16, .check_relay = false}, // ADRV_0x160
{0x1EA, 1, 32, .check_relay = false}, // ADRV_0x1ea
{0x200, 1, 8, .check_relay = false}, // ADRV_0x200
{0x345, 1, 8, .check_relay = false}, // ADRV_0x345
{0x1DA, 1, 32, .check_relay = false}, // ADRV_0x1da
};
static const CanMsg HYUNDAI_CANFD_LFA_STEERING_TX_MSGS[] = {
HYUNDAI_CANFD_CRUISE_BUTTON_TX_MSGS(2)
HYUNDAI_CANFD_LFA_STEERING_COMMON_TX_MSGS(0)
HYUNDAI_CANFD_SCC_CONTROL_COMMON_TX_MSGS(0, false)
};
// ADRV_0x160 is checked for radar liveness
static const CanMsg HYUNDAI_CANFD_LFA_STEERING_LONG_TX_MSGS[] = {
HYUNDAI_CANFD_CRUISE_BUTTON_TX_MSGS(2)
HYUNDAI_CANFD_LFA_STEERING_COMMON_TX_MSGS(0)
HYUNDAI_CANFD_SCC_CONTROL_COMMON_TX_MSGS(0, true)
{0x160, 0, 16, .check_relay = true}, // ADRV_0x160
{0x7D0, 0, 8, .check_relay = false}, // tester present for radar ECU disable
};
// ADRV_0x160 is checked for relay malfunction
#define HYUNDAI_CANFD_LFA_STEERING_CAMERA_SCC_TX_MSGS(longitudinal) \
HYUNDAI_CANFD_CRUISE_BUTTON_TX_MSGS(2) \
HYUNDAI_CANFD_LFA_STEERING_COMMON_TX_MSGS(0) \
HYUNDAI_CANFD_SCC_CONTROL_COMMON_TX_MSGS(0, (longitudinal)) \
{0x160, 0, 16, .check_relay = (longitudinal)}, /* ADRV_0x160 */ \
hyundai_common_init(param);
gen_crc_lookup_table_16(0x1021, hyundai_canfd_crc_lut);
hyundai_canfd_alt_buttons = GET_FLAG(param, HYUNDAI_PARAM_CANFD_ALT_BUTTONS);
hyundai_canfd_lka_steer_msg_alt = GET_FLAG(param, HYUNDAI_PARAM_CANFD_LKA_STEER_MSG_ALT);
safety_config ret;
if (hyundai_longitudinal) {
if (hyundai_canfd_lka_steer_msg) {
static RxCheck hyundai_canfd_lka_steer_msg_long_rx_checks[] = {
HYUNDAI_CANFD_STD_BUTTONS_RX_CHECKS(1)
};
ret = BUILD_SAFETY_CFG(hyundai_canfd_lka_steer_msg_long_rx_checks, HYUNDAI_CANFD_LKA_STEER_MSG_LONG_TX_MSGS);
} else {
// Longitudinal checks for LFA steering
static RxCheck hyundai_canfd_long_rx_checks[] = {
HYUNDAI_CANFD_STD_BUTTONS_RX_CHECKS(0)
};
static RxCheck hyundai_canfd_alt_buttons_long_rx_checks[] = {
HYUNDAI_CANFD_ALT_BUTTONS_RX_CHECKS(0)
};
static CanMsg hyundai_canfd_lfa_steering_camera_scc_tx_msgs[] = {
HYUNDAI_CANFD_LFA_STEERING_CAMERA_SCC_TX_MSGS(true)
};
if (hyundai_canfd_alt_buttons) {
SET_RX_CHECKS(hyundai_canfd_alt_buttons_long_rx_checks, ret);
} else {
SET_RX_CHECKS(hyundai_canfd_long_rx_checks, ret);
}
if (hyundai_camera_scc) {
SET_TX_MSGS(hyundai_canfd_lfa_steering_camera_scc_tx_msgs, ret);
} else {
SET_TX_MSGS(HYUNDAI_CANFD_LFA_STEERING_LONG_TX_MSGS, ret);
}
}
} else {
if (hyundai_canfd_lka_steer_msg) {
// *** LKA steering checks ***
// E-CAN is on bus 1, SCC messages are sent on cars with ADRV ECU.
// Does not use the alt buttons message
static RxCheck hyundai_canfd_lka_steer_msg_rx_checks[] = {
HYUNDAI_CANFD_STD_BUTTONS_RX_CHECKS(1)
HYUNDAI_CANFD_SCC_ADDR_CHECK(1)
};
SET_RX_CHECKS(hyundai_canfd_lka_steer_msg_rx_checks, ret);
if (hyundai_canfd_lka_steer_msg_alt) {
SET_TX_MSGS(HYUNDAI_CANFD_LKA_STEER_MSG_ALT_TX_MSGS, ret);
} else {
SET_TX_MSGS(HYUNDAI_CANFD_LKA_STEER_MSG_TX_MSGS, ret);
}
} else if (!hyundai_camera_scc) {
// Radar sends SCC messages on these cars instead of camera
static RxCheck hyundai_canfd_radar_scc_rx_checks[] = {
HYUNDAI_CANFD_STD_BUTTONS_RX_CHECKS(0)
HYUNDAI_CANFD_SCC_ADDR_CHECK(0)
};
static RxCheck hyundai_canfd_alt_buttons_radar_scc_rx_checks[] = {
HYUNDAI_CANFD_ALT_BUTTONS_RX_CHECKS(0)
HYUNDAI_CANFD_SCC_ADDR_CHECK(0)
};
SET_TX_MSGS(HYUNDAI_CANFD_LFA_STEERING_TX_MSGS, ret);
if (hyundai_canfd_alt_buttons) {
SET_RX_CHECKS(hyundai_canfd_alt_buttons_radar_scc_rx_checks, ret);
} else {
SET_RX_CHECKS(hyundai_canfd_radar_scc_rx_checks, ret);
}
} else {
// *** LFA steering checks ***
// Camera sends SCC messages on LFA steering cars.
// Both button messages exist on some platforms, so we ensure we track the correct one using flag
static RxCheck hyundai_canfd_rx_checks[] = {
HYUNDAI_CANFD_STD_BUTTONS_RX_CHECKS(0)
HYUNDAI_CANFD_SCC_ADDR_CHECK(2)
};
static RxCheck hyundai_canfd_alt_buttons_rx_checks[] = {
HYUNDAI_CANFD_ALT_BUTTONS_RX_CHECKS(0)
HYUNDAI_CANFD_SCC_ADDR_CHECK(2)
};
static CanMsg hyundai_canfd_lfa_steering_camera_scc_tx_msgs[] = {
HYUNDAI_CANFD_LFA_STEERING_CAMERA_SCC_TX_MSGS(false)
};
SET_TX_MSGS(hyundai_canfd_lfa_steering_camera_scc_tx_msgs, ret);
if (hyundai_canfd_alt_buttons) {
SET_RX_CHECKS(hyundai_canfd_alt_buttons_rx_checks, ret);
} else {
SET_RX_CHECKS(hyundai_canfd_rx_checks, ret);
}
}
}
return ret;
}
const safety_hooks hyundai_canfd_hooks = {
.init = hyundai_canfd_init,
.rx = hyundai_canfd_rx_hook,
.tx = hyundai_canfd_tx_hook,
.get_counter = hyundai_canfd_get_counter,
.get_checksum = hyundai_canfd_get_checksum,
.compute_checksum = hyundai_common_canfd_compute_checksum,
};
@@ -0,0 +1,206 @@
#pragma once
#include "opendbc/safety/declarations.h"
extern uint16_t hyundai_canfd_crc_lut[256];
uint16_t hyundai_canfd_crc_lut[256];
static const uint8_t HYUNDAI_PREV_BUTTON_SAMPLES = 8; // roughly 160 ms
extern const uint32_t HYUNDAI_STANDSTILL_THRSLD;
const uint32_t HYUNDAI_STANDSTILL_THRSLD = 12; // 0.375 kph
enum {
HYUNDAI_BTN_NONE = 0,
HYUNDAI_BTN_RESUME = 1,
HYUNDAI_BTN_SET = 2,
HYUNDAI_BTN_CANCEL = 4,
};
enum {
HYUNDAI_PARAM_SP_ESCC = 1,
HYUNDAI_PARAM_SP_LONGITUDINAL_MAIN_CRUISE_TOGGLEABLE = 2,
HYUNDAI_PARAM_SP_HAS_LDA_BUTTON = 4,
HYUNDAI_PARAM_SP_NON_SCC = 8,
};
// common state
extern bool hyundai_ev_gas_signal;
bool hyundai_ev_gas_signal = false;
extern bool hyundai_hybrid_gas_signal;
bool hyundai_hybrid_gas_signal = false;
extern bool hyundai_longitudinal;
bool hyundai_longitudinal = false;
extern bool hyundai_camera_scc;
bool hyundai_camera_scc = false;
extern bool hyundai_canfd_lka_steer_msg;
bool hyundai_canfd_lka_steer_msg = false;
extern bool hyundai_alt_limits;
bool hyundai_alt_limits = false;
extern bool hyundai_fcev_gas_signal;
bool hyundai_fcev_gas_signal = false;
extern bool hyundai_alt_limits_2;
bool hyundai_alt_limits_2 = false;
// ESCC
extern bool hyundai_escc;
bool hyundai_escc = false;
extern bool hyundai_longitudinal_main_cruise_toggleable;
bool hyundai_longitudinal_main_cruise_toggleable = false;
extern bool hyundai_has_lda_button;
bool hyundai_has_lda_button = false;
extern bool hyundai_non_scc;
bool hyundai_non_scc = false;
static uint8_t hyundai_last_button_interaction; // button messages since the user pressed an enable button
static bool main_button_prev;
static bool acc_main_on_prev;
static bool acc_main_on_tx;
static uint32_t acc_main_on_mismatches;
void hyundai_common_init(uint16_t param) {
const uint16_t HYUNDAI_PARAM_EV_GAS = 1;
const uint16_t HYUNDAI_PARAM_HYBRID_GAS = 2;
const uint16_t HYUNDAI_PARAM_CAMERA_SCC = 8;
const uint16_t HYUNDAI_PARAM_CANFD_LKA_STEER_MSG = 16;
const uint16_t HYUNDAI_PARAM_ALT_LIMITS = 64; // TODO: shift this down with the rest of the common flags
const uint16_t HYUNDAI_PARAM_FCEV_GAS = 256;
const uint16_t HYUNDAI_PARAM_ALT_LIMITS_2 = 512;
hyundai_ev_gas_signal = GET_FLAG(param, HYUNDAI_PARAM_EV_GAS);
hyundai_hybrid_gas_signal = !hyundai_ev_gas_signal && GET_FLAG(param, HYUNDAI_PARAM_HYBRID_GAS);
hyundai_camera_scc = GET_FLAG(param, HYUNDAI_PARAM_CAMERA_SCC);
hyundai_canfd_lka_steer_msg = GET_FLAG(param, HYUNDAI_PARAM_CANFD_LKA_STEER_MSG);
hyundai_alt_limits = GET_FLAG(param, HYUNDAI_PARAM_ALT_LIMITS);
hyundai_fcev_gas_signal = GET_FLAG(param, HYUNDAI_PARAM_FCEV_GAS);
hyundai_alt_limits_2 = GET_FLAG(param, HYUNDAI_PARAM_ALT_LIMITS_2);
hyundai_escc = GET_FLAG(current_safety_param_sp, HYUNDAI_PARAM_SP_ESCC);
hyundai_longitudinal_main_cruise_toggleable = GET_FLAG(current_safety_param_sp, HYUNDAI_PARAM_SP_LONGITUDINAL_MAIN_CRUISE_TOGGLEABLE);
hyundai_has_lda_button = GET_FLAG(current_safety_param_sp, HYUNDAI_PARAM_SP_HAS_LDA_BUTTON);
hyundai_non_scc = GET_FLAG(current_safety_param_sp, HYUNDAI_PARAM_SP_NON_SCC);
hyundai_last_button_interaction = HYUNDAI_PREV_BUTTON_SAMPLES;
main_button_prev = false;
acc_main_on_prev = false;
acc_main_on_tx = false;
acc_main_on_mismatches = 0U;
#ifdef ALLOW_DEBUG
const uint16_t HYUNDAI_PARAM_LONGITUDINAL = 4;
hyundai_longitudinal = GET_FLAG(param, HYUNDAI_PARAM_LONGITUDINAL);
#else
hyundai_longitudinal = false;
#endif
}
void hyundai_common_cruise_state_check(const bool cruise_engaged) {
// some newer HKG models can re-enable after spamming cancel button,
// so keep track of user button presses to deny engagement if no interaction
// enter controls on rising edge of ACC and recent user button press, exit controls when ACC off
if (!hyundai_longitudinal) {
if (cruise_engaged && !cruise_engaged_prev && (hyundai_last_button_interaction < HYUNDAI_PREV_BUTTON_SAMPLES)) {
controls_allowed = true;
}
if (!cruise_engaged) {
controls_allowed = false;
}
cruise_engaged_prev = cruise_engaged;
}
}
void hyundai_common_cruise_buttons_check(const int cruise_button, const bool main_button) {
if ((cruise_button == HYUNDAI_BTN_RESUME) || (cruise_button == HYUNDAI_BTN_SET) || (cruise_button == HYUNDAI_BTN_CANCEL) || main_button) {
hyundai_last_button_interaction = 0U;
} else {
hyundai_last_button_interaction = SAFETY_MIN(hyundai_last_button_interaction + 1U, HYUNDAI_PREV_BUTTON_SAMPLES);
}
if (hyundai_longitudinal) {
// enter controls on falling edge of resume or set
bool set = (cruise_button != HYUNDAI_BTN_SET) && (cruise_button_prev == HYUNDAI_BTN_SET);
bool res = (cruise_button != HYUNDAI_BTN_RESUME) && (cruise_button_prev == HYUNDAI_BTN_RESUME);
if (set || res) {
controls_allowed = true;
}
// exit controls on cancel press
if (cruise_button == HYUNDAI_BTN_CANCEL) {
controls_allowed = false;
}
// toggle main cruise state on rising edge of main cruise button
if (main_button && !main_button_prev && hyundai_longitudinal_main_cruise_toggleable) {
acc_main_on = !acc_main_on;
}
cruise_button_prev = cruise_button;
main_button_prev = main_button;
}
}
uint32_t hyundai_common_canfd_compute_checksum(const CANPacket_t *msg) {
int len = GET_LEN(msg);
uint32_t address = msg->addr;
uint16_t crc = 0;
for (int i = 2; i < len; i++) {
crc = (crc << 8U) ^ hyundai_canfd_crc_lut[(crc >> 8U) ^ msg->data[i]];
}
// Add address to crc
crc = (crc << 8U) ^ hyundai_canfd_crc_lut[(crc >> 8U) ^ ((address >> 0U) & 0xFFU)];
crc = (crc << 8U) ^ hyundai_canfd_crc_lut[(crc >> 8U) ^ ((address >> 8U) & 0xFFU)];
if (len == 24) {
crc ^= 0x819dU;
} else if (len == 32) {
crc ^= 0x9f5bU;
} else {
}
return crc;
}
// reset mismatches on rising edge of acc_main_on to avoid rare race conditions when using non-PCM main cruise state
void hyundai_common_reset_acc_main_on_mismatches(void) {
if (acc_main_on && !acc_main_on_prev) {
acc_main_on_mismatches = 0U;
}
acc_main_on_prev = acc_main_on;
}
// exit lateral controls allowed if sunnypilot and panda main cruise states are desynced
void hyundai_common_acc_main_on_sync(void) {
if (acc_main_on && !acc_main_on_tx) {
acc_main_on_mismatches += 1U;
if (acc_main_on_mismatches >= 3U) { // desync by 3 frames
acc_main_on = false;
mads_exit_controls(MADS_DISENGAGE_REASON_NON_PCM_ACC_MAIN_DESYNC);
}
} else {
acc_main_on_mismatches = 0U;
}
}
uint32_t get_acc_main_on_mismatches(void) {
return acc_main_on_mismatches;
}
+106
View File
@@ -0,0 +1,106 @@
#pragma once
#include "opendbc/safety/declarations.h"
// CAN msgs we care about
#define MAZDA_LKAS 0x243U
#define MAZDA_LKAS_HUD 0x440U
#define MAZDA_CRZ_CTRL 0x21cU
#define MAZDA_CRZ_BTNS 0x09dU
#define MAZDA_STEER_TORQUE 0x240U
#define MAZDA_ENGINE_DATA 0x202U
#define MAZDA_PEDALS 0x165U
// CAN bus numbers
#define MAZDA_MAIN 0
#define MAZDA_CAM 2
// track msgs coming from OP so that we know what CAM msgs to drop and what to forward
static void mazda_rx_hook(const CANPacket_t *msg) {
if ((int)msg->bus == MAZDA_MAIN) {
if (msg->addr == MAZDA_ENGINE_DATA) {
// sample speed: scale by 0.01 to get kph
int speed = (msg->data[2] << 8) | msg->data[3];
vehicle_moving = speed > 10; // moving when speed > 0.1 kph
}
if (msg->addr == MAZDA_STEER_TORQUE) {
int torque_driver_new = msg->data[0] - 127U;
// update array of samples
update_sample(&torque_driver, torque_driver_new);
}
// enter controls on rising edge of ACC, exit controls on ACC off
if (msg->addr == MAZDA_CRZ_CTRL) {
bool cruise_engaged = msg->data[0] & 0x8U;
pcm_cruise_check(cruise_engaged);
acc_main_on = GET_BIT(msg, 17U);
}
if (msg->addr == MAZDA_ENGINE_DATA) {
gas_pressed = (msg->data[4] || (msg->data[5] & 0xF0U));
}
if (msg->addr == MAZDA_PEDALS) {
brake_pressed = (msg->data[0] & 0x10U);
}
}
}
static bool mazda_tx_hook(const CANPacket_t *msg) {
const TorqueSteeringLimits MAZDA_STEERING_LIMITS = {
.max_torque = 800,
.max_rate_up = 10,
.max_rate_down = 25,
.max_rt_delta = 300,
.driver_torque_multiplier = 1,
.driver_torque_allowance = 15,
.type = TorqueDriverLimited,
};
bool tx = true;
// Check if msg is sent on the main BUS
if (msg->bus == (unsigned char)MAZDA_MAIN) {
// steer cmd checks
if (msg->addr == MAZDA_LKAS) {
int desired_torque = (((msg->data[0] & 0x0FU) << 8) | msg->data[1]) - 2048U;
if (steer_torque_cmd_checks(desired_torque, -1, MAZDA_STEERING_LIMITS)) {
tx = false;
}
}
// cruise buttons check
if (msg->addr == MAZDA_CRZ_BTNS) {
// allow resume spamming while controls allowed, but
// only allow cancel while controls not allowed
bool cancel_cmd = (msg->data[0] == 0x1U);
if (!controls_allowed && !cancel_cmd) {
tx = false;
}
}
}
return tx;
}
static safety_config mazda_init(uint16_t param) {
static const CanMsg MAZDA_TX_MSGS[] = {{MAZDA_LKAS, 0, 8, .check_relay = true}, {MAZDA_CRZ_BTNS, 0, 8, .check_relay = false}, {MAZDA_LKAS_HUD, 0, 8, .check_relay = true}};
static RxCheck mazda_rx_checks[] = {
{.msg = {{MAZDA_CRZ_CTRL, 0, 8, 50U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}},
{.msg = {{MAZDA_CRZ_BTNS, 0, 8, 10U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}},
{.msg = {{MAZDA_STEER_TORQUE, 0, 8, 83U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}},
{.msg = {{MAZDA_ENGINE_DATA, 0, 8, 100U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}},
{.msg = {{MAZDA_PEDALS, 0, 8, 50U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}},
};
SAFETY_UNUSED(param);
return BUILD_SAFETY_CFG(mazda_rx_checks, MAZDA_TX_MSGS);
}
const safety_hooks mazda_hooks = {
.init = mazda_init,
.rx = mazda_rx_hook,
.tx = mazda_tx_hook,
};
+190
View File
@@ -0,0 +1,190 @@
#pragma once
#include "opendbc/safety/declarations.h"
#define NISSAN_COMMON_RX_CHECKS \
{.msg = {{0x185, 0, 8, 100U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}}, /* STEER_TORQUE_SENSOR */ \
{.msg = {{0x285, 0, 8, 50U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, \
{0x285, 1, 8, 50U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }}}, /* WHEEL_SPEEDS_REAR */ \
{.msg = {{0x30f, 2, 3, 10U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, \
{0x30f, 1, 3, 10U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }}}, /* CRUISE_STATE */ \
{.msg = {{0x15c, 0, 8, 50U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, \
{0x15c, 1, 8, 50U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, \
{0x239, 0, 8, 50U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}}}, /* GAS_PEDAL */ \
{.msg = {{0x454, 0, 8, 10U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, \
{0x454, 1, 8, 10U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, \
{0x1cc, 0, 4, 100U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}}}, /* DOORS_LIGHTS / BRAKE */ \
#define NISSAN_PRO_PILOT_RX_CHECKS(alt_eps_bus) \
{.msg = {{0x1B6, alt_eps_bus, 8, 10U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \
static bool nissan_alt_eps = false;
static const AngleSteeringLimits NISSAN_STEERING_LIMITS = {
.max_angle = 60000, // 600 deg, reasonable limit
.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}
},
};
static void nissan_rx_hook(const CANPacket_t *msg) {
// Altima: on camera bus, others: on pt bus
if (msg->bus == 0U) {
if (msg->addr == 0x185U) {
// Current steering angle
int angle_meas_new = (msg->data[2] << 10) | (msg->data[3] << 2) | (msg->data[4] >> 6);
// Factor is -0.01, offset is 1310. Flip to correct sign, but keep units in CAN scale
angle_meas_new = -angle_meas_new + (1310.0f * NISSAN_STEERING_LIMITS.angle_deg_to_can);
// update array of samples
update_sample(&angle_meas, angle_meas_new);
}
}
if (msg->bus == (nissan_alt_eps ? 1U : 0U)) {
if (msg->addr == 0x285U) {
// Get current speed and standstill
uint16_t right_rear = (msg->data[0] << 8) | (msg->data[1]);
uint16_t left_rear = (msg->data[2] << 8) | (msg->data[3]);
vehicle_moving = (right_rear | left_rear) != 0U;
UPDATE_VEHICLE_SPEED((right_rear + left_rear) / 2.0 * 0.005 * KPH_TO_MS);
}
// X-Trail 0x15c, Leaf 0x239
if ((msg->addr == 0x15cU) || (msg->addr == 0x239U)) {
if (msg->addr == 0x15cU){
gas_pressed = ((msg->data[5] << 2) | ((msg->data[6] >> 6) & 0x3U)) > 3U;
} else {
gas_pressed = msg->data[0] > 3U;
}
}
// X-trail 0x454, Leaf 0x239
if ((msg->addr == 0x454U) || (msg->addr == 0x239U)) {
if (msg->addr == 0x454U){
brake_pressed = (msg->data[2] & 0x80U) != 0U;
} else {
brake_pressed = ((msg->data[4] >> 5) & 1U) != 0U;
}
}
}
// Handle cruise enabled
if ((msg->addr == 0x30fU) && (msg->bus == (nissan_alt_eps ? 1U : 2U))) {
bool cruise_engaged = (msg->data[0] >> 3) & 1U;
pcm_cruise_check(cruise_engaged);
}
if ((msg->addr == 0x239U) && (msg->bus == 0U)) {
acc_main_on = GET_BIT(msg, 17U);
}
if ((msg->addr == 0x1B6U) && (msg->bus == (nissan_alt_eps ? 2U : 1U))) {
acc_main_on = GET_BIT(msg, 36U);
}
}
static bool nissan_tx_hook(const CANPacket_t *msg) {
bool tx = true;
bool violation = false;
// steer cmd checks
if (msg->addr == 0x169U) {
int desired_angle = ((msg->data[0] << 10) | (msg->data[1] << 2) | ((msg->data[2] >> 6) & 0x3U));
bool lka_active = (msg->data[6] >> 4) & 1U;
// Factor is -0.01, offset is 1310. Flip to correct sign, but keep units in CAN scale
desired_angle = -desired_angle + (1310.0f * NISSAN_STEERING_LIMITS.angle_deg_to_can);
if (steer_angle_cmd_checks(desired_angle, lka_active, NISSAN_STEERING_LIMITS)) {
violation = true;
}
}
// acc button check, only allow cancel button to be sent
if (msg->addr == 0x20bU) {
// Violation of any button other than cancel is pressed
violation |= ((msg->data[1] & 0x3dU) > 0U);
}
if (violation) {
tx = false;
}
return tx;
}
static safety_config nissan_init(uint16_t param) {
static const CanMsg NISSAN_TX_MSGS_LEAF[] = {
{0x169, 0, 8, .check_relay = true}, // LKAS
{0x2b1, 0, 8, .check_relay = true}, // PROPILOT_HUD
{0x4cc, 0, 8, .check_relay = true}, // PROPILOT_HUD_INFO_MSG
{0x239, 2, 8, .check_relay = true} // CRUISE_THROTTLE (Leaf)
};
static const CanMsg NISSAN_TX_MSGS_XTRAIL[] = {
{0x169, 0, 8, .check_relay = true}, // LKAS
{0x2b1, 0, 8, .check_relay = true}, // PROPILOT_HUD
{0x4cc, 0, 8, .check_relay = true}, // PROPILOT_HUD_INFO_MSG
{0x20b, 2, 6, .check_relay = false} // CRUISE_THROTTLE (X-Trail)
};
static const CanMsg NISSAN_TX_MSGS_ALTIMA[] = {
{0x169, 0, 8, .check_relay = true}, // LKAS
{0x2b1, 0, 8, .check_relay = true}, // PROPILOT_HUD
{0x4cc, 0, 8, .check_relay = true}, // PROPILOT_HUD_INFO_MSG
{0x20b, 1, 6, .check_relay = false} // CRUISE_THROTTLE (Altima)
};
// Signals duplicated below due to the fact that these messages can come in on either CAN bus, depending on car model.
static RxCheck nissan_rx_checks[] = {
NISSAN_COMMON_RX_CHECKS
NISSAN_PRO_PILOT_RX_CHECKS(1)
};
static RxCheck nissan_alt_eps_rx_checks[] = {
NISSAN_COMMON_RX_CHECKS
NISSAN_PRO_PILOT_RX_CHECKS(2)
};
static RxCheck nissan_leaf_rx_checks[] = {
NISSAN_COMMON_RX_CHECKS
};
// EPS Location. false = V-CAN, true = C-CAN
const uint16_t NISSAN_PARAM_ALT_EPS_BUS = 1;
const uint16_t NISSAN_PARAM_SP_LEAF = 1;
nissan_alt_eps = GET_FLAG(param, NISSAN_PARAM_ALT_EPS_BUS);
const bool nissan_leaf = GET_FLAG(current_safety_param_sp, NISSAN_PARAM_SP_LEAF);
safety_config ret;
if (nissan_leaf) {
SET_TX_MSGS(NISSAN_TX_MSGS_LEAF, ret);
SET_RX_CHECKS(nissan_leaf_rx_checks, ret);
} else if (nissan_alt_eps) {
SET_TX_MSGS(NISSAN_TX_MSGS_ALTIMA, ret);
SET_RX_CHECKS(nissan_alt_eps_rx_checks, ret);
} else {
SET_TX_MSGS(NISSAN_TX_MSGS_XTRAIL, ret);
SET_RX_CHECKS(nissan_rx_checks, ret);
}
return ret;
}
const safety_hooks nissan_hooks = {
.init = nissan_init,
.rx = nissan_rx_hook,
.tx = nissan_tx_hook,
};
+144
View File
@@ -0,0 +1,144 @@
#pragma once
#include "opendbc/safety/declarations.h"
#define PSA_STEERING 757U // RX from XXX, driver torque
#define PSA_STEERING_ALT 773U // RX from EPS, steering angle
#define PSA_DYN_CMM 520U // RX from CMM, gas pedal
#define PSA_HS2_DYN_ABR_38D 909U // RX from UC_FREIN, speed
#define PSA_HS2_DAT_MDD_CMD_452 1106U // RX from BSI, cruise state
#define PSA_DAT_BSI 1042U // RX from BSI, brake
#define PSA_LANE_KEEP_ASSIST 1010U // TX from OP, EPS
// CAN bus
#define PSA_MAIN_BUS 0U
#define PSA_ADAS_BUS 1U
#define PSA_CAM_BUS 2U
static uint8_t psa_get_counter(const CANPacket_t *msg) {
uint8_t cnt = 0;
if (msg->addr == PSA_HS2_DAT_MDD_CMD_452) {
cnt = (msg->data[3] >> 4) & 0xFU;
} else if (msg->addr == PSA_HS2_DYN_ABR_38D) {
cnt = (msg->data[5] >> 4) & 0xFU;
} else {
}
return cnt;
}
static uint32_t psa_get_checksum(const CANPacket_t *msg) {
return msg->data[5] & 0xFU;
}
static uint8_t _psa_compute_checksum(const CANPacket_t *msg, uint8_t chk_ini, int chk_pos) {
int len = GET_LEN(msg);
uint8_t sum = 0;
for (int i = 0; i < len; i++) {
uint8_t b = msg->data[i];
if (i == chk_pos) {
// set checksum in low nibble to 0
b &= 0xF0U;
}
sum += (b >> 4) + (b & 0xFU);
}
return (chk_ini - sum) & 0xFU;
}
static uint32_t psa_compute_checksum(const CANPacket_t *msg) {
uint8_t chk = 0;
if (msg->addr == PSA_HS2_DAT_MDD_CMD_452) {
chk = _psa_compute_checksum(msg, 0x4, 5);
} else if (msg->addr == PSA_HS2_DYN_ABR_38D) {
chk = _psa_compute_checksum(msg, 0x7, 5);
} else {
}
return chk;
}
static void psa_rx_hook(const CANPacket_t *msg) {
if (msg->bus == PSA_MAIN_BUS) {
if (msg->addr == PSA_DYN_CMM) {
gas_pressed = msg->data[3] > 0U; // P002_Com_rAPP
}
if (msg->addr == PSA_STEERING_ALT) {
int angle_meas_new = to_signed((msg->data[0] << 8) | msg->data[1], 16); // ANGLE
update_sample(&angle_meas, angle_meas_new);
}
if (msg->addr == PSA_HS2_DYN_ABR_38D) {
int speed = (msg->data[0] << 8) | msg->data[1];
vehicle_moving = speed > 0;
UPDATE_VEHICLE_SPEED(speed * 0.01 * KPH_TO_MS); // VITESSE_VEHICULE_ROUES
}
}
if (msg->bus == PSA_ADAS_BUS) {
if (msg->addr == PSA_HS2_DAT_MDD_CMD_452) {
pcm_cruise_check((msg->data[2U] >> 7U) & 1U); // RVV_ACC_ACTIVATION_REQ
}
}
if (msg->bus == PSA_CAM_BUS) {
if (msg->addr == PSA_DAT_BSI) {
brake_pressed = (msg->data[0U] >> 5U) & 1U; // P013_MainBrake
}
}
}
static bool psa_tx_hook(const CANPacket_t *msg) {
bool tx = true;
static const AngleSteeringLimits PSA_STEERING_LIMITS = {
.max_angle = 3900,
.angle_deg_to_can = 10,
.angle_rate_up_lookup = {
{0., 5., 25.},
{2.5, 1.5, .2},
},
.angle_rate_down_lookup = {
{0., 5., 25.},
{5., 2., .3},
},
};
// Safety check for LKA
if (msg->addr == PSA_LANE_KEEP_ASSIST) {
// SET_ANGLE
int desired_angle = to_signed((msg->data[6] << 6) | ((msg->data[7] & 0xFCU) >> 2), 14);
// TORQUE_FACTOR
bool lka_active = ((msg->data[5] & 0xFEU) >> 1) == 100U;
if (steer_angle_cmd_checks(desired_angle, lka_active, PSA_STEERING_LIMITS)) {
tx = false;
}
}
return tx;
}
static safety_config psa_init(uint16_t param) {
SAFETY_UNUSED(param);
static const CanMsg PSA_TX_MSGS[] = {
{PSA_LANE_KEEP_ASSIST, PSA_MAIN_BUS, 8, .check_relay = true}, // EPS steering
};
static RxCheck psa_rx_checks[] = {
{.msg = {{PSA_HS2_DAT_MDD_CMD_452, PSA_ADAS_BUS, 6, 20U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}}, // cruise state
{.msg = {{PSA_HS2_DYN_ABR_38D, PSA_MAIN_BUS, 8, 25U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}}, // speed
{.msg = {{PSA_STEERING_ALT, PSA_MAIN_BUS, 7, 100U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}}, // steering angle
{.msg = {{PSA_STEERING, PSA_MAIN_BUS, 7, 100U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}}, // driver torque
{.msg = {{PSA_DYN_CMM, PSA_MAIN_BUS, 8, 100U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}}, // gas pedal
{.msg = {{PSA_DAT_BSI, PSA_CAM_BUS, 8, 20U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}}, // brake
};
return BUILD_SAFETY_CFG(psa_rx_checks, PSA_TX_MSGS);
}
const safety_hooks psa_hooks = {
.init = psa_init,
.rx = psa_rx_hook,
.tx = psa_tx_hook,
.get_counter = psa_get_counter,
.get_checksum = psa_get_checksum,
.compute_checksum = psa_compute_checksum,
};
+182
View File
@@ -0,0 +1,182 @@
#pragma once
#include "opendbc/safety/declarations.h"
static uint8_t rivian_get_counter(const CANPacket_t *msg) {
// Signal: ESP_Status_Counter, VDM_PropStatus_Counter
return msg->data[1] & 0xFU;
}
static uint32_t rivian_get_checksum(const CANPacket_t *msg) {
// Signal: ESP_Status_Checksum, VDM_PropStatus_Checksum
return msg->data[0];
}
static uint8_t _rivian_compute_checksum(const CANPacket_t *msg, uint8_t poly, uint8_t xor_output) {
int len = GET_LEN(msg);
uint8_t crc = 0;
// Skip the checksum byte
for (int i = 1; i < len; i++) {
crc ^= msg->data[i];
for (int j = 0; j < 8; j++) {
if ((crc & 0x80U) != 0U) {
crc = (crc << 1) ^ poly;
} else {
crc <<= 1;
}
}
}
return crc ^ xor_output;
}
static uint32_t rivian_compute_checksum(const CANPacket_t *msg) {
uint8_t chksum = 0;
if (msg->addr == 0x208U) {
chksum = _rivian_compute_checksum(msg, 0x1D, 0xB1);
} else if (msg->addr == 0x150U) {
chksum = _rivian_compute_checksum(msg, 0x1D, 0x9A);
} else {
}
return chksum;
}
static bool rivian_get_quality_flag_valid(const CANPacket_t *msg) {
bool valid = false;
if (msg->addr == 0x208U) {
valid = ((msg->data[3] >> 3) & 0x3U) == 0x1U; // ESP_Vehicle_Speed_Q
} else if (msg->addr == 0x150U) {
valid = (msg->data[1] >> 6) == 0x1U; // VDM_VehicleSpeedQ
} else {
}
return valid;
}
static void rivian_rx_hook(const CANPacket_t *msg) {
if (msg->bus == 0U) {
// Vehicle speed
if (msg->addr == 0x208U) {
float speed = ((msg->data[6] << 8) | msg->data[7]) * 0.01;
vehicle_moving = speed > 0.0;
UPDATE_VEHICLE_SPEED(speed * KPH_TO_MS);
}
// Gas pressed and second speed source for variable torque limit
if (msg->addr == 0x150U) {
gas_pressed = msg->data[3] | (msg->data[4] & 0xC0U);
// Disable controls if speeds from VDM and ESP ECUs are too far apart.
float vdm_speed = ((msg->data[5] << 8) | msg->data[6]) * 0.01 * KPH_TO_MS;
speed_mismatch_check(vdm_speed);
}
// Driver torque
if (msg->addr == 0x380U) {
int torque_driver_new = (((msg->data[2] << 4) | (msg->data[3] >> 4))) - 2050U;
update_sample(&torque_driver, torque_driver_new);
}
// Brake pressed
if (msg->addr == 0x38fU) {
brake_pressed = (msg->data[2] >> 7) & 1U;
}
}
if (msg->bus == 2U) {
// Cruise state
if (msg->addr == 0x100U) {
const int feature_status = msg->data[2] >> 5U;
pcm_cruise_check(feature_status == 1);
}
}
}
static bool rivian_tx_hook(const CANPacket_t *msg) {
// Rivian utilizes more torque at low speed to maintain the same lateral accel
const TorqueSteeringLimits RIVIAN_STEERING_LIMITS = {
.max_torque = 350,
.dynamic_max_torque = true,
.max_torque_lookup = {
{9., 17., 17.},
{350, 250, 250},
},
.max_rate_up = 3,
.max_rate_down = 5,
.max_rt_delta = 125,
.driver_torque_multiplier = 2,
.driver_torque_allowance = 100,
.type = TorqueDriverLimited,
};
const LongitudinalLimits RIVIAN_LONG_LIMITS = {
.max_accel = 200,
.min_accel = -350,
.inactive_accel = 0,
};
bool tx = true;
if (msg->bus == 0U) {
// Steering control
if (msg->addr == 0x120U) {
int desired_torque = ((msg->data[2] << 3U) | (msg->data[3] >> 5U)) - 1024U;
bool steer_req = (msg->data[3] >> 4) & 1U;
if (steer_torque_cmd_checks(desired_torque, steer_req, RIVIAN_STEERING_LIMITS)) {
tx = false;
}
}
// Longitudinal control
if (msg->addr == 0x160U) {
int raw_accel = ((msg->data[2] << 3) | (msg->data[3] >> 5)) - 1024U;
if (longitudinal_accel_checks(raw_accel, RIVIAN_LONG_LIMITS)) {
tx = false;
}
}
}
return tx;
}
static safety_config rivian_init(uint16_t param) {
// SCCM_WheelTouch: for hiding hold wheel alert
// VDM_AdasSts: for canceling stock ACC
// 0x120 = ACM_lkaHbaCmd, 0x321 = SCCM_WheelTouch, 0x162 = VDM_AdasSts
static const CanMsg RIVIAN_TX_MSGS[] = {{0x120, 0, 8, .check_relay = true}, {0x321, 2, 7, .check_relay = true}, {0x162, 2, 8, .check_relay = true}};
// 0x160 = ACM_longitudinalRequest
static const CanMsg RIVIAN_LONG_TX_MSGS[] = {{0x120, 0, 8, .check_relay = true}, {0x321, 2, 7, .check_relay = true}, {0x160, 0, 5, .check_relay = true}};
static RxCheck rivian_rx_checks[] = {
{.msg = {{0x208, 0, 8, 50U, .max_counter = 14U}, { 0 }, { 0 }}}, // ESP_Status (speed)
{.msg = {{0x150, 0, 7, 50U, .max_counter = 14U}, { 0 }, { 0 }}}, // VDM_PropStatus (gas pedal & 2nd speed)
{.msg = {{0x380, 0, 5, 100U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}}, // EPAS_SystemStatus (driver torque)
{.msg = {{0x38f, 0, 6, 50U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}}, // iBESP2 (brakes)
{.msg = {{0x100, 2, 8, 100U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}}, // ACM_Status (cruise state)
};
bool rivian_longitudinal = false;
SAFETY_UNUSED(param);
#ifdef ALLOW_DEBUG
const int FLAG_RIVIAN_LONG_CONTROL = 1;
rivian_longitudinal = GET_FLAG(param, FLAG_RIVIAN_LONG_CONTROL);
#endif
// FIXME: cppcheck thinks that rivian_longitudinal is always false. This is not true
// if ALLOW_DEBUG is defined but cppcheck is run without ALLOW_DEBUG
// cppcheck-suppress knownConditionTrueFalse
return rivian_longitudinal ? BUILD_SAFETY_CFG(rivian_rx_checks, RIVIAN_LONG_TX_MSGS) : \
BUILD_SAFETY_CFG(rivian_rx_checks, RIVIAN_TX_MSGS);
}
const safety_hooks rivian_hooks = {
.init = rivian_init,
.rx = rivian_rx_hook,
.tx = rivian_tx_hook,
.get_counter = rivian_get_counter,
.get_checksum = rivian_get_checksum,
.compute_checksum = rivian_compute_checksum,
.get_quality_flag_valid = rivian_get_quality_flag_valid,
};
+282
View File
@@ -0,0 +1,282 @@
#pragma once
#include "opendbc/safety/declarations.h"
#include "opendbc/safety/modes/subaru_common.h"
#define SUBARU_STEERING_LIMITS_GENERATOR(steer_max, rate_up, rate_down) \
{ \
.max_torque = (steer_max), \
.max_rt_delta = 940, \
.max_rate_up = (rate_up), \
.max_rate_down = (rate_down), \
.driver_torque_multiplier = 50, \
.driver_torque_allowance = 60, \
.type = TorqueDriverLimited, \
/* the EPS will temporary fault if the steering rate is too high, so we cut the \
the steering torque every 7 frames for 1 frame if the steering rate is high */ \
.min_valid_request_frames = 7, \
.max_invalid_request_frames = 1, \
.min_valid_request_rt_interval = 144000, /* 10% tolerance */ \
.has_steer_req_tolerance = true, \
}
#define MSG_SUBARU_Brake_Status 0x13cU
#define MSG_SUBARU_CruiseControl 0x240U
#define MSG_SUBARU_Throttle 0x40U
#define MSG_SUBARU_Steering_Torque 0x119U
#define MSG_SUBARU_Wheel_Speeds 0x13aU
#define MSG_SUBARU_Brake_Pedal 0x139U
#define MSG_SUBARU_ES_LKAS 0x122U
#define MSG_SUBARU_ES_Brake 0x220U
#define MSG_SUBARU_ES_Distance 0x221U
#define MSG_SUBARU_ES_Status 0x222U
#define MSG_SUBARU_ES_DashStatus 0x321U
#define MSG_SUBARU_ES_LKAS_State 0x322U
#define MSG_SUBARU_ES_Infotainment 0x323U
#define MSG_SUBARU_ES_UDS_Request 0x787U
#define MSG_SUBARU_ES_HighBeamAssist 0x22AU
#define MSG_SUBARU_ES_STATIC_1 0x325U
#define MSG_SUBARU_ES_STATIC_2 0x121U
#define SUBARU_MAIN_BUS 0U
#define SUBARU_ALT_BUS 1U
#define SUBARU_CAM_BUS 2U
#define SUBARU_BASE_TX_MSGS(alt_bus, lkas_msg) \
{lkas_msg, SUBARU_MAIN_BUS, 8, .check_relay = true}, \
{MSG_SUBARU_ES_DashStatus, SUBARU_MAIN_BUS, 8, .check_relay = true}, \
{MSG_SUBARU_ES_LKAS_State, SUBARU_MAIN_BUS, 8, .check_relay = true}, \
{MSG_SUBARU_ES_Infotainment, SUBARU_MAIN_BUS, 8, .check_relay = true}, \
#define SUBARU_COMMON_TX_MSGS(alt_bus) \
{MSG_SUBARU_ES_Distance, alt_bus, 8, .check_relay = false}, \
#define SUBARU_COMMON_LONG_TX_MSGS(alt_bus) \
{MSG_SUBARU_ES_Distance, alt_bus, 8, .check_relay = true}, \
{MSG_SUBARU_ES_Brake, alt_bus, 8, .check_relay = true}, \
{MSG_SUBARU_ES_Status, alt_bus, 8, .check_relay = true}, \
#define SUBARU_GEN2_LONG_ADDITIONAL_TX_MSGS() \
{MSG_SUBARU_ES_UDS_Request, SUBARU_CAM_BUS, 8, .check_relay = false}, \
{MSG_SUBARU_ES_HighBeamAssist, SUBARU_MAIN_BUS, 8, .check_relay = false}, \
{MSG_SUBARU_ES_STATIC_1, SUBARU_MAIN_BUS, 8, .check_relay = false}, \
{MSG_SUBARU_ES_STATIC_2, SUBARU_MAIN_BUS, 8, .check_relay = false}, \
#define SUBARU_STOP_AND_GO_TX_MSGS \
{MSG_SUBARU_Throttle, SUBARU_CAM_BUS, 8, .check_relay = true}, \
{MSG_SUBARU_Brake_Pedal, SUBARU_CAM_BUS, 8, .check_relay = true}, \
#define SUBARU_COMMON_RX_CHECKS(alt_bus) \
{.msg = {{MSG_SUBARU_Throttle, SUBARU_MAIN_BUS, 8, 100U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \
{.msg = {{MSG_SUBARU_Steering_Torque, SUBARU_MAIN_BUS, 8, 50U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \
{.msg = {{MSG_SUBARU_Wheel_Speeds, alt_bus, 8, 50U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \
{.msg = {{MSG_SUBARU_Brake_Status, alt_bus, 8, 50U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \
{.msg = {{MSG_SUBARU_CruiseControl, alt_bus, 8, 20U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \
{.msg = {{MSG_SUBARU_ES_LKAS_State, SUBARU_CAM_BUS, 8, 10U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \
static bool subaru_gen2 = false;
static bool subaru_longitudinal = false;
static uint32_t subaru_get_checksum(const CANPacket_t *msg) {
return (uint8_t)msg->data[0];
}
static uint8_t subaru_get_counter(const CANPacket_t *msg) {
return (uint8_t)(msg->data[1] & 0xFU);
}
static uint32_t subaru_compute_checksum(const CANPacket_t *msg) {
int len = GET_LEN(msg);
uint8_t checksum = (uint8_t)(msg->addr) + (uint8_t)((unsigned int)(msg->addr) >> 8U);
for (int i = 1; i < len; i++) {
checksum += (uint8_t)msg->data[i];
}
return checksum;
}
static void subaru_rx_hook(const CANPacket_t *msg) {
const unsigned int alt_main_bus = subaru_gen2 ? SUBARU_ALT_BUS : SUBARU_MAIN_BUS;
if ((msg->addr == MSG_SUBARU_Steering_Torque) && (msg->bus == SUBARU_MAIN_BUS)) {
int torque_driver_new;
torque_driver_new = ((GET_BYTES(msg, 0, 4) >> 16) & 0x7FFU);
torque_driver_new = -1 * to_signed(torque_driver_new, 11);
update_sample(&torque_driver, torque_driver_new);
}
if ((msg->addr == MSG_SUBARU_ES_LKAS_State) && (msg->bus == SUBARU_CAM_BUS)) {
int lkas_hud = (msg->data[2] & 0x0CU) >> 2U;
if ((lkas_hud >= 1) && (lkas_hud <= 3)) {
mads_button_press = MADS_BUTTON_PRESSED;
}
}
// enter controls on rising edge of ACC, exit controls on ACC off
if ((msg->addr == MSG_SUBARU_CruiseControl) && (msg->bus == alt_main_bus)) {
bool cruise_engaged = (msg->data[5] >> 1) & 1U;
pcm_cruise_check(cruise_engaged);
acc_main_on = GET_BIT(msg, 40U);
}
// update vehicle moving with any non-zero wheel speed
if ((msg->addr == MSG_SUBARU_Wheel_Speeds) && (msg->bus == alt_main_bus)) {
uint32_t fr = (GET_BYTES(msg, 1, 3) >> 4) & 0x1FFFU;
uint32_t rr = (GET_BYTES(msg, 3, 3) >> 1) & 0x1FFFU;
uint32_t rl = (GET_BYTES(msg, 4, 3) >> 6) & 0x1FFFU;
uint32_t fl = (GET_BYTES(msg, 6, 2) >> 3) & 0x1FFFU;
vehicle_moving = (fr > 0U) || (rr > 0U) || (rl > 0U) || (fl > 0U);
UPDATE_VEHICLE_SPEED((fr + rr + rl + fl) / 4.0 * 0.057 * KPH_TO_MS);
}
if ((msg->addr == MSG_SUBARU_Brake_Status) && (msg->bus == alt_main_bus)) {
brake_pressed = (msg->data[7] >> 6) & 1U;
}
if ((msg->addr == MSG_SUBARU_Throttle) && (msg->bus == SUBARU_MAIN_BUS)) {
gas_pressed = msg->data[4] != 0U;
}
}
static bool subaru_tx_hook(const CANPacket_t *msg) {
const TorqueSteeringLimits SUBARU_STEERING_LIMITS = SUBARU_STEERING_LIMITS_GENERATOR(2047, 50, 70);
const TorqueSteeringLimits SUBARU_GEN2_STEERING_LIMITS = SUBARU_STEERING_LIMITS_GENERATOR(1500, 35, 50);
const LongitudinalLimits SUBARU_LONG_LIMITS = {
.min_gas = 808, // appears to be engine braking
.max_gas = 3400, // approx 2 m/s^2 when maxing cruise_rpm and cruise_throttle
.inactive_gas = 1818, // this is zero acceleration
.max_brake = 600, // approx -3.5 m/s^2
.min_transmission_rpm = 0,
.max_transmission_rpm = 3600,
};
bool tx = true;
bool violation = false;
// steer cmd checks
if (msg->addr == MSG_SUBARU_ES_LKAS) {
int desired_torque = ((GET_BYTES(msg, 0, 4) >> 16) & 0x1FFFU);
desired_torque = -1 * to_signed(desired_torque, 13);
bool steer_req = (msg->data[3] >> 5) & 1U;
const TorqueSteeringLimits limits = subaru_gen2 ? SUBARU_GEN2_STEERING_LIMITS : SUBARU_STEERING_LIMITS;
violation |= steer_torque_cmd_checks(desired_torque, steer_req, limits);
}
// check es_brake brake_pressure limits
if (msg->addr == MSG_SUBARU_ES_Brake) {
int es_brake_pressure = GET_BYTES(msg, 2, 2);
violation |= longitudinal_brake_checks(es_brake_pressure, SUBARU_LONG_LIMITS);
}
// check es_distance cruise_throttle limits
if (msg->addr == MSG_SUBARU_ES_Distance) {
int cruise_throttle = (GET_BYTES(msg, 2, 2) & 0x1FFFU);
bool cruise_cancel = (msg->data[7] >> 0) & 1U;
if (subaru_longitudinal) {
violation |= longitudinal_gas_checks(cruise_throttle, SUBARU_LONG_LIMITS);
} else {
// If openpilot is not controlling long, only allow ES_Distance for cruise cancel requests,
// (when Cruise_Cancel is true, and Cruise_Throttle is inactive)
violation |= (cruise_throttle != SUBARU_LONG_LIMITS.inactive_gas);
violation |= (!cruise_cancel);
}
}
// check es_status transmission_rpm limits
if (msg->addr == MSG_SUBARU_ES_Status) {
int transmission_rpm = (GET_BYTES(msg, 2, 2) & 0x1FFFU);
violation |= longitudinal_transmission_rpm_checks(transmission_rpm, SUBARU_LONG_LIMITS);
}
if (msg->addr == MSG_SUBARU_ES_UDS_Request) {
// tester present ('\x02\x3E\x80\x00\x00\x00\x00\x00') is allowed for gen2 longitudinal to keep eyesight disabled
bool is_tester_present = (GET_BYTES(msg, 0, 4) == 0x00803E02U) && (GET_BYTES(msg, 4, 4) == 0x0U);
// reading ES button data by identifier (b'\x03\x22\x11\x30\x00\x00\x00\x00') is also allowed (DID 0x1130)
bool is_button_rdbi = (GET_BYTES(msg, 0, 4) == 0x30112203U) && (GET_BYTES(msg, 4, 4) == 0x0U);
violation |= !(is_tester_present || is_button_rdbi);
}
if (violation){
tx = false;
}
return tx;
}
static safety_config subaru_init(uint16_t param) {
static const CanMsg SUBARU_TX_MSGS[] = {
SUBARU_BASE_TX_MSGS(SUBARU_MAIN_BUS, MSG_SUBARU_ES_LKAS)
SUBARU_COMMON_TX_MSGS(SUBARU_MAIN_BUS)
};
static const CanMsg SUBARU_LONG_TX_MSGS[] = {
SUBARU_BASE_TX_MSGS(SUBARU_MAIN_BUS, MSG_SUBARU_ES_LKAS)
SUBARU_COMMON_LONG_TX_MSGS(SUBARU_MAIN_BUS)
};
static const CanMsg SUBARU_GEN2_TX_MSGS[] = {
SUBARU_BASE_TX_MSGS(SUBARU_ALT_BUS, MSG_SUBARU_ES_LKAS)
SUBARU_COMMON_TX_MSGS(SUBARU_ALT_BUS)
};
static const CanMsg SUBARU_GEN2_LONG_TX_MSGS[] = {
SUBARU_BASE_TX_MSGS(SUBARU_ALT_BUS, MSG_SUBARU_ES_LKAS)
SUBARU_COMMON_LONG_TX_MSGS(SUBARU_ALT_BUS)
SUBARU_GEN2_LONG_ADDITIONAL_TX_MSGS()
};
static const CanMsg subaru_stop_and_go_tx_msgs[] = {
SUBARU_BASE_TX_MSGS(SUBARU_MAIN_BUS, MSG_SUBARU_ES_LKAS)
SUBARU_COMMON_TX_MSGS(SUBARU_MAIN_BUS)
SUBARU_STOP_AND_GO_TX_MSGS
};
static RxCheck subaru_rx_checks[] = {
SUBARU_COMMON_RX_CHECKS(SUBARU_MAIN_BUS)
};
static RxCheck subaru_gen2_rx_checks[] = {
SUBARU_COMMON_RX_CHECKS(SUBARU_ALT_BUS)
};
const uint16_t SUBARU_PARAM_GEN2 = 1;
subaru_gen2 = GET_FLAG(param, SUBARU_PARAM_GEN2);
subaru_common_init();
#ifdef ALLOW_DEBUG
const uint16_t SUBARU_PARAM_LONGITUDINAL = 2;
subaru_longitudinal = GET_FLAG(param, SUBARU_PARAM_LONGITUDINAL);
#endif
safety_config ret;
if (subaru_gen2) {
ret = subaru_longitudinal ? BUILD_SAFETY_CFG(subaru_gen2_rx_checks, SUBARU_GEN2_LONG_TX_MSGS) : \
BUILD_SAFETY_CFG(subaru_gen2_rx_checks, SUBARU_GEN2_TX_MSGS);
} else {
ret = subaru_longitudinal ? BUILD_SAFETY_CFG(subaru_rx_checks, SUBARU_LONG_TX_MSGS) : \
subaru_stop_and_go ? BUILD_SAFETY_CFG(subaru_rx_checks, subaru_stop_and_go_tx_msgs) : \
BUILD_SAFETY_CFG(subaru_rx_checks, SUBARU_TX_MSGS);
}
return ret;
}
const safety_hooks subaru_hooks = {
.init = subaru_init,
.rx = subaru_rx_hook,
.tx = subaru_tx_hook,
.get_counter = subaru_get_counter,
.get_checksum = subaru_get_checksum,
.compute_checksum = subaru_compute_checksum,
};
@@ -0,0 +1,30 @@
/**
* Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
*
* This file is part of sunnypilot and is licensed under the MIT License.
* See the LICENSE.md file in the root directory for more details.
*/
#pragma once
extern bool subaru_stop_and_go;
bool subaru_stop_and_go = false;
void subaru_common_init(void) {
const uint16_t SUBARU_PARAM_SP_STOP_AND_GO = 1;
subaru_stop_and_go = GET_FLAG(current_safety_param_sp, SUBARU_PARAM_SP_STOP_AND_GO);
}
/*
bool subaru_common_stop_and_go_throttle_check(const int throttle_pedal) {
bool violation = throttle_pedal != 5U || !controls_allowed || vehicle_moving;
return violation;
}
bool subaru_common_stop_and_go_brake_pedal_check(const int speed, const bool is_preglobal) {
int val = is_preglobal ? 1U : 3U;
bool violation = speed != val || !controls_allowed || vehicle_moving;
return violation;
}
*/
@@ -0,0 +1,124 @@
#pragma once
#include "opendbc/safety/declarations.h"
#include "opendbc/safety/modes/subaru_common.h"
// Preglobal platform
// 0x161 is ES_CruiseThrottle
// 0x164 is ES_LKAS
#define MSG_SUBARU_PG_CruiseControl 0x144U
#define MSG_SUBARU_PG_Throttle 0x140U
#define MSG_SUBARU_PG_Wheel_Speeds 0xD4U
#define MSG_SUBARU_PG_Brake_Pedal 0xD1U
#define MSG_SUBARU_PG_ES_LKAS 0x164U
#define MSG_SUBARU_PG_ES_Distance 0x161U
#define MSG_SUBARU_PG_Steering_Torque 0x371U
#define SUBARU_PG_MAIN_BUS 0U
#define SUBARU_PG_CAM_BUS 2U
#define SUBARU_PG_COMMON_TX_MSGS \
{MSG_SUBARU_PG_ES_Distance, SUBARU_PG_MAIN_BUS, 8, .check_relay = true}, \
{MSG_SUBARU_PG_ES_LKAS, SUBARU_PG_MAIN_BUS, 8, .check_relay = true}, \
#define SUBARU_PG_STOP_AND_GO_TX_MSGS \
{MSG_SUBARU_PG_Throttle, SUBARU_PG_CAM_BUS, 8, .check_relay = false}, \
{MSG_SUBARU_PG_Brake_Pedal, SUBARU_PG_CAM_BUS, 4, .check_relay = false}, \
static bool subaru_pg_reversed_driver_torque = false;
static void subaru_preglobal_rx_hook(const CANPacket_t *msg) {
if (msg->bus == SUBARU_PG_MAIN_BUS) {
if (msg->addr == MSG_SUBARU_PG_Steering_Torque) {
int torque_driver_new;
torque_driver_new = (msg->data[3] >> 5) + (msg->data[4] << 3);
torque_driver_new = to_signed(torque_driver_new, 11);
torque_driver_new = subaru_pg_reversed_driver_torque ? -torque_driver_new : torque_driver_new;
update_sample(&torque_driver, torque_driver_new);
}
// enter controls on rising edge of ACC, exit controls on ACC off
if (msg->addr == MSG_SUBARU_PG_CruiseControl) {
bool cruise_engaged = (msg->data[6] >> 1) & 1U;
pcm_cruise_check(cruise_engaged);
acc_main_on = GET_BIT(msg, 48U);
}
// update vehicle moving with any non-zero wheel speed
if (msg->addr == MSG_SUBARU_PG_Wheel_Speeds) {
vehicle_moving = ((GET_BYTES(msg, 0, 4) >> 12) != 0U) || (GET_BYTES(msg, 4, 4) != 0U);
}
if (msg->addr == MSG_SUBARU_PG_Brake_Pedal) {
brake_pressed = ((GET_BYTES(msg, 0, 4) >> 16) & 0xFFU) > 0U;
}
if (msg->addr == MSG_SUBARU_PG_Throttle) {
gas_pressed = msg->data[0] != 0U;
}
}
}
static bool subaru_preglobal_tx_hook(const CANPacket_t *msg) {
const TorqueSteeringLimits SUBARU_PG_STEERING_LIMITS = {
.max_torque = 2047,
.max_rt_delta = 940,
.max_rate_up = 50,
.max_rate_down = 70,
.driver_torque_multiplier = 10,
.driver_torque_allowance = 75,
.type = TorqueDriverLimited,
};
bool tx = true;
// steer cmd checks
if (msg->addr == MSG_SUBARU_PG_ES_LKAS) {
int desired_torque = ((GET_BYTES(msg, 0, 4) >> 8) & 0x1FFFU);
desired_torque = -1 * to_signed(desired_torque, 13);
bool steer_req = (msg->data[3] >> 0) & 1U;
if (steer_torque_cmd_checks(desired_torque, steer_req, SUBARU_PG_STEERING_LIMITS)) {
tx = false;
}
}
return tx;
}
static safety_config subaru_preglobal_init(uint16_t param) {
static const CanMsg SUBARU_PG_TX_MSGS[] = {
SUBARU_PG_COMMON_TX_MSGS
};
static const CanMsg subaru_pg_stop_and_go_tx_msgs[] = {
SUBARU_PG_COMMON_TX_MSGS
SUBARU_PG_STOP_AND_GO_TX_MSGS
};
// TODO: do checksum and counter checks after adding the signals to the outback dbc file
static RxCheck subaru_preglobal_rx_checks[] = {
{.msg = {{MSG_SUBARU_PG_Throttle, SUBARU_PG_MAIN_BUS, 8, 100U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}},
{.msg = {{MSG_SUBARU_PG_Steering_Torque, SUBARU_PG_MAIN_BUS, 8, 50U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}},
{.msg = {{MSG_SUBARU_PG_CruiseControl, SUBARU_PG_MAIN_BUS, 8, 50U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}},
{.msg = {{MSG_SUBARU_PG_Wheel_Speeds, SUBARU_PG_MAIN_BUS, 8, 50U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}},
{.msg = {{MSG_SUBARU_PG_Brake_Pedal, SUBARU_PG_MAIN_BUS, 4, 50U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}},
};
subaru_common_init();
const uint16_t SUBARU_PG_PARAM_REVERSED_DRIVER_TORQUE = 4;
subaru_pg_reversed_driver_torque = GET_FLAG(param, SUBARU_PG_PARAM_REVERSED_DRIVER_TORQUE);
safety_config ret = subaru_stop_and_go ? BUILD_SAFETY_CFG(subaru_preglobal_rx_checks, subaru_pg_stop_and_go_tx_msgs) : \
BUILD_SAFETY_CFG(subaru_preglobal_rx_checks, SUBARU_PG_TX_MSGS);
return ret;
}
const safety_hooks subaru_preglobal_hooks = {
.init = subaru_preglobal_init,
.rx = subaru_preglobal_rx_hook,
.tx = subaru_preglobal_tx_hook,
};
+427
View File
@@ -0,0 +1,427 @@
#pragma once
#include "opendbc/safety/declarations.h"
#define TESLA_COMMON_RX_CHECKS \
{.msg = {{0x2b9, 2, 8, 25U, .max_counter = 7U, .ignore_quality_flag = true}, { 0 }, { 0 }}}, /* DAS_control */ \
{.msg = {{0x488, 2, 4, 50U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}}, /* DAS_steeringControl */ \
{.msg = {{0x257, 0, 8, 50U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}}, /* DI_speed (speed in kph) */ \
{.msg = {{0x155, 0, 8, 50U, .max_counter = 15U}, { 0 }, { 0 }}}, /* ESP_B (2nd speed in kph) */ \
{.msg = {{0x370, 0, 8, 100U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}}, /* EPAS3S_sysStatus (steering angle) */ \
{.msg = {{0x118, 0, 8, 100U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}}, /* DI_systemStatus (gas pedal) */ \
{.msg = {{0x145, 0, 8, 50U, .max_counter = 15U}, { 0 }, { 0 }}}, /* ESP_status (brakes) */ \
{.msg = {{0x286, 0, 8, 10U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}}, /* DI_state (acc state) */ \
{.msg = {{0x311, 0, 7, 10U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}}, /* UI_warning (blinkers, buckle switch & doors) */ \
#define TESLA_VEHICLE_BUS_ADDR_CHECK \
{.msg = {{0x3DF, 1, 8, 2U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true, .ignore_frequency_check = true}, { 0 }, { 0 }}}, /* UI_status2 */ \
static bool tesla_longitudinal = false;
static bool tesla_fsd_14 = false;
static bool tesla_stock_aeb = false;
// Only rising edges while controls are not allowed are considered for these systems:
// TODO: Only LKAS (non-emergency) is currently supported since we've only seen it
static bool tesla_stock_lkas = false;
static bool tesla_stock_lkas_prev = false;
// Only Summon is currently supported due to Autopark not setting Autopark state properly
static bool tesla_autopark = false;
static bool tesla_autopark_prev = false;
// Detected VEHICLE bus
extern bool tesla_has_vehicle_bus;
bool tesla_has_vehicle_bus = false;
static uint8_t tesla_get_counter(const CANPacket_t *msg) {
uint8_t cnt = 0;
if (msg->addr == 0x2b9U) {
// Signal: DAS_controlCounter
cnt = msg->data[6] >> 5;
} else if (msg->addr == 0x488U) {
// Signal: DAS_steeringControlCounter
cnt = msg->data[2] & 0x0FU;
} else if ((msg->addr == 0x257U) || (msg->addr == 0x118U) || (msg->addr == 0x145U) || (msg->addr == 0x286U) || (msg->addr == 0x311U)) {
// Signal: DI_speedCounter, DI_systemStatusCounter, ESP_statusCounter, DI_locStatusCounter, UI_warningCounter
cnt = msg->data[1] & 0x0FU;
} else if (msg->addr == 0x155U) {
// Signal: ESP_wheelRotationCounter
cnt = msg->data[6] >> 4;
} else if (msg->addr == 0x370U) {
// Signal: EPAS3S_sysStatusCounter
cnt = msg->data[6] & 0x0FU;
} else {
}
return cnt;
}
static int _tesla_get_checksum_byte(const int addr) {
int checksum_byte = -1;
if ((addr == 0x370) || (addr == 0x2b9) || (addr == 0x155)) {
// Signal: EPAS3S_sysStatusChecksum, DAS_controlChecksum, ESP_wheelRotationChecksum
checksum_byte = 7;
} else if (addr == 0x488) {
// Signal: DAS_steeringControlChecksum
checksum_byte = 3;
} else if ((addr == 0x257) || (addr == 0x118) || (addr == 0x145) || (addr == 0x286) || (addr == 0x311)) {
// Signal: DI_speedChecksum, DI_systemStatusChecksum, ESP_statusChecksum, DI_locStatusChecksum, UI_warningChecksum
checksum_byte = 0;
} else {
}
return checksum_byte;
}
static uint32_t tesla_get_checksum(const CANPacket_t *msg) {
uint8_t chksum = 0;
int checksum_byte = _tesla_get_checksum_byte(msg->addr);
if (checksum_byte != -1) {
chksum = msg->data[checksum_byte];
}
return chksum;
}
static uint32_t tesla_compute_checksum(const CANPacket_t *msg) {
uint8_t chksum = 0;
int checksum_byte = _tesla_get_checksum_byte(msg->addr);
if (checksum_byte != -1) {
chksum = (uint8_t)((msg->addr & 0xFFU) + ((msg->addr >> 8) & 0xFFU));
int len = GET_LEN(msg);
for (int i = 0; i < len; i++) {
if (i != checksum_byte) {
chksum += msg->data[i];
}
}
}
return chksum;
}
static bool tesla_get_quality_flag_valid(const CANPacket_t *msg) {
bool valid = false;
if (msg->addr == 0x155U) {
valid = (msg->data[5] & 0x1U) == 0x1U; // ESP_wheelSpeedsQF
} else if (msg->addr == 0x145U) {
int user_brake_status = (msg->data[3] >> 5) & 0x03U;
valid = (user_brake_status != 0) && (user_brake_status != 3); // ESP_driverBrakeApply=NotInit_orOff, Faulty_SNA
} else {
}
return valid;
}
static int tesla_get_steer_ctrl_type(const int ctrl_type) {
// Returns ANGLE_CONTROL-equivalent control type for FSD 14
int steer_ctrl_type = ctrl_type;
if (tesla_fsd_14) {
if (ctrl_type == 1) {
steer_ctrl_type = 2;
} else if (ctrl_type == 2) {
steer_ctrl_type = 1;
} else {
}
}
return steer_ctrl_type;
}
static void tesla_rx_hook(const CANPacket_t *msg) {
if (msg->bus == 0U) {
// Steering angle: (0.1 * val) - 819.2 in deg.
if (msg->addr == 0x370U) {
// Store it 1/10 deg to match steering request
const int angle_meas_new = (((msg->data[4] & 0x3FU) << 8) | msg->data[5]) - 8192U;
update_sample(&angle_meas, angle_meas_new);
const int hands_on_level = msg->data[4] >> 6; // EPAS3S_handsOnLevel
const int eac_status = msg->data[6] >> 5; // EPAS3S_eacStatus
const int eac_error_code = msg->data[2] >> 4; // EPAS3S_eacErrorCode
// Disengage on normal user override, or if high angle rate fault from user overriding extremely quickly
steering_disengage = (hands_on_level >= 3) || ((eac_status == 0) && (eac_error_code == 9));
}
// Vehicle speed (DI_speed)
if (msg->addr == 0x257U) {
// Vehicle speed: ((val * 0.08) - 40) / MS_TO_KPH
float speed = ((((msg->data[2] << 4) | (msg->data[1] >> 4)) * 0.08) - 40.) * KPH_TO_MS;
UPDATE_VEHICLE_SPEED(speed);
}
// 2nd vehicle speed (ESP_B)
if (msg->addr == 0x155U) {
// Disable controls if speeds from DI (Drive Inverter) and ESP ECUs are too far apart.
float esp_speed = (((msg->data[6] & 0x0FU) << 6) | (msg->data[5] >> 2)) * 0.5 * KPH_TO_MS;
speed_mismatch_check(esp_speed);
}
// Gas pressed
if (msg->addr == 0x118U) {
gas_pressed = (msg->data[4] != 0U);
}
// Brake pressed
if (msg->addr == 0x145U) {
brake_pressed = ((msg->data[3] >> 5) & 0x03U) == 2U;
}
// Cruise and Autopark/Summon state
if (msg->addr == 0x286U) {
// Autopark state
int autopark_state = (msg->data[3] >> 1) & 0x0FU; // DI_autoparkState
bool tesla_autopark_now = (autopark_state == 3) || // ACTIVE
(autopark_state == 4) || // COMPLETE
(autopark_state == 9); // SELFPARK_STARTED
// Only consider rising edges while controls are not allowed
if (tesla_autopark_now && !tesla_autopark_prev && !cruise_engaged_prev) {
tesla_autopark = true;
}
if (!tesla_autopark_now) {
tesla_autopark = false;
}
tesla_autopark_prev = tesla_autopark_now;
// Cruise state
int cruise_state = (msg->data[1] >> 4) & 0x07U;
bool cruise_engaged = (cruise_state == 2) || // ENABLED
(cruise_state == 3) || // STANDSTILL
(cruise_state == 4) || // OVERRIDE
(cruise_state == 6) || // PRE_FAULT
(cruise_state == 7); // PRE_CANCEL
cruise_engaged = cruise_engaged && !tesla_autopark;
pcm_cruise_check(cruise_engaged);
}
if (msg->addr == 0x155U) {
vehicle_moving = !GET_BIT(msg, 41U); // ESP_vehicleStandstillSts
}
}
if (msg->bus == 1U) {
if (msg->addr == 0x3DFU) {
mads_button_press = (msg->data[3] == 3U) ? MADS_BUTTON_PRESSED : MADS_BUTTON_NOT_PRESSED;
}
}
if (msg->bus == 2U) {
// DAS_control
if (msg->addr == 0x2b9U) {
// "AEB_ACTIVE"
tesla_stock_aeb = (msg->data[2] & 0x03U) == 1U;
}
// DAS_steeringControl
if (msg->addr == 0x488U) {
int steering_control_type = msg->data[2] >> 6;
bool tesla_stock_lkas_now = steering_control_type == tesla_get_steer_ctrl_type(2); // "LANE_KEEP_ASSIST"
// Only consider rising edges while controls are not allowed
if (tesla_stock_lkas_now && !tesla_stock_lkas_prev && !(controls_allowed || controls_allowed_lateral)) {
tesla_stock_lkas = true;
}
if (!tesla_stock_lkas_now) {
tesla_stock_lkas = false;
}
tesla_stock_lkas_prev = tesla_stock_lkas_now;
}
}
}
static bool tesla_tx_hook(const CANPacket_t *msg) {
const AngleSteeringLimits TESLA_STEERING_LIMITS = {
.max_angle = 3600, // 360 deg, EPAS faults above this
.angle_deg_to_can = 10,
.frequency = 50U,
};
// NOTE: based off TESLA_MODEL_Y to match openpilot
const AngleSteeringParams TESLA_STEERING_PARAMS = {
.slip_factor = -0.000580374383851451, // calc_slip_factor(VM)
.steer_ratio = 12.,
.wheelbase = 2.89,
};
const LongitudinalLimits TESLA_LONG_LIMITS = {
.max_accel = 425, // 2 m/s^2
.min_accel = 288, // -3.48 m/s^2
.inactive_accel = 375, // 0. m/s^2
};
bool tx = true;
bool violation = false;
// Don't send any messages when Autopark is active
if (tesla_autopark) {
violation = true;
}
// Steering control: (0.1 * val) - 1638.35 in deg.
if (msg->addr == 0x488U) {
// We use 1/10 deg as a unit here
int raw_angle_can = ((msg->data[0] & 0x7FU) << 8) | msg->data[1];
int desired_angle = raw_angle_can - 16384;
int steer_control_type = msg->data[2] >> 6;
const int angle_ctrl_type = tesla_get_steer_ctrl_type(1);
const int lkas_ctrl_type = tesla_get_steer_ctrl_type(2);
bool steer_control_enabled = (steer_control_type == angle_ctrl_type) || // ANGLE_CONTROL
(steer_control_type == lkas_ctrl_type); // LANE_KEEP_ASSIST
if (steer_angle_cmd_checks_vm(desired_angle, steer_control_enabled, TESLA_STEERING_LIMITS, TESLA_STEERING_PARAMS)) {
violation = true;
}
bool valid_steer_control_type = (steer_control_type == 0) || // NONE
(steer_control_type == angle_ctrl_type) || // ANGLE_CONTROL
(steer_control_type == lkas_ctrl_type); // LANE_KEEP_ASSIST
if (!valid_steer_control_type) {
violation = true;
}
if (tesla_stock_lkas) {
// Don't allow any steering commands when stock LKAS is active
violation = true;
}
}
// DAS_control: longitudinal control message
if (msg->addr == 0x2b9U) {
// No AEB events may be sent by openpilot
int aeb_event = msg->data[2] & 0x03U;
if (aeb_event != 0) {
violation = true;
}
// Don't send long/cancel messages when the stock AEB system is active
if (tesla_stock_aeb) {
violation = true;
}
int raw_accel_max = ((msg->data[6] & 0x1FU) << 4) | (msg->data[5] >> 4);
int raw_accel_min = ((msg->data[5] & 0x0FU) << 5) | (msg->data[4] >> 3);
int acc_state = msg->data[1] >> 4;
if (tesla_longitudinal) {
// Prevent both acceleration from being negative, as this could cause the car to reverse after coming to standstill
if ((raw_accel_max < TESLA_LONG_LIMITS.inactive_accel) && (raw_accel_min < TESLA_LONG_LIMITS.inactive_accel)) {
violation = true;
}
// Don't allow any acceleration limits above the safety limits
violation |= longitudinal_accel_checks(raw_accel_max, TESLA_LONG_LIMITS);
violation |= longitudinal_accel_checks(raw_accel_min, TESLA_LONG_LIMITS);
} else {
// Can only send cancel longitudinal messages when not controlling longitudinal
if (acc_state != 13) { // ACC_CANCEL_GENERIC_SILENT
violation = true;
}
// No actuation is allowed when not controlling longitudinal
if ((raw_accel_max != TESLA_LONG_LIMITS.inactive_accel) || (raw_accel_min != TESLA_LONG_LIMITS.inactive_accel)) {
violation = true;
}
}
}
if (violation) {
tx = false;
}
return tx;
}
static bool tesla_fwd_hook(int bus_num, int addr) {
bool block_msg = false;
if (bus_num == 2) {
if (!tesla_autopark) {
// APS_eacMonitor
if (addr == 0x27d) {
block_msg = true;
}
// DAS_steeringControl
if ((addr == 0x488) && !tesla_stock_lkas) {
block_msg = true;
}
// DAS_control
if (tesla_longitudinal && (addr == 0x2b9) && !tesla_stock_aeb) {
block_msg = true;
}
}
}
return block_msg;
}
static safety_config tesla_init(uint16_t param) {
static const CanMsg TESLA_M3_Y_TX_MSGS[] = {
{0x488, 0, 4, .check_relay = true, .disable_static_blocking = true}, // DAS_steeringControl
{0x2b9, 0, 8, .check_relay = false}, // DAS_control (for cancel)
{0x27D, 0, 3, .check_relay = true, .disable_static_blocking = true}, // APS_eacMonitor
};
static const CanMsg TESLA_M3_Y_LONG_TX_MSGS[] = {
{0x488, 0, 4, .check_relay = true, .disable_static_blocking = true}, // DAS_steeringControl
{0x2b9, 0, 8, .check_relay = true, .disable_static_blocking = true}, // DAS_control
{0x27D, 0, 3, .check_relay = true, .disable_static_blocking = true}, // APS_eacMonitor
};
const uint16_t TESLA_FLAG_FSD_14 = 2;
tesla_fsd_14 = GET_FLAG(param, TESLA_FLAG_FSD_14);
#ifdef ALLOW_DEBUG
const uint16_t TESLA_FLAG_LONGITUDINAL_CONTROL = 1;
tesla_longitudinal = GET_FLAG(param, TESLA_FLAG_LONGITUDINAL_CONTROL);
#endif
const uint16_t TESLA_PARAM_SP_VEHICLE_BUS = 1;
tesla_has_vehicle_bus = GET_FLAG(current_safety_param_sp, TESLA_PARAM_SP_VEHICLE_BUS);
tesla_stock_aeb = false;
tesla_stock_lkas = false;
tesla_stock_lkas_prev = false;
// we need to assume Autopark/Summon on startup since DI_state is a low freq msg.
// this is so that we don't fault if starting while these systems are active
tesla_autopark = true;
tesla_autopark_prev = false;
static RxCheck tesla_model3_y_rx_checks[] = {
TESLA_COMMON_RX_CHECKS
};
static RxCheck tesla_model3_y_vehicle_bus_rx_checks[] = {
TESLA_COMMON_RX_CHECKS
TESLA_VEHICLE_BUS_ADDR_CHECK
};
safety_config ret;
if (tesla_longitudinal) {
SET_TX_MSGS(TESLA_M3_Y_LONG_TX_MSGS, ret);
} else {
SET_TX_MSGS(TESLA_M3_Y_TX_MSGS, ret);
}
if (tesla_has_vehicle_bus) {
SET_RX_CHECKS(tesla_model3_y_vehicle_bus_rx_checks, ret);
} else {
SET_RX_CHECKS(tesla_model3_y_rx_checks, ret);
}
return ret;
}
const safety_hooks tesla_hooks = {
.init = tesla_init,
.rx = tesla_rx_hook,
.tx = tesla_tx_hook,
.fwd = tesla_fwd_hook,
.get_counter = tesla_get_counter,
.get_checksum = tesla_get_checksum,
.compute_checksum = tesla_compute_checksum,
.get_quality_flag_valid = tesla_get_quality_flag_valid,
};
+576
View File
@@ -0,0 +1,576 @@
#pragma once
#include "opendbc/safety/declarations.h"
// Stock longitudinal
#define TOYOTA_BASE_TX_MSGS \
{0x191, 0, 8, .check_relay = true}, {0x412, 0, 8, .check_relay = true}, {0x1D2, 0, 8, .check_relay = false}, /* LKAS + LTA + PCM cancel cmd */ \
#define TOYOTA_COMMON_TX_MSGS \
TOYOTA_BASE_TX_MSGS \
{0x2E4, 0, 5, .check_relay = true}, \
{0x343, 0, 8, .check_relay = false}, /* ACC cancel cmd */ \
#define TOYOTA_COMMON_SECOC_TX_MSGS \
TOYOTA_BASE_TX_MSGS \
{0x2E4, 0, 8, .check_relay = true}, {0x131, 0, 8, .check_relay = true}, \
{0x343, 0, 8, .check_relay = false}, /* ACC cancel cmd */ \
#define TOYOTA_COMMON_LONG_TX_MSGS \
TOYOTA_COMMON_TX_MSGS \
/* DSU bus 0 */ \
{0x283, 0, 7, .check_relay = false}, {0x2E6, 0, 8, .check_relay = false}, {0x2E7, 0, 8, .check_relay = false}, {0x33E, 0, 7, .check_relay = false}, \
{0x344, 0, 8, .check_relay = false}, {0x365, 0, 7, .check_relay = false}, {0x366, 0, 7, .check_relay = false}, {0x4CB, 0, 8, .check_relay = false}, \
/* DSU bus 1 */ \
{0x128, 1, 6, .check_relay = false}, {0x141, 1, 4, .check_relay = false}, {0x160, 1, 8, .check_relay = false}, {0x161, 1, 7, .check_relay = false}, \
{0x470, 1, 4, .check_relay = false}, \
/* PCS_HUD */ \
{0x411, 0, 8, .check_relay = false}, \
/* radar diagnostic address */ \
{0x750, 0, 8, .check_relay = false}, \
/* ACC */ \
{0x343, 0, 8, .check_relay = true}, \
#define TOYOTA_COMMON_SECOC_LONG_TX_MSGS \
TOYOTA_COMMON_SECOC_TX_MSGS \
{0x343, 0, 8, .check_relay = true}, \
{0x183, 0, 8, .check_relay = true}, /* ACC_CONTROL_2 */ \
#define TOYOTA_COMMON_RX_CHECKS(lta) \
{.msg = {{ 0xaa, 0, 8, 83U, .ignore_checksum = true, .ignore_counter = true}, { 0 }, { 0 }}}, \
{.msg = {{0x260, 0, 8, 50U, .ignore_counter = true, .ignore_quality_flag=!(lta)}, { 0 }, { 0 }}}, \
#define TOYOTA_RX_CHECKS(lta) \
TOYOTA_COMMON_RX_CHECKS(lta) \
{.msg = {{0x1D2, 0, 8, 33U, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \
{.msg = {{0x226, 0, 8, 40U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \
#define TOYOTA_ALT_BRAKE_RX_CHECKS(lta) \
TOYOTA_COMMON_RX_CHECKS(lta) \
{.msg = {{0x1D2, 0, 8, 33U, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \
{.msg = {{0x224, 0, 8, 40U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \
#define TOYOTA_SECOC_RX_CHECKS \
TOYOTA_COMMON_RX_CHECKS(false) \
{.msg = {{0x176, 0, 8, 32U, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \
{.msg = {{0x116, 0, 8, 42U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \
{.msg = {{0x101, 0, 8, 50U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \
#define TOYOTA_PCM_CRUISE_2_ADDR_CHECK \
{.msg = {{0x1D3, 0, 8, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true, .frequency = 33U}, { 0 }, { 0 }}}, \
#define TOYOTA_DSU_CRUISE_ADDR_CHECK \
{.msg = {{0x365, 0, 7, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true, .frequency = 5U, .ignore_frequency_check = true}, { 0 }, { 0 }}}, \
#define TOYOTA_GAS_INTERCEPTOR_ADDR_CHECK \
{.msg = {{0x201, 0, 6, 50U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \
static bool toyota_secoc = false;
static bool toyota_alt_brake = false;
static bool toyota_stock_longitudinal = false;
static bool toyota_lta = false;
static int toyota_dbc_eps_torque_factor = 100; // conversion factor for STEER_TORQUE_EPS in %: see dbc file
static uint32_t toyota_compute_checksum(const CANPacket_t *msg) {
int len = GET_LEN(msg);
uint8_t checksum = (uint8_t)(msg->addr) + (uint8_t)((unsigned int)(msg->addr) >> 8U) + (uint8_t)(len);
for (int i = 0; i < (len - 1); i++) {
checksum += (uint8_t)msg->data[i];
}
return checksum;
}
static uint32_t toyota_get_checksum(const CANPacket_t *msg) {
int checksum_byte = GET_LEN(msg) - 1U;
return (uint8_t)(msg->data[checksum_byte]);
}
static bool toyota_get_quality_flag_valid(const CANPacket_t *msg) {
bool valid = false;
if (msg->addr == 0x260U) {
valid = !GET_BIT(msg, 3U); // STEER_TORQUE_SENSOR.STEER_ANGLE_INITIALIZING
} else if (msg->addr == 0xaaU) { // WHEEL_SPEEDS
// each wheel speed is 1-bit fault + 15-bit speed
valid = true;
for (uint8_t i = 0U; i < 4U; i += 1U) {
if (GET_BIT(msg, (i * 16U) + 7U)) { // WHEEL_SPEED_xx_FAULT
valid = false;
break;
}
}
} else {
}
return valid;
}
static int TOYOTA_GET_INTERCEPTOR(const CANPacket_t *msg) {
uint16_t val1 = (uint16_t)((uint16_t)msg->data[0] << 8U) | (uint16_t)msg->data[1];
uint16_t val2 = (uint16_t)((uint16_t)msg->data[2] << 8U) | (uint16_t)msg->data[3];
uint16_t avg = (uint16_t)((val1 + val2) / 2U);
return (int)avg;
}
static void toyota_rx_hook(const CANPacket_t *msg) {
if (msg->bus == 0U) {
// get eps motor torque (0.66 factor in dbc)
if (msg->addr == 0x260U) {
int torque_meas_new = (msg->data[5] << 8) | msg->data[6];
torque_meas_new = to_signed(torque_meas_new, 16);
// scale by dbc_factor
torque_meas_new = (torque_meas_new * toyota_dbc_eps_torque_factor) / 100;
// update array of sample
update_sample(&torque_meas, torque_meas_new);
// increase torque_meas by 1 to be conservative on rounding
torque_meas.min--;
torque_meas.max++;
// driver torque for angle limiting
int torque_driver_new = (msg->data[1] << 8) | msg->data[2];
torque_driver_new = to_signed(torque_driver_new, 16);
update_sample(&torque_driver, torque_driver_new);
// LTA request angle should match current angle while inactive, clipped to max accepted angle.
// note that angle can be relative to init angle on some TSS2 platforms, LTA has the same offset
bool steer_angle_initializing = GET_BIT(msg, 3U);
if (!steer_angle_initializing) {
int angle_meas_new = (msg->data[3] << 8U) | msg->data[4];
angle_meas_new = to_signed(angle_meas_new, 16);
update_sample(&angle_meas, angle_meas_new);
}
}
// enter controls on rising edge of ACC, exit controls on ACC off
// exit controls on rising edge of gas press, if not alternative experience
// exit controls on rising edge of brake press
if (toyota_secoc) {
if (msg->addr == 0x176U) {
bool cruise_engaged = GET_BIT(msg, 5U); // PCM_CRUISE.CRUISE_ACTIVE
pcm_cruise_check(cruise_engaged);
}
if (msg->addr == 0x116U) {
gas_pressed = msg->data[1] != 0U; // GAS_PEDAL.GAS_PEDAL_USER
}
if (msg->addr == 0x101U) {
brake_pressed = GET_BIT(msg, 3U); // BRAKE_MODULE.BRAKE_PRESSED (toyota_rav4_prime_generated.dbc)
}
} else {
if (msg->addr == 0x1D2U) {
bool cruise_engaged = GET_BIT(msg, 5U); // PCM_CRUISE.CRUISE_ACTIVE
pcm_cruise_check(cruise_engaged);
if (!enable_gas_interceptor) {
gas_pressed = !GET_BIT(msg, 4U); // PCM_CRUISE.GAS_RELEASED
}
}
if (!toyota_alt_brake && (msg->addr == 0x226U)) {
brake_pressed = GET_BIT(msg, 37U); // BRAKE_MODULE.BRAKE_PRESSED (toyota_nodsu_pt_generated.dbc)
}
if (toyota_alt_brake && (msg->addr == 0x224U)) {
brake_pressed = GET_BIT(msg, 5U); // BRAKE_MODULE.BRAKE_PRESSED (toyota_new_mc_pt_generated.dbc)
}
}
// sample speed
if (msg->addr == 0xaaU) {
int speed = 0;
// sum 4 wheel speeds. conversion: raw * 0.01 - 67.67
for (uint8_t i = 0U; i < 8U; i += 2U) {
int wheel_speed = ((msg->data[i] & 0x7FU) << 8U) | msg->data[(i + 1U)];
speed += wheel_speed - 6767;
}
// check that all wheel speeds are at zero value
vehicle_moving = speed != 0;
UPDATE_VEHICLE_SPEED(speed / 4.0 * 0.01 * KPH_TO_MS);
}
if (msg->addr == 0x1D3U) {
acc_main_on = GET_BIT(msg, 15U);
}
if (msg->addr == 0x365U) {
acc_main_on = GET_BIT(msg, 0U);
}
// sample gas interceptor
if (msg->addr == 0x201U) {
// 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;
int gas_interceptor = TOYOTA_GET_INTERCEPTOR(msg);
gas_pressed = gas_interceptor > toyota_gas_interceptor_thrsld;
gas_interceptor_prev = gas_interceptor;
}
}
}
static bool toyota_tx_hook(const CANPacket_t *msg) {
const TorqueSteeringLimits TOYOTA_TORQUE_STEERING_LIMITS = {
.max_torque = 1500,
.max_rate_up = 15, // ramp up slow
.max_rate_down = 25, // ramp down fast
.max_torque_error = 350, // max torque cmd in excess of motor torque
.max_rt_delta = 450, // the real time limit is 1800/sec, a 20% buffer
.type = TorqueMotorLimited,
// the EPS faults when the steering angle rate is above a certain threshold for too long. to prevent this,
// we allow setting STEER_REQUEST bit to 0 while maintaining the requested torque value for a single frame
.min_valid_request_frames = 17,
.max_invalid_request_frames = 1,
.min_valid_request_rt_interval = 162000, // 162ms; a ~10% buffer on cutting every 18 frames
.has_steer_req_tolerance = true,
};
static const AngleSteeringLimits TOYOTA_ANGLE_STEERING_LIMITS = {
// LTA angle limits
// factor for STEER_TORQUE_SENSOR->STEER_ANGLE and STEERING_LTA->STEER_ANGLE_CMD (1 / 0.0573)
.max_angle = 1657, // EPS only accepts up to 94.9461
.angle_deg_to_can = 17.452007,
.angle_rate_up_lookup = {
{5., 25., 25.},
{0.3, 0.15, 0.15}
},
.angle_rate_down_lookup = {
{5., 25., 25.},
{0.36, 0.26, 0.26}
},
};
const int TOYOTA_LTA_MAX_MEAS_TORQUE = 1500;
const int TOYOTA_LTA_MAX_DRIVER_TORQUE = 150;
// longitudinal limits
const LongitudinalLimits TOYOTA_LONG_LIMITS = {
.max_accel = 2000, // 2.0 m/s2
.min_accel = -3500, // -3.5 m/s2
};
bool tx = true;
// Check if msg is sent on BUS 0
if (msg->bus == 0U) {
// ACCEL: safety check on byte 1-2
if (msg->addr == 0x343U) {
int desired_accel = (msg->data[0] << 8) | msg->data[1];
desired_accel = to_signed(desired_accel, 16);
bool violation = false;
if (toyota_secoc) {
// SecOC cars move accel to 0x183. Only allow inactive accel on 0x343 to match stock behavior
violation = desired_accel != TOYOTA_LONG_LIMITS.inactive_accel;
}
violation |= longitudinal_accel_checks(desired_accel, TOYOTA_LONG_LIMITS);
// only ACC messages that cancel are allowed when openpilot is not controlling longitudinal
if (toyota_stock_longitudinal) {
bool cancel_req = GET_BIT(msg, 24U);
if (!cancel_req) {
violation = true;
}
if (desired_accel != TOYOTA_LONG_LIMITS.inactive_accel) {
violation = true;
}
}
if (violation) {
tx = false;
}
}
if (msg->addr == 0x183U) {
int desired_accel = (msg->data[0] << 8) | msg->data[1];
desired_accel = to_signed(desired_accel, 16);
tx = !longitudinal_accel_checks(desired_accel, TOYOTA_LONG_LIMITS);
}
// AEB: block all actuation. only used when DSU is unplugged
if (msg->addr == 0x283U) {
// only allow the checksum, which is the last byte
bool block = (GET_BYTES(msg, 0, 4) != 0U) || (msg->data[4] != 0U) || (msg->data[5] != 0U);
if (block) {
tx = false;
}
}
// STEERING_LTA angle steering check
if (msg->addr == 0x191U) {
// check the STEER_REQUEST, STEER_REQUEST_2, TORQUE_WIND_DOWN, STEER_ANGLE_CMD signals
bool lta_request = GET_BIT(msg, 0U);
bool lta_request2 = GET_BIT(msg, 25U);
int torque_wind_down = msg->data[5];
int lta_angle = (msg->data[1] << 8) | msg->data[2];
lta_angle = to_signed(lta_angle, 16);
bool steer_control_enabled = lta_request || lta_request2;
if (!toyota_lta) {
// using torque (LKA), block LTA msgs with actuation requests
if (steer_control_enabled || (lta_angle != 0) || (torque_wind_down != 0)) {
tx = false;
}
} else {
// check angle rate limits and inactive angle
if (steer_angle_cmd_checks(lta_angle, steer_control_enabled, TOYOTA_ANGLE_STEERING_LIMITS)) {
tx = false;
}
if (lta_request != lta_request2) {
tx = false;
}
// TORQUE_WIND_DOWN is gated on steer request
if (!steer_control_enabled && (torque_wind_down != 0)) {
tx = false;
}
// TORQUE_WIND_DOWN can only be no or full torque
if ((torque_wind_down != 0) && (torque_wind_down != 100)) {
tx = false;
}
// check if we should wind down torque
int driver_torque = SAFETY_MIN(SAFETY_ABS(torque_driver.min), SAFETY_ABS(torque_driver.max));
if ((driver_torque > TOYOTA_LTA_MAX_DRIVER_TORQUE) && (torque_wind_down != 0)) {
tx = false;
}
int eps_torque = SAFETY_MIN(SAFETY_ABS(torque_meas.min), SAFETY_ABS(torque_meas.max));
if ((eps_torque > TOYOTA_LTA_MAX_MEAS_TORQUE) && (torque_wind_down != 0)) {
tx = false;
}
}
}
// STEERING_LTA_2 angle steering check (SecOC)
if (toyota_secoc && (msg->addr == 0x131U)) {
// SecOC cars block any form of LTA actuation for now
bool lta_request = GET_BIT(msg, 3U); // STEERING_LTA_2.STEER_REQUEST
bool lta_request2 = GET_BIT(msg, 0U); // STEERING_LTA_2.STEER_REQUEST_2
int lta_angle_msb = msg->data[2]; // STEERING_LTA_2.STEER_ANGLE_CMD (MSB)
int lta_angle_lsb = msg->data[3]; // STEERING_LTA_2.STEER_ANGLE_CMD (LSB)
bool actuation = lta_request || lta_request2 || (lta_angle_msb != 0) || (lta_angle_lsb != 0);
if (actuation) {
tx = false;
}
}
// STEER: safety check on bytes 2-3
if (msg->addr == 0x2E4U) {
int desired_torque = (msg->data[1] << 8) | msg->data[2];
desired_torque = to_signed(desired_torque, 16);
bool steer_req = GET_BIT(msg, 0U);
// When using LTA (angle control), assert no actuation on LKA message
if (!toyota_lta) {
if (steer_torque_cmd_checks(desired_torque, steer_req, TOYOTA_TORQUE_STEERING_LIMITS)) {
tx = false;
}
} else {
if ((desired_torque != 0) || steer_req) {
tx = false;
}
}
}
// GAS PEDAL: safety check
if (msg->addr == 0x200U) {
if (longitudinal_interceptor_checks(msg)) {
tx = false;
}
}
}
// UDS: Only tester present ("\x0F\x02\x3E\x00\x00\x00\x00\x00") allowed on diagnostics address
if (msg->addr == 0x750U) {
// this address is sub-addressed. only allow tester present to radar (0xF)
bool invalid_uds_msg = (GET_BYTES(msg, 0, 4) != 0x003E020FU) || (GET_BYTES(msg, 4, 4) != 0x0U);
if (invalid_uds_msg) {
tx = false;
}
}
return tx;
}
static safety_config toyota_init(uint16_t param) {
static const CanMsg TOYOTA_TX_MSGS[] = {
TOYOTA_COMMON_TX_MSGS
};
static const CanMsg TOYOTA_SECOC_TX_MSGS[] = {
TOYOTA_COMMON_SECOC_TX_MSGS
};
static const CanMsg TOYOTA_LONG_TX_MSGS[] = {
TOYOTA_COMMON_LONG_TX_MSGS
};
static const CanMsg TOYOTA_SECOC_LONG_TX_MSGS[] = {
TOYOTA_COMMON_SECOC_LONG_TX_MSGS
};
static const CanMsg TOYOTA_INTERCEPTOR_TX_MSGS[] = {
TOYOTA_COMMON_LONG_TX_MSGS
{0x200, 0, 6, .check_relay = false}, // gas interceptor
};
// safety param flags
// first byte is for EPS factor, second is for flags
const uint32_t TOYOTA_PARAM_OFFSET = 8U;
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 uint16_t TOYOTA_PARAM_SP_UNSUPPORTED_DSU = 1;
const uint16_t TOYTOA_PARAM_SP_GAS_INTERCEPTOR = 2;
#ifdef ALLOW_DEBUG
const uint32_t TOYOTA_PARAM_SECOC = 8UL << TOYOTA_PARAM_OFFSET;
toyota_secoc = GET_FLAG(param, TOYOTA_PARAM_SECOC);
#endif
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);
toyota_dbc_eps_torque_factor = param & TOYOTA_EPS_FACTOR;
const bool toyota_unsupported_dsu = GET_FLAG(current_safety_param_sp, TOYOTA_PARAM_SP_UNSUPPORTED_DSU);
enable_gas_interceptor = GET_FLAG(current_safety_param_sp, TOYTOA_PARAM_SP_GAS_INTERCEPTOR);
// gas interceptor should not be used if openpilot is not controlling longitudinal or is a TSK car
if (toyota_stock_longitudinal || toyota_secoc) {
enable_gas_interceptor = false;
}
safety_config ret;
if (toyota_secoc) {
if (toyota_stock_longitudinal) {
SET_TX_MSGS(TOYOTA_SECOC_TX_MSGS, ret);
} else {
SET_TX_MSGS(TOYOTA_SECOC_LONG_TX_MSGS, ret);
}
} else {
if (toyota_stock_longitudinal) {
SET_TX_MSGS(TOYOTA_TX_MSGS, ret);
} else {
SET_TX_MSGS(TOYOTA_LONG_TX_MSGS, ret);
}
}
if (toyota_secoc) {
static RxCheck toyota_secoc_rx_checks[] = {
TOYOTA_SECOC_RX_CHECKS
TOYOTA_PCM_CRUISE_2_ADDR_CHECK
};
SET_RX_CHECKS(toyota_secoc_rx_checks, ret);
} else if (toyota_lta) {
// Check the quality flag for angle measurement when using LTA, since it's not set on TSS-P cars
static RxCheck toyota_lta_rx_checks[] = {
TOYOTA_RX_CHECKS(true)
TOYOTA_PCM_CRUISE_2_ADDR_CHECK
};
SET_RX_CHECKS(toyota_lta_rx_checks, ret);
} else {
static RxCheck toyota_lka_rx_checks[] = {
TOYOTA_RX_CHECKS(false)
TOYOTA_PCM_CRUISE_2_ADDR_CHECK
};
static RxCheck toyota_lka_alt_brake_rx_checks[] = {
TOYOTA_ALT_BRAKE_RX_CHECKS(false)
TOYOTA_PCM_CRUISE_2_ADDR_CHECK
};
static RxCheck toyota_lka_unsupported_dsu_rx_checks[] = {
TOYOTA_RX_CHECKS(false)
TOYOTA_DSU_CRUISE_ADDR_CHECK
};
static RxCheck toyota_lka_alt_brake_unsupported_dsu_rx_checks[] = {
TOYOTA_ALT_BRAKE_RX_CHECKS(false)
TOYOTA_DSU_CRUISE_ADDR_CHECK
};
if (!toyota_alt_brake) {
if (toyota_unsupported_dsu) {
SET_RX_CHECKS(toyota_lka_unsupported_dsu_rx_checks, ret);
} else {
SET_RX_CHECKS(toyota_lka_rx_checks, ret);
}
} else {
if (toyota_unsupported_dsu) {
SET_RX_CHECKS(toyota_lka_alt_brake_unsupported_dsu_rx_checks, ret);
} else {
SET_RX_CHECKS(toyota_lka_alt_brake_rx_checks, ret);
}
}
}
if (enable_gas_interceptor) {
SET_TX_MSGS(TOYOTA_INTERCEPTOR_TX_MSGS, ret);
if (toyota_lta) {
static RxCheck toyota_lta_interceptor_rx_checks[] = {
TOYOTA_RX_CHECKS(true)
TOYOTA_PCM_CRUISE_2_ADDR_CHECK
TOYOTA_GAS_INTERCEPTOR_ADDR_CHECK
};
SET_RX_CHECKS(toyota_lta_interceptor_rx_checks, ret);
} else {
static RxCheck toyota_lka_interceptor_rx_checks[] = {
TOYOTA_RX_CHECKS(false)
TOYOTA_PCM_CRUISE_2_ADDR_CHECK
TOYOTA_GAS_INTERCEPTOR_ADDR_CHECK
};
static RxCheck toyota_lka_alt_brake_interceptor_rx_checks[] = {
TOYOTA_ALT_BRAKE_RX_CHECKS(false)
TOYOTA_PCM_CRUISE_2_ADDR_CHECK
TOYOTA_GAS_INTERCEPTOR_ADDR_CHECK
};
static RxCheck toyota_lka_unsupported_dsu_interceptor_rx_checks[] = {
TOYOTA_RX_CHECKS(false)
TOYOTA_DSU_CRUISE_ADDR_CHECK
TOYOTA_GAS_INTERCEPTOR_ADDR_CHECK
};
static RxCheck toyota_lka_alt_brake_unsupported_dsu_interceptor_rx_checks[] = {
TOYOTA_ALT_BRAKE_RX_CHECKS(false)
TOYOTA_DSU_CRUISE_ADDR_CHECK
TOYOTA_GAS_INTERCEPTOR_ADDR_CHECK
};
if (!toyota_alt_brake) {
if (toyota_unsupported_dsu) {
SET_RX_CHECKS(toyota_lka_unsupported_dsu_interceptor_rx_checks, ret);
} else {
SET_RX_CHECKS(toyota_lka_interceptor_rx_checks, ret);
}
} else {
if (toyota_unsupported_dsu) {
SET_RX_CHECKS(toyota_lka_alt_brake_unsupported_dsu_interceptor_rx_checks, ret);
} else {
SET_RX_CHECKS(toyota_lka_alt_brake_interceptor_rx_checks, ret);
}
}
}
}
return ret;
}
const safety_hooks toyota_hooks = {
.init = toyota_init,
.rx = toyota_rx_hook,
.tx = toyota_tx_hook,
.get_checksum = toyota_get_checksum,
.compute_checksum = toyota_compute_checksum,
.get_quality_flag_valid = toyota_get_quality_flag_valid,
};
@@ -0,0 +1,110 @@
#pragma once
extern const uint16_t FLAG_VOLKSWAGEN_LONG_CONTROL;
const uint16_t FLAG_VOLKSWAGEN_LONG_CONTROL = 1;
static uint8_t volkswagen_crc8_lut_8h2f[256]; // Static lookup table for CRC8 poly 0x2F, aka 8H2F/AUTOSAR
extern bool volkswagen_longitudinal;
bool volkswagen_longitudinal = false;
extern bool volkswagen_set_button_prev;
bool volkswagen_set_button_prev = false;
extern bool volkswagen_resume_button_prev;
bool volkswagen_resume_button_prev = false;
extern bool volkswagen_brake_pedal_switch;
extern bool volkswagen_brake_pressure_detected;
bool volkswagen_brake_pedal_switch = false;
bool volkswagen_brake_pressure_detected = false;
#define MSG_LH_EPS_03 0x09FU // RX from EPS, for driver steering torque
#define MSG_ESP_19 0x0B2U // RX from ABS, for wheel speeds
#define MSG_ESP_05 0x106U // RX from ABS, for brake switch state
#define MSG_TSK_06 0x120U // RX from ECU, for ACC status from drivetrain coordinator
#define MSG_MOTOR_20 0x121U // RX from ECU, for driver throttle input
#define MSG_ACC_06 0x122U // TX by OP, ACC control instructions to the drivetrain coordinator
#define MSG_HCA_01 0x126U // TX by OP, Heading Control Assist steering torque
#define MSG_GRA_ACC_01 0x12BU // TX by OP, ACC control buttons for cancel/resume
#define MSG_ACC_07 0x12EU // TX by OP, ACC control instructions to the drivetrain coordinator
#define MSG_ACC_02 0x30CU // TX by OP, ACC HUD data to the instrument cluster
#define MSG_LDW_02 0x397U // TX by OP, Lane line recognition and text alerts
#define MSG_MOTOR_14 0x3BEU // RX from ECU, for brake switch status
// MLB only messages
#define MSG_ESP_03 0x103U // RX from ABS, for wheel speeds
#define MSG_LS_01 0x10BU // TX by OP, ACC control buttons for cancel/resume
#define MSG_MOTOR_03 0x105U // RX from ECU, for driver throttle input and brake switch status
#define MSG_TSK_04 0x10EU // RX from ECU, for ACC status from drivetrain coordinator
static void volkswagen_common_init(void) {
volkswagen_set_button_prev = false;
volkswagen_resume_button_prev = false;
volkswagen_brake_pedal_switch = false;
volkswagen_brake_pressure_detected = false;
gen_crc_lookup_table_8(0x2F, volkswagen_crc8_lut_8h2f);
return;
}
static uint32_t volkswagen_mqb_meb_get_checksum(const CANPacket_t *msg) {
return (uint8_t)msg->data[0];
}
static uint8_t volkswagen_mqb_meb_get_counter(const CANPacket_t *msg) {
// MQB/MEB message counters are consistently found at LSB 8.
return (uint8_t)msg->data[1] & 0xFU;
}
static uint32_t volkswagen_mqb_meb_compute_crc(const CANPacket_t *msg) {
int len = GET_LEN(msg);
// This is CRC-8H2F/AUTOSAR with a twist. See the opendbc/car/volkswagen/ implementation
// of this algorithm for a version with explanatory comments.
uint8_t crc = 0xFFU;
for (int i = 1; i < len; i++) {
crc ^= (uint8_t)msg->data[i];
crc = volkswagen_crc8_lut_8h2f[crc];
}
uint8_t counter = volkswagen_mqb_meb_get_counter(msg);
if (msg->addr == MSG_LH_EPS_03) {
crc ^= (uint8_t[]){0xF5, 0xF5, 0xF5, 0xF5, 0xF5, 0xF5, 0xF5, 0xF5, 0xF5, 0xF5, 0xF5, 0xF5, 0xF5, 0xF5, 0xF5, 0xF5}[counter];
} else if (msg->addr == MSG_ESP_05) {
crc ^= (uint8_t[]){0x07, 0x07, 0x07, 0x07, 0x07, 0x07, 0x07, 0x07, 0x07, 0x07, 0x07, 0x07, 0x07, 0x07, 0x07, 0x07}[counter];
} else if (msg->addr == MSG_TSK_06) {
crc ^= (uint8_t[]){0xC4, 0xE2, 0x4F, 0xE4, 0xF8, 0x2F, 0x56, 0x81, 0x9F, 0xE5, 0x83, 0x44, 0x05, 0x3F, 0x97, 0xDF}[counter];
} else if (msg->addr == MSG_MOTOR_20) {
crc ^= (uint8_t[]){0xE9, 0x65, 0xAE, 0x6B, 0x7B, 0x35, 0xE5, 0x5F, 0x4E, 0xC7, 0x86, 0xA2, 0xBB, 0xDD, 0xEB, 0xB4}[counter];
} else if (msg->addr == MSG_GRA_ACC_01) {
crc ^= (uint8_t[]){0x6A, 0x38, 0xB4, 0x27, 0x22, 0xEF, 0xE1, 0xBB, 0xF8, 0x80, 0x84, 0x49, 0xC7, 0x9E, 0x1E, 0x2B}[counter];
} else {
// Undefined CAN message, CRC check expected to fail
}
crc = volkswagen_crc8_lut_8h2f[crc];
return (uint8_t)(crc ^ 0xFFU);
}
static int volkswagen_mlb_mqb_driver_input_torque(const CANPacket_t *msg) {
// Signal: LH_EPS_03.EPS_Lenkmoment (absolute torque)
// Signal: LH_EPS_03.EPS_VZ_Lenkmoment (direction)
int torque_driver_new = msg->data[5] | ((msg->data[6] & 0x1FU) << 8);
bool sign = GET_BIT(msg, 55U);
if (sign) {
torque_driver_new *= -1;
}
return torque_driver_new;
}
static int volkswagen_mlb_mqb_steering_control_torque(const CANPacket_t *msg) {
// Signal: HCA_01.HCA_01_LM_Offset (absolute torque)
// Signal: HCA_01.HCA_01_LM_OffSign (direction)
int desired_torque = msg->data[2] | ((msg->data[3] & 0x1U) << 8);
bool sign = GET_BIT(msg, 31U);
if (sign) {
desired_torque *= -1;
}
return desired_torque;
}
@@ -0,0 +1,284 @@
#pragma once
#include "opendbc/safety/declarations.h"
#include "opendbc/safety/modes/volkswagen_common.h"
#define MSG_ESC_51 0xFCU // RX, for wheel speeds
#define MSG_ACC_18 0x14DU // TX by OP, ACC control instructions to the drivetrain coordinator
#define MSG_ESP_21 0xFDU // RX, redundant vehicle speed source
#define MSG_HCA_03 0x303U
#define MSG_ACC_19 0x300U // TX by OP, ACC HUD data to the instrument cluster
#define MSG_QFK_01 0x13DU
#define MSG_Motor_51 0x10BU // RX for TSK state and accel pedal
#define MSG_KLR_01 0x25DU // TX, for capacitive steering wheel
#define MSG_TA_01 0x26BU // TX by OP, Travel Assist status
static bool volkswagen_meb_alt_crc = false;
#define VOLKSWAGEN_MEB_COMMON_RX_CHECKS \
{.msg = {{MSG_LH_EPS_03, 0, 8, 100U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \
{.msg = {{MSG_MOTOR_14, 0, 8, 10U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \
{.msg = {{MSG_GRA_ACC_01, 0, 8, 33U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \
{.msg = {{MSG_QFK_01, 0, 32, 50U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \
{.msg = {{MSG_ESP_21, 0, 8, 50U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}},
static uint32_t volkswagen_meb_compute_crc(const CANPacket_t *msg) {
int len = GET_LEN(msg);
uint8_t crc = 0xFFU;
for (int i = 1; i < len; i++) {
crc ^= (uint8_t)msg->data[i];
crc = volkswagen_crc8_lut_8h2f[crc];
}
uint8_t counter = volkswagen_mqb_meb_get_counter(msg);
if (msg->addr == MSG_LH_EPS_03) {
crc ^= (uint8_t[]){0xF5, 0xF5, 0xF5, 0xF5, 0xF5, 0xF5, 0xF5, 0xF5, 0xF5, 0xF5, 0xF5, 0xF5, 0xF5, 0xF5, 0xF5, 0xF5}[counter];
} else if (msg->addr == MSG_GRA_ACC_01) {
crc ^= (uint8_t[]){0x6A, 0x38, 0xB4, 0x27, 0x22, 0xEF, 0xE1, 0xBB, 0xF8, 0x80, 0x84, 0x49, 0xC7, 0x9E, 0x1E, 0x2B}[counter];
} else if (msg->addr == MSG_QFK_01) {
crc ^= (uint8_t[]){0x20, 0xCA, 0x68, 0xD5, 0x1B, 0x31, 0xE2, 0xDA, 0x08, 0x0A, 0xD4, 0xDE, 0x9C, 0xE4, 0x35, 0x5B}[counter];
} else if (msg->addr == MSG_ESC_51) {
crc ^= (uint8_t[]){0x77, 0x5C, 0xA0, 0x89, 0x4B, 0x7C, 0xBB, 0xD6, 0x1F, 0x6C, 0x4F, 0xF6, 0x20, 0x2B, 0x43, 0xDD}[counter];
} else if (msg->addr == MSG_ESP_21) {
crc ^= (uint8_t[]){0xB4, 0xEF, 0xF8, 0x49, 0x1E, 0xE5, 0xC2, 0xC0, 0x97, 0x19, 0x3C, 0xC9, 0xF1, 0x98, 0xD6, 0x61}[counter];
} else if (msg->addr == MSG_Motor_51) {
crc ^= (uint8_t[]){0x77, 0x5C, 0xA0, 0x89, 0x4B, 0x7C, 0xBB, 0xD6, 0x1F, 0x6C, 0x4F, 0xF6, 0x20, 0x2B, 0x43, 0xDD}[counter];
} else {
// Undefined CAN message, CRC check expected to fail
}
crc = volkswagen_crc8_lut_8h2f[crc];
return (uint8_t)(crc ^ 0xFFU);
}
static uint32_t volkswagen_meb_alt_crc_compute(const CANPacket_t *msg) {
uint32_t ret = volkswagen_meb_compute_crc(msg);
int len = 0;
if (volkswagen_meb_alt_crc) {
if (msg->addr == MSG_QFK_01) {
len = 28;
} else if (msg->addr == MSG_ESC_51) {
len = 60;
} else if (msg->addr == MSG_Motor_51) {
len = 44;
} else {
len = 0;
}
}
if (len > 0) {
uint8_t crc = 0xFFU;
for (int i = 1; i < len; i++) {
crc ^= (uint8_t)msg->data[i];
crc = volkswagen_crc8_lut_8h2f[crc];
}
uint8_t counter = volkswagen_mqb_meb_get_counter(msg);
if (msg->addr == MSG_QFK_01) {
crc ^= (uint8_t[]){0x18, 0x71, 0x10, 0x8D, 0xD7, 0xAA, 0xB0, 0x78, 0xAC, 0x12, 0xAE, 0x0C, 0xDD, 0xF1, 0x85, 0x68}[counter];
} else if (msg->addr == MSG_ESC_51) {
crc ^= (uint8_t[]){0x69, 0xDC, 0xF9, 0x64, 0x6A, 0xCE, 0x55, 0x2C, 0xC4, 0x38, 0x8F, 0xD1, 0xC6, 0x43, 0xB4, 0xB1}[counter];
} else {
crc ^= (uint8_t[]){0x2C, 0xB1, 0x1A, 0x75, 0xBB, 0x65, 0x79, 0x47, 0x81, 0x2B, 0xCC, 0x96, 0x17, 0xDB, 0xC0, 0x94}[counter];
}
crc = (uint8_t)(volkswagen_crc8_lut_8h2f[crc] ^ 0xFFU);
if (crc == msg->data[0]) {
ret = crc;
}
}
return ret;
}
static safety_config volkswagen_meb_init(uint16_t param) {
// Transmit of GRA_ACC_01 is allowed on bus 0 and 2 to keep compatibility with gateway and camera integration
static const CanMsg VOLKSWAGEN_MEB_STOCK_TX_MSGS[] = {
{MSG_HCA_03, 0, 24, .check_relay = true},
{MSG_GRA_ACC_01, 0, 8, .check_relay = false},
{MSG_GRA_ACC_01, 2, 8, .check_relay = false},
{MSG_LDW_02, 0, 8, .check_relay = true},
{MSG_KLR_01, 0, 8, .check_relay = false},
{MSG_KLR_01, 2, 8, .check_relay = true},
};
static const CanMsg VOLKSWAGEN_MEB_LONG_TX_MSGS[] = {
{MSG_HCA_03, 0, 24, .check_relay = true},
{MSG_LDW_02, 0, 8, .check_relay = true},
{MSG_KLR_01, 0, 8, .check_relay = false},
{MSG_KLR_01, 2, 8, .check_relay = true},
{MSG_ACC_19, 0, 48, .check_relay = true},
{MSG_ACC_18, 0, 32, .check_relay = true},
{MSG_TA_01, 0, 8, .check_relay = true},
};
static RxCheck volkswagen_meb_rx_checks[] = {
VOLKSWAGEN_MEB_COMMON_RX_CHECKS
{.msg = {{MSG_Motor_51, 0, 32, 50U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}},
{.msg = {{MSG_ESC_51, 0, 48, 50U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}},
};
static RxCheck volkswagen_meb_gen2_rx_checks[] = {
VOLKSWAGEN_MEB_COMMON_RX_CHECKS
{.msg = {{MSG_Motor_51, 0, 48, 50U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}},
{.msg = {{MSG_ESC_51, 0, 64, 50U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}},
};
volkswagen_common_init();
const uint16_t FLAG_VOLKSWAGEN_MEB_ALT_CRC = 2;
volkswagen_meb_alt_crc = GET_FLAG(param, FLAG_VOLKSWAGEN_MEB_ALT_CRC);
#ifdef ALLOW_DEBUG
volkswagen_longitudinal = GET_FLAG(param, FLAG_VOLKSWAGEN_LONG_CONTROL);
#endif
safety_config ret;
if (volkswagen_longitudinal && volkswagen_meb_alt_crc) {
ret = BUILD_SAFETY_CFG(volkswagen_meb_gen2_rx_checks, VOLKSWAGEN_MEB_LONG_TX_MSGS);
} else if (volkswagen_longitudinal) {
ret = BUILD_SAFETY_CFG(volkswagen_meb_rx_checks, VOLKSWAGEN_MEB_LONG_TX_MSGS);
} else if (volkswagen_meb_alt_crc) {
ret = BUILD_SAFETY_CFG(volkswagen_meb_gen2_rx_checks, VOLKSWAGEN_MEB_STOCK_TX_MSGS);
} else {
ret = BUILD_SAFETY_CFG(volkswagen_meb_rx_checks, VOLKSWAGEN_MEB_STOCK_TX_MSGS);
}
return ret;
}
static void volkswagen_meb_rx_hook(const CANPacket_t *msg) {
if (msg->bus == 0U) {
// Update in-motion state by sampling wheel speeds
if (msg->addr == MSG_ESC_51) {
uint32_t fl = msg->data[8] | (msg->data[9] << 8);
uint32_t fr = msg->data[10] | (msg->data[11] << 8);
uint32_t rl = msg->data[12] | (msg->data[13] << 8);
uint32_t rr = msg->data[14] | (msg->data[15] << 8);
vehicle_moving = (fr > 0U) || (rr > 0U) || (rl > 0U) || (fl > 0U);
UPDATE_VEHICLE_SPEED((fr + rr + rl + fl) / 4.0 * 0.0075 * KPH_TO_MS);
}
// Check vehicle speed with redundant source
if (msg->addr == MSG_ESP_21) {
// Signal: ESP_v_Signal
float esp_speed = ((msg->data[5] << 8) | msg->data[4]) * 0.01 * KPH_TO_MS;
UPDATE_VEHICLE_SPEED_2(esp_speed);
}
if (msg->addr == MSG_QFK_01) {
int current_curvature = ((msg->data[6] & 0x7FU) << 8) | msg->data[5];
current_curvature *= GET_BIT(msg, 55U) ? 1 : -1;
update_sample(&curvature_state.meas, current_curvature);
}
if (msg->addr == MSG_LH_EPS_03) {
update_sample(&torque_driver, volkswagen_mlb_mqb_driver_input_torque(msg));
}
if (msg->addr == MSG_Motor_51) {
int acc_status = (msg->data[11] & 0x07U);
bool cruise_engaged = (acc_status == 3) || (acc_status == 4) || (acc_status == 5);
acc_main_on = cruise_engaged || (acc_status == 2);
if (!volkswagen_longitudinal) {
pcm_cruise_check(cruise_engaged);
}
if (!acc_main_on) {
controls_allowed = false;
}
int accel_pedal_value = ((msg->data[1] >> 4) & 0x0FU) | ((msg->data[2] & 0x1FU) << 4);
gas_pressed = accel_pedal_value > 0;
}
if (msg->addr == MSG_GRA_ACC_01) {
// If using openpilot longitudinal, enter controls on falling edge of Set or Resume with main switch on
// Signal: GRA_ACC_01.GRA_Tip_Setzen
// Signal: GRA_ACC_01.GRA_Tip_Wiederaufnahme
if (volkswagen_longitudinal) {
bool set_button = GET_BIT(msg, 16U);
bool resume_button = GET_BIT(msg, 19U);
if ((volkswagen_set_button_prev && !set_button) || (volkswagen_resume_button_prev && !resume_button)) {
controls_allowed = acc_main_on;
}
volkswagen_set_button_prev = set_button;
volkswagen_resume_button_prev = resume_button;
}
// Always exit controls on rising edge of Cancel
if (GET_BIT(msg, 13U)) {
controls_allowed = false;
}
}
if (msg->addr == MSG_MOTOR_14) {
brake_pressed = GET_BIT(msg, 28U);
}
}
}
static bool volkswagen_meb_tx_hook(const CANPacket_t *msg) {
// acceleration in m/s2 * 1000 to avoid floating point math
const LongitudinalLimits VOLKSWAGEN_MEB_LONG_LIMITS = {
.max_accel = 2000,
.min_accel = -3500,
.inactive_accel = 3010, // VW sends one increment above the max range when inactive
};
bool tx = true;
// Safety check for MSG_ACC_18 acceleration requests
if (msg->addr == MSG_ACC_18) {
// Signal: ACC_18.ACC_Sollbeschleunigung_02 (acceleration in m/s2, scale 0.005, offset -7.22)
int desired_accel = ((((msg->data[4] & 0x7U) << 8) | msg->data[3]) * 5U) - 7220U;
// allow ACCEL_OVERRIDE (0) while controls are allowed even when the driver is on the gas
bool accel_override = controls_allowed && (desired_accel == 0);
if (!accel_override && longitudinal_accel_checks(desired_accel, VOLKSWAGEN_MEB_LONG_LIMITS)) {
tx = false;
}
}
if (msg->addr == MSG_HCA_03) {
const CurvatureSteeringLimits VOLKSWAGEN_MEB_STEERING_LIMITS = {
.max_curvature = 29105,
.curvature_to_can = 149253.7313f,
.frequency = 50, // Hz
.max_curvature_error = 0, // disabled, MEB doesn't track rack
.curvature_error_min_speed = 0.0, // disabled
.max_steer_power = 125,
.inactive_curvature_is_zero = false, // MEB winds down with measured curvature
};
int desired_curvature_raw = GET_BYTES(msg, 3, 2) & 0x7FFFU;
bool desired_curvature_sign = GET_BIT(msg, 39U);
if (!desired_curvature_sign) {
desired_curvature_raw *= -1;
}
bool steer_req = (((msg->data[1] >> 4) & 0x0FU) == 4U);
int steer_power = msg->data[2];
if (steer_curvature_cmd_checks(desired_curvature_raw, steer_power, steer_req, VOLKSWAGEN_MEB_STEERING_LIMITS)) {
tx = false;
}
}
if ((msg->addr == MSG_GRA_ACC_01) && !controls_allowed) {
// only allow cancel button: bit 13
if (!GET_BIT(msg, 13U)) {
tx = false;
}
}
return tx;
}
const safety_hooks volkswagen_meb_hooks = {
.init = volkswagen_meb_init,
.rx = volkswagen_meb_rx_hook,
.tx = volkswagen_meb_tx_hook,
.get_counter = volkswagen_mqb_meb_get_counter,
.get_checksum = volkswagen_mqb_meb_get_checksum,
.compute_checksum = volkswagen_meb_alt_crc_compute,
};
@@ -0,0 +1,127 @@
#pragma once
#include "opendbc/safety/declarations.h"
#include "opendbc/safety/modes/volkswagen_common.h"
static safety_config volkswagen_mlb_init(uint16_t param) {
// Transmit of LS_01 is allowed on bus 0 and 2 to keep compatibility with gateway and camera integration
static const CanMsg VOLKSWAGEN_MLB_STOCK_TX_MSGS[] = {{MSG_HCA_01, 0, 8, .check_relay = true}, {MSG_LDW_02, 0, 8, .check_relay = true},
{MSG_LS_01, 0, 4, .check_relay = false}, {MSG_LS_01, 2, 4, .check_relay = false}};
static RxCheck volkswagen_mlb_rx_checks[] = {
// TODO: implement checksum validation
{.msg = {{MSG_ESP_03, 0, 8, 50U, .ignore_checksum = true, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}},
{.msg = {{MSG_LH_EPS_03, 0, 8, 100U, .ignore_checksum = true, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}},
{.msg = {{MSG_ESP_05, 0, 8, 50U, .ignore_checksum = true, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}},
{.msg = {{MSG_TSK_04, 1, 8, 50U, .ignore_checksum = true, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}},
{.msg = {{MSG_MOTOR_03, 0, 8, 100U, .ignore_checksum = true, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}},
{.msg = {{MSG_LS_01, 0, 4, 10U, .ignore_checksum = true, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}},
};
SAFETY_UNUSED(param);
volkswagen_common_init();
return BUILD_SAFETY_CFG(volkswagen_mlb_rx_checks, VOLKSWAGEN_MLB_STOCK_TX_MSGS);
}
static void volkswagen_mlb_rx_hook(const CANPacket_t *msg) {
if (msg->bus == 0U) {
// Check all wheel speeds for any movement
// Signals: ESP_03.ESP_[VL|VR|HL|HR]_Radgeschw
if (msg->addr == MSG_ESP_03) {
uint32_t speed = 0;
speed += ((msg->data[3] & 0xFU) << 8) | msg->data[2]; // FL
speed += (msg->data[4] << 4) | (msg->data[3] >> 4); // FR
speed += ((msg->data[6] & 0xFU) << 8) | msg->data[5]; // RL
speed += (msg->data[7] << 4) | (msg->data[6] >> 4); // RR
vehicle_moving = speed > 0U;
}
// Update driver input torque
if (msg->addr == MSG_LH_EPS_03) {
update_sample(&torque_driver, volkswagen_mlb_mqb_driver_input_torque(msg));
}
if (msg->addr == MSG_LS_01) {
// Always exit controls on rising edge of Cancel
// Signal: LS_01.LS_Abbrechen
if (GET_BIT(msg, 13U)) {
controls_allowed = false;
}
}
// Signal: Motor_03.MO_Fahrpedalrohwert_01
// Signal: Motor_03.MO_Fahrer_bremst
if (msg->addr == MSG_MOTOR_03) {
gas_pressed = msg->data[6] != 0U;
volkswagen_brake_pedal_switch = GET_BIT(msg, 35U);
}
if (msg->addr == MSG_ESP_05) {
volkswagen_brake_pressure_detected = GET_BIT(msg, 26U);
}
brake_pressed = volkswagen_brake_pedal_switch || volkswagen_brake_pressure_detected;
}
if (msg->bus == 1U) {
if (msg->addr == MSG_TSK_04) {
// When using stock ACC, enter controls on rising edge of stock ACC engage, exit on disengage
// Signal: TSK_04.TSK_Status_GRA_ACC_02
int acc_status = (msg->data[7] & 0xC0U) >> 6;
bool cruise_engaged = (acc_status == 1) || (acc_status == 2);
pcm_cruise_check(cruise_engaged);
}
}
}
static bool volkswagen_mlb_tx_hook(const CANPacket_t *msg) {
// lateral limits
const TorqueSteeringLimits VOLKSWAGEN_MLB_STEERING_LIMITS = {
.max_torque = 300, // 3.0 Nm (EPS side max of 3.0Nm with fault if violated)
.max_rt_delta = 169, // 10 max rate up * 50Hz send rate * 250000 RT interval / 1000000 = 112.5 ; 112.5 * 1.5 for safety pad = 168.75
.max_rate_up = 9, // 5.0 Nm/s RoC limit (EPS rack has own soft-limit of 5.0 Nm/s)
.max_rate_down = 10, // 5.0 Nm/s RoC limit (EPS rack has own soft-limit of 5.0 Nm/s)
.driver_torque_allowance = 60,
.driver_torque_multiplier = 3,
.type = TorqueDriverLimited,
};
bool tx = true;
// Safety check for HCA_01 Heading Control Assist torque
if (msg->addr == MSG_HCA_01) {
int desired_torque = volkswagen_mlb_mqb_steering_control_torque(msg);
int steer_status = msg->data[4] & 0xFU;
bool steer_req = (steer_status == 5) || (steer_status == 7);
if (steer_torque_cmd_checks(desired_torque, steer_req, VOLKSWAGEN_MLB_STEERING_LIMITS)) {
tx = false;
}
}
// FORCE CANCEL: ensuring that only the cancel button press is sent when controls are off.
// This avoids unintended engagements while still allowing resume spam
if ((msg->addr == MSG_LS_01) && !controls_allowed) {
// disallow resume and set: bits 16 and 19
if (GET_BIT(msg, 16U) || GET_BIT(msg, 19U)) {
tx = false;
}
}
return tx;
}
// TODO: rename these functions to MXB or something
const safety_hooks volkswagen_mlb_hooks = {
.init = volkswagen_mlb_init,
.rx = volkswagen_mlb_rx_hook,
.tx = volkswagen_mlb_tx_hook,
.get_counter = volkswagen_mqb_meb_get_counter,
.get_checksum = volkswagen_mqb_meb_get_checksum,
.compute_checksum = volkswagen_mqb_meb_compute_crc,
};
@@ -0,0 +1,189 @@
#pragma once
#include "opendbc/safety/declarations.h"
#include "opendbc/safety/modes/volkswagen_common.h"
static safety_config volkswagen_mqb_init(uint16_t param) {
// Transmit of GRA_ACC_01 is allowed on bus 0 and 2 to keep compatibility with gateway and camera integration
// MSG_LH_EPS_03: openpilot needs to replace apparent driver steering input torque to pacify VW Emergency Assist
static const CanMsg VOLKSWAGEN_MQB_STOCK_TX_MSGS[] = {{MSG_HCA_01, 0, 8, .check_relay = true}, {MSG_GRA_ACC_01, 0, 8, .check_relay = false}, {MSG_GRA_ACC_01, 2, 8, .check_relay = false},
{MSG_LDW_02, 0, 8, .check_relay = true}, {MSG_LH_EPS_03, 2, 8, .check_relay = true}};
static const CanMsg VOLKSWAGEN_MQB_LONG_TX_MSGS[] = {{MSG_HCA_01, 0, 8, .check_relay = true}, {MSG_LDW_02, 0, 8, .check_relay = true}, {MSG_LH_EPS_03, 2, 8, .check_relay = true},
{MSG_ACC_02, 0, 8, .check_relay = true}, {MSG_ACC_06, 0, 8, .check_relay = true}, {MSG_ACC_07, 0, 8, .check_relay = true}};
static RxCheck volkswagen_mqb_rx_checks[] = {
{.msg = {{MSG_ESP_19, 0, 8, 100U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}},
{.msg = {{MSG_LH_EPS_03, 0, 8, 100U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}},
{.msg = {{MSG_ESP_05, 0, 8, 50U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}},
{.msg = {{MSG_TSK_06, 0, 8, 50U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}},
{.msg = {{MSG_MOTOR_20, 0, 8, 50U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}},
{.msg = {{MSG_MOTOR_14, 0, 8, 10U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}},
{.msg = {{MSG_GRA_ACC_01, 0, 8, 33U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}},
};
volkswagen_common_init();
#ifdef ALLOW_DEBUG
volkswagen_longitudinal = GET_FLAG(param, FLAG_VOLKSWAGEN_LONG_CONTROL);
#else
SAFETY_UNUSED(param);
#endif
return volkswagen_longitudinal ? BUILD_SAFETY_CFG(volkswagen_mqb_rx_checks, VOLKSWAGEN_MQB_LONG_TX_MSGS) : \
BUILD_SAFETY_CFG(volkswagen_mqb_rx_checks, VOLKSWAGEN_MQB_STOCK_TX_MSGS);
}
static void volkswagen_mqb_rx_hook(const CANPacket_t *msg) {
if (msg->bus == 0U) {
// Update in-motion state by sampling wheel speeds
if (msg->addr == MSG_ESP_19) {
// sum 4 wheel speeds
int speed = 0;
for (uint8_t i = 0U; i < 8U; i += 2U) {
int wheel_speed = msg->data[i] | (msg->data[i + 1U] << 8);
speed += wheel_speed;
}
// Check all wheel speeds for any movement
vehicle_moving = speed > 0;
}
// Update driver input torque samples
// Signal: LH_EPS_03.EPS_Lenkmoment (absolute torque)
// Signal: LH_EPS_03.EPS_VZ_Lenkmoment (direction)
if (msg->addr == MSG_LH_EPS_03) {
update_sample(&torque_driver, volkswagen_mlb_mqb_driver_input_torque(msg));
}
if (msg->addr == MSG_TSK_06) {
// When using stock ACC, enter controls on rising edge of stock ACC engage, exit on disengage
// Always exit controls on main switch off
// Signal: TSK_06.TSK_Status
int acc_status = (msg->data[3] & 0x7U);
bool cruise_engaged = (acc_status == 3) || (acc_status == 4) || (acc_status == 5);
acc_main_on = cruise_engaged || (acc_status == 2);
if (!volkswagen_longitudinal) {
pcm_cruise_check(cruise_engaged);
}
if (!acc_main_on) {
controls_allowed = false;
}
}
if (msg->addr == MSG_GRA_ACC_01) {
// If using openpilot longitudinal, enter controls on falling edge of Set or Resume with main switch on
// Signal: GRA_ACC_01.GRA_Tip_Setzen
// Signal: GRA_ACC_01.GRA_Tip_Wiederaufnahme
if (volkswagen_longitudinal) {
bool set_button = GET_BIT(msg, 16U);
bool resume_button = GET_BIT(msg, 19U);
if ((volkswagen_set_button_prev && !set_button) || (volkswagen_resume_button_prev && !resume_button)) {
controls_allowed = acc_main_on;
}
volkswagen_set_button_prev = set_button;
volkswagen_resume_button_prev = resume_button;
}
// Always exit controls on rising edge of Cancel
// Signal: GRA_ACC_01.GRA_Abbrechen
if (GET_BIT(msg, 13U)) {
controls_allowed = false;
}
}
// Signal: Motor_20.MO_Fahrpedalrohwert_01
if (msg->addr == MSG_MOTOR_20) {
gas_pressed = ((GET_BYTES(msg, 0, 4) >> 12) & 0xFFU) != 0U;
}
// Signal: Motor_14.MO_Fahrer_bremst (ECU detected brake pedal switch F63)
if (msg->addr == MSG_MOTOR_14) {
volkswagen_brake_pedal_switch = GET_BIT(msg, 28U);
}
// Signal: ESP_05.ESP_Fahrer_bremst (ESP detected driver brake pressure above platform specified threshold)
if (msg->addr == MSG_ESP_05) {
volkswagen_brake_pressure_detected = GET_BIT(msg, 26U);
}
brake_pressed = volkswagen_brake_pedal_switch || volkswagen_brake_pressure_detected;
}
}
static bool volkswagen_mqb_tx_hook(const CANPacket_t *msg) {
// lateral limits
const TorqueSteeringLimits VOLKSWAGEN_MQB_STEERING_LIMITS = {
.max_torque = 300, // 3.0 Nm (EPS side max of 3.0Nm with fault if violated)
.max_rt_delta = 75, // 4 max rate up * 50Hz send rate * 250000 RT interval / 1000000 = 50 ; 50 * 1.5 for safety pad = 75
.max_rate_up = 4, // 2.0 Nm/s RoC limit (EPS rack has own soft-limit of 5.0 Nm/s)
.max_rate_down = 10, // 5.0 Nm/s RoC limit (EPS rack has own soft-limit of 5.0 Nm/s)
.driver_torque_allowance = 80,
.driver_torque_multiplier = 3,
.type = TorqueDriverLimited,
};
// longitudinal limits
// acceleration in m/s2 * 1000 to avoid floating point math
const LongitudinalLimits VOLKSWAGEN_MQB_LONG_LIMITS = {
.max_accel = 2000,
.min_accel = -3500,
.inactive_accel = 3010, // VW sends one increment above the max range when inactive
};
bool tx = true;
// Safety check for HCA_01 Heading Control Assist torque
if (msg->addr == MSG_HCA_01) {
int desired_torque = volkswagen_mlb_mqb_steering_control_torque(msg);
bool steer_req = GET_BIT(msg, 30U);
if (steer_torque_cmd_checks(desired_torque, steer_req, VOLKSWAGEN_MQB_STEERING_LIMITS)) {
tx = false;
}
}
// Safety check for both ACC_06 and ACC_07 acceleration requests
// To avoid floating point math, scale upward and compare to pre-scaled safety m/s2 boundaries
if ((msg->addr == MSG_ACC_06) || (msg->addr == MSG_ACC_07)) {
bool violation = false;
int desired_accel = 0;
if (msg->addr == MSG_ACC_06) {
// Signal: ACC_06.ACC_Sollbeschleunigung_02 (acceleration in m/s2, scale 0.005, offset -7.22)
desired_accel = ((((msg->data[4] & 0x7U) << 8) | msg->data[3]) * 5U) - 7220U;
} else {
// Signal: ACC_07.ACC_Folgebeschl (acceleration in m/s2, scale 0.03, offset -4.6)
int secondary_accel = (msg->data[4] * 30U) - 4600U;
violation |= (secondary_accel != 3020); // enforce always inactive (one increment above max range) at this time
// Signal: ACC_07.ACC_Sollbeschleunigung_02 (acceleration in m/s2, scale 0.005, offset -7.22)
desired_accel = (((msg->data[7] << 3) | ((msg->data[6] & 0xE0U) >> 5)) * 5U) - 7220U;
}
violation |= longitudinal_accel_checks(desired_accel, VOLKSWAGEN_MQB_LONG_LIMITS);
if (violation) {
tx = false;
}
}
// FORCE CANCEL: ensuring that only the cancel button press is sent when controls are off.
// This avoids unintended engagements while still allowing resume spam
if ((msg->addr == MSG_GRA_ACC_01) && !controls_allowed) {
// disallow resume and set: bits 16 and 19
if ((msg->data[2] & 0x9U) != 0U) {
tx = false;
}
}
return tx;
}
const safety_hooks volkswagen_mqb_hooks = {
.init = volkswagen_mqb_init,
.rx = volkswagen_mqb_rx_hook,
.tx = volkswagen_mqb_tx_hook,
.get_counter = volkswagen_mqb_meb_get_counter,
.get_checksum = volkswagen_mqb_meb_get_checksum,
.compute_checksum = volkswagen_mqb_meb_compute_crc,
};
@@ -0,0 +1,219 @@
#pragma once
#include "opendbc/safety/declarations.h"
#include "opendbc/safety/modes/volkswagen_common.h"
#define MSG_LENKHILFE_3 0x0D0U // RX from EPS, for steering angle and driver steering torque
#define MSG_HCA_1 0x0D2U // TX by OP, Heading Control Assist steering torque
#define MSG_BREMSE_1 0x1A0U // RX from ABS, for ego speed
#define MSG_MOTOR_2 0x288U // RX from ECU, for CC state and brake switch state
#define MSG_ACC_SYSTEM 0x368U // TX by OP, longitudinal acceleration controls
#define MSG_MOTOR_3 0x380U // RX from ECU, for driver throttle input
#define MSG_GRA_NEU 0x38AU // TX by OP, ACC control buttons for cancel/resume
#define MSG_MOTOR_5 0x480U // RX from ECU, for ACC main switch state
#define MSG_ACC_GRA_ANZEIGE 0x56AU // TX by OP, ACC HUD
#define MSG_LDW_1 0x5BEU // TX by OP, Lane line recognition and text alerts
static uint32_t volkswagen_pq_get_checksum(const CANPacket_t *msg) {
return (uint32_t)msg->data[(msg->addr == MSG_MOTOR_5) ? 7 : 0];
}
static uint8_t volkswagen_pq_get_counter(const CANPacket_t *msg) {
uint8_t counter = 0U;
if (msg->addr == MSG_LENKHILFE_3) {
counter = (uint8_t)(msg->data[1] & 0xF0U) >> 4;
} else if (msg->addr == MSG_GRA_NEU) {
counter = (uint8_t)(msg->data[2] & 0xF0U) >> 4;
} else {
}
return counter;
}
static uint32_t volkswagen_pq_compute_checksum(const CANPacket_t *msg) {
int len = GET_LEN(msg);
uint8_t checksum = 0U;
int checksum_byte = (msg->addr == MSG_MOTOR_5) ? 7 : 0;
// Simple XOR over the payload, except for the byte where the checksum lives.
for (int i = 0; i < len; i++) {
if (i != checksum_byte) {
checksum ^= (uint8_t)msg->data[i];
}
}
return checksum;
}
static safety_config volkswagen_pq_init(uint16_t param) {
// Transmit of GRA_Neu is allowed on bus 0 and 2 to keep compatibility with gateway and camera integration
static const CanMsg VOLKSWAGEN_PQ_STOCK_TX_MSGS[] = {{MSG_HCA_1, 0, 5, .check_relay = true}, {MSG_LDW_1, 0, 8, .check_relay = true},
{MSG_GRA_NEU, 0, 4, .check_relay = false}, {MSG_GRA_NEU, 2, 4, .check_relay = false}};
static const CanMsg VOLKSWAGEN_PQ_LONG_TX_MSGS[] = {{MSG_HCA_1, 0, 5, .check_relay = true}, {MSG_LDW_1, 0, 8, .check_relay = true},
{MSG_ACC_SYSTEM, 0, 8, .check_relay = true}, {MSG_ACC_GRA_ANZEIGE, 0, 8, .check_relay = true}};
static RxCheck volkswagen_pq_rx_checks[] = {
{.msg = {{MSG_LENKHILFE_3, 0, 6, 100U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}},
{.msg = {{MSG_BREMSE_1, 0, 8, 100U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}},
{.msg = {{MSG_MOTOR_2, 0, 8, 50U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}},
{.msg = {{MSG_MOTOR_3, 0, 8, 100U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}},
{.msg = {{MSG_MOTOR_5, 0, 8, 50U, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}},
{.msg = {{MSG_GRA_NEU, 0, 4, 30U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}},
};
volkswagen_common_init();
#ifdef ALLOW_DEBUG
volkswagen_longitudinal = GET_FLAG(param, FLAG_VOLKSWAGEN_LONG_CONTROL);
#else
SAFETY_UNUSED(param);
#endif
return volkswagen_longitudinal ? BUILD_SAFETY_CFG(volkswagen_pq_rx_checks, VOLKSWAGEN_PQ_LONG_TX_MSGS) : \
BUILD_SAFETY_CFG(volkswagen_pq_rx_checks, VOLKSWAGEN_PQ_STOCK_TX_MSGS);
}
static void volkswagen_pq_rx_hook(const CANPacket_t *msg) {
if (msg->bus == 0U) {
// Update in-motion state from speed value.
// Signal: Bremse_1.BR1_Rad_kmh
if (msg->addr == MSG_BREMSE_1) {
int speed = ((msg->data[2] & 0xFEU) >> 1) | (msg->data[3] << 7);
vehicle_moving = speed > 0;
}
// Update driver input torque samples
// Signal: Lenkhilfe_3.LH3_LM (absolute torque)
// Signal: Lenkhilfe_3.LH3_LMSign (direction)
if (msg->addr == MSG_LENKHILFE_3) {
int torque_driver_new = msg->data[2] | ((msg->data[3] & 0x3U) << 8);
int sign = (msg->data[3] & 0x4U) >> 2;
if (sign == 1) {
torque_driver_new *= -1;
}
update_sample(&torque_driver, torque_driver_new);
}
if (volkswagen_longitudinal) {
if (msg->addr == MSG_MOTOR_5) {
// ACC main switch on is a prerequisite to enter controls, exit controls immediately on main switch off
// Signal: Motor_5.MO5_GRA_Hauptsch
acc_main_on = GET_BIT(msg, 50U);
if (!acc_main_on) {
controls_allowed = false;
}
}
if (msg->addr == MSG_GRA_NEU) {
// If ACC main switch is on, enter controls on falling edge of Set or Resume
// Signal: GRA_Neu.GRA_Neu_Setzen
// Signal: GRA_Neu.GRA_Neu_Recall
bool set_button = GET_BIT(msg, 16U);
bool resume_button = GET_BIT(msg, 17U);
if ((volkswagen_set_button_prev && !set_button) || (volkswagen_resume_button_prev && !resume_button)) {
controls_allowed = acc_main_on;
}
volkswagen_set_button_prev = set_button;
volkswagen_resume_button_prev = resume_button;
// Exit controls on rising edge of Cancel, override Set/Resume if present simultaneously
// Signal: GRA_ACC_01.GRA_Abbrechen
if (GET_BIT(msg, 9U)) {
controls_allowed = false;
}
}
} else {
if (msg->addr == MSG_MOTOR_2) {
// Enter controls on rising edge of stock ACC, exit controls if stock ACC disengages
// Signal: Motor_2.MO2_Sta_GRA
int acc_status = (msg->data[2] & 0xC0U) >> 6;
bool cruise_engaged = (acc_status == 1) || (acc_status == 2);
pcm_cruise_check(cruise_engaged);
}
}
// Signal: Motor_3.MO3_Pedalwert
if (msg->addr == MSG_MOTOR_3) {
gas_pressed = (msg->data[2]);
}
// Signal: Motor_2.MO2_BLS
if (msg->addr == MSG_MOTOR_2) {
brake_pressed = (msg->data[2] & 0x1U);
}
}
}
static bool volkswagen_pq_tx_hook(const CANPacket_t *msg) {
// lateral limits
const TorqueSteeringLimits VOLKSWAGEN_PQ_STEERING_LIMITS = {
.max_torque = 300, // 3.0 Nm (EPS side max of 3.0Nm with fault if violated)
.max_rt_delta = 113, // 6 max rate up * 50Hz send rate * 250000 RT interval / 1000000 = 75 ; 125 * 1.5 for safety pad = 113
.max_rate_up = 6, // 3.0 Nm/s RoC limit (EPS rack has own soft-limit of 5.0 Nm/s)
.max_rate_down = 10, // 5.0 Nm/s RoC limit (EPS rack has own soft-limit of 5.0 Nm/s)
.driver_torque_multiplier = 3,
.driver_torque_allowance = 80,
.type = TorqueDriverLimited,
};
// longitudinal limits
// acceleration in m/s2 * 1000 to avoid floating point math
const LongitudinalLimits VOLKSWAGEN_PQ_LONG_LIMITS = {
.max_accel = 2000,
.min_accel = -3500,
.inactive_accel = 3010, // VW sends one increment above the max range when inactive
};
bool tx = true;
// Safety check for HCA_1 Heading Control Assist torque
// Signal: HCA_1.LM_Offset (absolute torque)
// Signal: HCA_1.LM_Offsign (direction)
if (msg->addr == MSG_HCA_1) {
int desired_torque = msg->data[2] | ((msg->data[3] & 0x7FU) << 8);
desired_torque = desired_torque / 32; // DBC scale from PQ network to centi-Nm
int sign = (msg->data[3] & 0x80U) >> 7;
if (sign == 1) {
desired_torque *= -1;
}
uint32_t hca_status = ((msg->data[1] >> 4) & 0xFU);
bool steer_req = ((hca_status == 5U) || (hca_status == 7U));
if (steer_torque_cmd_checks(desired_torque, steer_req, VOLKSWAGEN_PQ_STEERING_LIMITS)) {
tx = false;
}
}
// Safety check for acceleration commands
// To avoid floating point math, scale upward and compare to pre-scaled safety m/s2 boundaries
if (msg->addr == MSG_ACC_SYSTEM) {
// Signal: ACC_System.ACS_Sollbeschl (acceleration in m/s2, scale 0.005, offset -7.22)
int desired_accel = ((((msg->data[4] & 0x7U) << 8) | msg->data[3]) * 5U) - 7220U;
if (longitudinal_accel_checks(desired_accel, VOLKSWAGEN_PQ_LONG_LIMITS)) {
tx = false;
}
}
// FORCE CANCEL: ensuring that only the cancel button press is sent when controls are off.
// This avoids unintended engagements while still allowing resume spam
if ((msg->addr == MSG_GRA_NEU) && !controls_allowed) {
// Signal: GRA_Neu.GRA_Neu_Setzen
// Signal: GRA_Neu.GRA_Neu_Recall
if (GET_BIT(msg, 16U) || GET_BIT(msg, 17U)) {
tx = false;
}
}
return tx;
}
const safety_hooks volkswagen_pq_hooks = {
.init = volkswagen_pq_init,
.rx = volkswagen_pq_rx_hook,
.tx = volkswagen_pq_tx_hook,
.get_counter = volkswagen_pq_get_counter,
.get_checksum = volkswagen_pq_get_checksum,
.compute_checksum = volkswagen_pq_compute_checksum,
};
+562
View File
@@ -0,0 +1,562 @@
#pragma once
#include "opendbc/safety/helpers.h"
#include "opendbc/safety/lateral.h"
#include "opendbc/safety/longitudinal.h"
#include "opendbc/safety/declarations.h"
#include "opendbc/safety/can.h"
// all the safety modes
#include "opendbc/safety/modes/defaults.h"
#include "opendbc/safety/modes/honda.h"
#include "opendbc/safety/modes/toyota.h"
#include "opendbc/safety/modes/tesla.h"
#include "opendbc/safety/modes/gm.h"
#include "opendbc/safety/modes/ford.h"
#include "opendbc/safety/modes/hyundai.h"
#include "opendbc/safety/modes/chrysler.h"
#include "opendbc/safety/modes/chrysler_cusw.h"
#include "opendbc/safety/modes/rivian.h"
#include "opendbc/safety/modes/subaru.h"
#include "opendbc/safety/modes/subaru_preglobal.h"
#include "opendbc/safety/modes/mazda.h"
#include "opendbc/safety/modes/nissan.h"
#include "opendbc/safety/modes/volkswagen_mlb.h"
#include "opendbc/safety/modes/volkswagen_mqb.h"
#include "opendbc/safety/modes/volkswagen_meb.h"
#include "opendbc/safety/modes/volkswagen_pq.h"
#include "opendbc/safety/modes/elm327.h"
#include "opendbc/safety/modes/body.h"
#include "opendbc/safety/modes/psa.h"
#include "opendbc/safety/modes/hyundai_canfd.h"
uint32_t GET_BYTES(const CANPacket_t *msg, int start, int len) {
uint32_t ret = 0U;
for (int i = 0; i < len; i++) {
const uint32_t shift = i * 8;
ret |= (((uint32_t)msg->data[start + i]) << shift);
}
return ret;
}
const int MAX_WRONG_COUNTERS = 5;
// This can be set by the safety hooks
bool controls_allowed = false;
bool relay_malfunction = false;
bool gas_pressed = false;
bool gas_pressed_prev = false;
bool brake_pressed = false;
bool brake_pressed_prev = false;
bool regen_braking = false;
bool regen_braking_prev = false;
bool steering_disengage;
bool steering_disengage_prev;
bool cruise_engaged_prev = false;
struct sample_t vehicle_speed;
struct sample_t vehicle_speed_2;
bool vehicle_moving = false;
bool acc_main_on = false; // referred to as "ACC off" in ISO 15622:2018
int cruise_button_prev = 0;
bool safety_rx_checks_invalid = false;
bool enable_gas_interceptor = false;
int gas_interceptor_prev = 0;
// for safety modes with torque steering control
int desired_torque_last = 0; // last desired steer torque
int rt_torque_last = 0; // last desired torque for real time check
int valid_steer_req_count = 0; // counter for steer request bit matching non-zero torque
int invalid_steer_req_count = 0; // counter to allow multiple frames of mismatching torque request bit
struct sample_t torque_meas; // last 6 motor torques produced by the eps
struct sample_t torque_driver; // last 6 driver torques measured
uint32_t ts_torque_check_last = 0;
uint32_t ts_steer_req_mismatch_last = 0; // last timestamp steer req was mismatched with torque
// state for controls_allowed timeout logic
bool heartbeat_engaged = false; // openpilot enabled, passed in heartbeat USB command
uint32_t heartbeat_engaged_mismatches = 0; // count of mismatches between heartbeat_engaged and controls_allowed
// for safety modes with angle steering control
uint32_t rt_angle_msgs = 0;
uint32_t ts_angle_check_last = 0;
int desired_angle_last = 0;
struct sample_t angle_meas; // last 6 steer angles
// for safety modes with curvature steering control
CurvatureSteeringState curvature_state;
int alternative_experience = 0;
// time since safety mode has been changed
uint32_t safety_mode_cnt = 0U;
uint16_t current_safety_mode = SAFETY_SILENT;
uint16_t current_safety_param = 0;
uint16_t current_safety_param_sp = 0;
static const safety_hooks *current_hooks = &nooutput_hooks;
safety_config current_safety_config;
static void generic_rx_checks(void);
static void stock_ecu_check(bool stock_ecu_detected);
static bool is_msg_valid(RxCheck addr_list[], 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)) {
valid = false;
controls_allowed = false;
}
}
return valid;
}
static int get_addr_check_index(const CANPacket_t *msg, RxCheck addr_list[], const int len) {
int addr = msg->addr;
int length = GET_LEN(msg);
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].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) && (msg->bus == addr_list[i].msg[j].bus) &&
(length == addr_list[i].msg[j].len)) {
addr_list[i].status.index = j;
addr_list[i].status.msg_seen = true;
break;
}
}
}
if (addr_list[i].status.msg_seen) {
int idx = addr_list[i].status.index;
if ((addr == addr_list[i].msg[idx].addr) && (msg->bus == addr_list[i].msg[idx].bus) &&
(length == addr_list[i].msg[idx].len)) {
index = i;
break;
}
}
}
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 bool rx_msg_safety_check(const CANPacket_t *msg,
const safety_config *cfg,
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) {
// 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) {
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;
} else {
cfg->rx_checks[index].status.valid_checksum = cfg->rx_checks[index].msg[cfg->rx_checks[index].status.index].ignore_checksum;
}
// counter check
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(msg);
update_counter(cfg->rx_checks, index, 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;
}
// 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);
} else {
cfg->rx_checks[index].status.valid_quality_flag = cfg->rx_checks[index].msg[cfg->rx_checks[index].status.index].ignore_quality_flag;
}
}
return is_msg_valid(cfg->rx_checks, index);
}
bool safety_rx_hook(const CANPacket_t *msg) {
bool controls_allowed_prev = controls_allowed;
bool valid = rx_msg_safety_check(msg, &current_safety_config, current_hooks);
bool whitelisted = get_addr_check_index(msg, current_safety_config.rx_checks, current_safety_config.rx_checks_len) != -1;
if (valid && whitelisted) {
current_hooks->rx(msg);
}
// Handles gas, brake, and regen paddle
generic_rx_checks();
// the relay malfunction hook runs on all incoming rx messages.
// check all applicable tx msgs for liveness on sending bus.
// used to detect a relay malfunction or control messages from disabled ECUs like the radar
const int addr = msg->addr;
for (int i = 0; i < current_safety_config.tx_msgs_len; i++) {
const CanMsg *m = &current_safety_config.tx_msgs[i];
if (m->check_relay) {
stock_ecu_check((m->addr == addr) && (m->bus == msg->bus));
}
}
// reset mismatches on rising edge of controls_allowed to avoid rare race condition
if (controls_allowed && !controls_allowed_prev) {
heartbeat_engaged_mismatches = 0;
}
return valid;
}
static bool tx_msg_safety_check(const CANPacket_t *msg, const CanMsg msg_list[], int len) {
int addr = msg->addr;
int length = GET_LEN(msg);
bool whitelisted = false;
for (int i = 0; i < len; i++) {
if ((addr == msg_list[i].addr) && (msg->bus == msg_list[i].bus) && (length == msg_list[i].len)) {
whitelisted = true;
break;
}
}
return whitelisted;
}
bool safety_tx_hook(CANPacket_t *msg) {
bool whitelisted = tx_msg_safety_check(msg, current_safety_config.tx_msgs, current_safety_config.tx_msgs_len);
if ((current_safety_mode == SAFETY_ALLOUTPUT) || (current_safety_mode == SAFETY_ELM327)) {
whitelisted = true;
}
bool safety_allowed = false;
if (whitelisted) {
safety_allowed = current_hooks->tx(msg);
}
return !relay_malfunction && whitelisted && safety_allowed;
}
static int get_fwd_bus(int bus_num) {
int destination_bus;
if (bus_num == 0) {
destination_bus = 2;
} else if (bus_num == 2) {
destination_bus = 0;
} else {
destination_bus = -1;
}
return destination_bus;
}
int safety_fwd_hook(int bus_num, int addr) {
bool blocked = relay_malfunction || current_safety_config.disable_forwarding;
// Block messages that are being checked for relay malfunctions. Safety modes can opt out of this
// in the case of selective AEB forwarding
const int destination_bus = get_fwd_bus(bus_num);
if (!blocked) {
for (int i = 0; i < current_safety_config.tx_msgs_len; i++) {
const CanMsg *m = &current_safety_config.tx_msgs[i];
if (m->check_relay && !m->disable_static_blocking && (m->addr == addr) && (m->bus == (unsigned int)destination_bus)) {
blocked = true;
break;
}
}
}
if (!blocked && (current_hooks->fwd != NULL)) {
blocked = current_hooks->fwd(bus_num, addr);
}
return blocked ? -1 : destination_bus;
}
// 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_8(uint8_t poly, uint8_t crc_lut[]) {
for (uint16_t i = 0U; i <= 0xFFU; i++) {
uint8_t crc = (uint8_t)i;
for (int j = 0; j < 8; j++) {
if ((crc & 0x80U) != 0U) {
crc = (uint8_t)((crc << 1) ^ poly);
} 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;
}
}
// 1Hz safety function called by main. Now just a check for lagging safety messages
void safety_tick(const safety_config *cfg) {
const uint8_t MAX_MISSED_MSGS = 10U;
bool rx_checks_invalid = false;
uint32_t ts = microsecond_timer_get();
if (cfg != NULL) {
for (int i=0; i < cfg->rx_checks_len; i++) {
uint32_t elapsed_time = safety_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 frequency = cfg->rx_checks[i].msg[cfg->rx_checks[i].status.index].frequency;
uint32_t timestep = 1e6 / frequency;
bool lagging = elapsed_time > SAFETY_MAX(timestep * MAX_MISSED_MSGS, 1e6);
cfg->rx_checks[i].status.lagging = lagging;
if (lagging) {
controls_allowed = false;
mads_exit_controls(MADS_DISENGAGE_REASON_LAG);
}
// enforce minimum frequency for safety-relevant messages
bool frequency_invalid = !cfg->rx_checks[i].msg[cfg->rx_checks[i].status.index].ignore_frequency_check && (frequency < 10U);
if (lagging || frequency_invalid || !is_msg_valid(cfg->rx_checks, i)) {
rx_checks_invalid = true;
controls_allowed = false;
}
}
}
safety_rx_checks_invalid = rx_checks_invalid;
}
static void relay_malfunction_set(void) {
relay_malfunction = true;
}
static void generic_rx_checks(void) {
gas_pressed_prev = gas_pressed;
// exit controls on rising edge of brake press
if (brake_pressed && (!brake_pressed_prev || vehicle_moving)) {
controls_allowed = false;
}
brake_pressed_prev = brake_pressed;
// exit controls on rising edge of regen paddle
if (regen_braking && (!regen_braking_prev || vehicle_moving)) {
controls_allowed = false;
}
regen_braking_prev = regen_braking;
// exit controls on rising edge of steering override/disengage
if (steering_disengage && !steering_disengage_prev) {
controls_allowed = false;
}
steering_disengage_prev = steering_disengage;
}
static void stock_ecu_check(bool stock_ecu_detected) {
// allow 1s of transition timeout after relay changes state before assessing malfunctioning
const uint32_t RELAY_TRNS_TIMEOUT = 1U;
// check if stock ECU is on bus broken by car harness
if ((safety_mode_cnt > RELAY_TRNS_TIMEOUT) && stock_ecu_detected) {
relay_malfunction_set();
}
mads_state_update(vehicle_moving, acc_main_on, controls_allowed, brake_pressed || regen_braking, steering_disengage);
}
static void relay_malfunction_reset(void) {
relay_malfunction = false;
}
// resets values and min/max for sample_t struct
static void reset_sample(struct sample_t *sample) {
for (int i = 0; i < MAX_SAMPLE_VALS; i++) {
sample->values[i] = 0;
}
update_sample(sample, 0);
}
int set_safety_hooks(uint16_t mode, uint16_t param) {
const safety_hook_config safety_hook_registry[] = {
{SAFETY_SILENT, &nooutput_hooks},
{SAFETY_HONDA_NIDEC, &honda_nidec_hooks},
{SAFETY_TOYOTA, &toyota_hooks},
{SAFETY_ELM327, &elm327_hooks},
{SAFETY_GM, &gm_hooks},
{SAFETY_HONDA_BOSCH, &honda_bosch_hooks},
{SAFETY_HYUNDAI, &hyundai_hooks},
{SAFETY_CHRYSLER, &chrysler_hooks},
{SAFETY_SUBARU, &subaru_hooks},
{SAFETY_VOLKSWAGEN_MQB, &volkswagen_mqb_hooks},
{SAFETY_VOLKSWAGEN_MEB, &volkswagen_meb_hooks},
{SAFETY_NISSAN, &nissan_hooks},
{SAFETY_NOOUTPUT, &nooutput_hooks},
{SAFETY_HYUNDAI_LEGACY, &hyundai_legacy_hooks},
{SAFETY_MAZDA, &mazda_hooks},
{SAFETY_BODY, &body_hooks},
{SAFETY_FORD, &ford_hooks},
{SAFETY_RIVIAN, &rivian_hooks},
{SAFETY_TESLA, &tesla_hooks},
{SAFETY_HYUNDAI_CANFD, &hyundai_canfd_hooks},
#ifdef ALLOW_DEBUG
{SAFETY_CHRYSLER_CUSW, &chrysler_cusw_hooks},
{SAFETY_PSA, &psa_hooks},
{SAFETY_SUBARU_PREGLOBAL, &subaru_preglobal_hooks},
{SAFETY_VOLKSWAGEN_MLB, &volkswagen_mlb_hooks},
{SAFETY_VOLKSWAGEN_PQ, &volkswagen_pq_hooks},
{SAFETY_ALLOUTPUT, &alloutput_hooks},
#endif
};
// reset state set by safety mode
safety_mode_cnt = 0U;
relay_malfunction = false;
gas_pressed = false;
gas_pressed_prev = false;
brake_pressed = false;
brake_pressed_prev = false;
regen_braking = false;
regen_braking_prev = false;
steering_disengage = false;
steering_disengage_prev = false;
cruise_engaged_prev = false;
vehicle_moving = false;
acc_main_on = false;
cruise_button_prev = 0;
desired_torque_last = 0;
rt_torque_last = 0;
rt_angle_msgs = 0;
ts_angle_check_last = 0;
desired_angle_last = 0;
curvature_state.desired_last = 0;
curvature_state.rt_msgs = 0;
curvature_state.rt_msgs_prev = 0;
curvature_state.ts_check_last = 0;
curvature_state.steer_power_last = 0;
ts_torque_check_last = 0;
ts_steer_req_mismatch_last = 0;
valid_steer_req_count = 0;
invalid_steer_req_count = 0;
// gas interceptor
enable_gas_interceptor = false;
gas_interceptor_prev = 0;
// reset samples
reset_sample(&vehicle_speed);
reset_sample(&vehicle_speed_2);
reset_sample(&torque_meas);
reset_sample(&torque_driver);
reset_sample(&angle_meas);
reset_sample(&curvature_state.meas);
controls_allowed = false;
relay_malfunction_reset();
safety_rx_checks_invalid = false;
current_safety_config.rx_checks = NULL;
current_safety_config.rx_checks_len = 0;
current_safety_config.tx_msgs = NULL;
current_safety_config.tx_msgs_len = 0;
current_safety_config.disable_forwarding = false;
int set_status = -1; // not set
int hook_config_count = sizeof(safety_hook_registry) / sizeof(safety_hook_config);
for (int i = 0; i < hook_config_count; i++) {
if (safety_hook_registry[i].id == mode) {
current_hooks = safety_hook_registry[i].hooks;
current_safety_mode = mode;
current_safety_param = param;
set_status = 0; // set
}
}
if ((set_status == 0) && (current_hooks->init != NULL)) {
safety_config cfg = current_hooks->init(param);
current_safety_config.rx_checks = cfg.rx_checks;
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;
current_safety_config.disable_forwarding = cfg.disable_forwarding;
// 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};
}
}
return set_status;
}
// convert a trimmed integer to signed 32 bit int
int to_signed(int d, int bits) {
int d_signed = d;
int max_value = (1 << SAFETY_MAX((bits - 1), 0));
if (d >= max_value) {
d_signed = d - (1 << SAFETY_MAX(bits, 0));
}
return d_signed;
}
// given a new sample, update the sample_t struct
void update_sample(struct sample_t *sample, int sample_new) {
for (int i = MAX_SAMPLE_VALS - 1; i > 0; i--) {
sample->values[i] = sample->values[i-1];
}
sample->values[0] = sample_new;
// get the minimum and maximum measured samples
sample->min = sample->values[0];
sample->max = sample->values[0];
for (int i = 1; i < MAX_SAMPLE_VALS; i++) {
if (sample->values[i] < sample->min) {
sample->min = sample->values[i];
}
if (sample->values[i] > sample->max) {
sample->max = sample->values[i];
}
}
}
int ROUND(float val) {
return val + ((val > 0.0) ? 0.5 : -0.5);
}
void pcm_cruise_check(bool cruise_engaged) {
// Enter controls on rising edge of stock ACC, exit controls if stock ACC disengages
if (!cruise_engaged) {
controls_allowed = false;
}
if (cruise_engaged && !cruise_engaged_prev) {
controls_allowed = true;
}
cruise_engaged_prev = cruise_engaged;
}
void speed_mismatch_check(const float speed_2) {
// Disable controls if speeds from two sources are too far apart.
// For safety modes that use speed to adjust torque or angle limits
const float MAX_SPEED_DELTA = 2.0; // m/s
bool is_invalid_speed = SAFETY_ABS(speed_2 - ((float)vehicle_speed.values[0] / VEHICLE_SPEED_FACTOR)) > MAX_SPEED_DELTA;
if (is_invalid_speed) {
controls_allowed = false;
}
}
@@ -0,0 +1,192 @@
/**
* Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
*
* This file is part of sunnypilot and is licensed under the MIT License.
* See the LICENSE.md file in the root directory for more details.
*/
#pragma once
#include "opendbc/safety/sunnypilot/mads_declarations.h"
// ===============================
// Global Variables
// ===============================
ButtonState mads_button_press = MADS_BUTTON_UNAVAILABLE;
MADSState m_mads_state;
bool controls_allowed_lateral = false;
// state for mads controls_allowed_lateral timeout logic
bool heartbeat_engaged_mads = false; // MADS enabled, passed in heartbeat USB command
uint32_t heartbeat_engaged_mads_mismatches = 0U; // count of mismatches between heartbeat_engaged_mads and controls_allowed_lateral
// ===============================
// State Update Helpers
// ===============================
inline EdgeTransition m_get_edge_transition(const bool current, const bool last) {
EdgeTransition state;
if (current && !last) {
state = MADS_EDGE_RISING;
} else if (!current && last) {
state = MADS_EDGE_FALLING;
} else {
state = MADS_EDGE_NO_CHANGE;
}
return state;
}
inline void m_mads_state_init(void) {
m_mads_state.is_vehicle_moving = NULL;
m_mads_state.acc_main.current = NULL;
m_mads_state.mads_button.current = MADS_BUTTON_UNAVAILABLE;
m_mads_state.system_enabled = false;
m_mads_state.disengage_lateral_on_brake = false;
m_mads_state.pause_lateral_on_brake = false;
m_mads_state.acc_main.previous = false;
m_mads_state.acc_main.transition = MADS_EDGE_NO_CHANGE;
m_mads_state.mads_button.last = MADS_BUTTON_UNAVAILABLE;
m_mads_state.mads_button.transition = MADS_EDGE_NO_CHANGE;
m_mads_state.current_disengage.active_reason = MADS_DISENGAGE_REASON_NONE;
m_mads_state.current_disengage.pending_reasons = MADS_DISENGAGE_REASON_NONE;
m_mads_state.controls_requested_lateral = false;
controls_allowed_lateral = false;
}
inline void m_update_button_state(ButtonStateTracking *button_state) {
if (button_state->current != MADS_BUTTON_UNAVAILABLE) {
button_state->transition = m_get_edge_transition(button_state->current == MADS_BUTTON_PRESSED, button_state->last == MADS_BUTTON_PRESSED);
button_state->last = button_state->current;
}
}
inline void m_update_binary_state(BinaryStateTracking *state) {
state->transition = m_get_edge_transition(state->current, state->previous);
state->previous = state->current;
}
/**
* @brief Updates the MADS control state based on current system conditions
*
* @return void
*/
inline void m_update_control_state(void) {
bool allowed = true;
// Initial control requests from button or ACC transitions
if ((m_mads_state.acc_main.transition == MADS_EDGE_RISING) ||
(m_mads_state.mads_button.transition == MADS_EDGE_RISING) ||
(m_mads_state.op_controls_allowed.transition == MADS_EDGE_RISING)) {
m_mads_state.controls_requested_lateral = true;
}
// Primary control blockers - these prevent any further control processing
if (m_mads_state.acc_main.transition == MADS_EDGE_FALLING) {
mads_exit_controls(MADS_DISENGAGE_REASON_ACC_MAIN_OFF);
allowed = false; // No matter what, no further control processing on this cycle
}
if (m_mads_state.mads_steering_disengage.transition == MADS_EDGE_RISING) {
mads_exit_controls(MADS_DISENGAGE_REASON_STEERING_DISENGAGE);
allowed = false; // No matter what, no further control processing on this cycle
}
if (m_mads_state.disengage_lateral_on_brake && (m_mads_state.braking.transition == MADS_EDGE_RISING)) {
mads_exit_controls(MADS_DISENGAGE_REASON_BRAKE);
allowed = false;
}
// Secondary control conditions - only checked if primary conditions don't block further control processing
if (allowed && m_mads_state.pause_lateral_on_brake) {
// Brake rising edge immediately blocks controls
// Brake release might request controls if brake was the ONLY reason for disengagement
if (m_mads_state.braking.transition == MADS_EDGE_RISING) {
mads_exit_controls(MADS_DISENGAGE_REASON_BRAKE);
allowed = false;
} else if ((m_mads_state.braking.transition == MADS_EDGE_FALLING) &&
(m_mads_state.current_disengage.active_reason == MADS_DISENGAGE_REASON_BRAKE) &&
(m_mads_state.current_disengage.pending_reasons == MADS_DISENGAGE_REASON_BRAKE)) {
m_mads_state.controls_requested_lateral = true;
} else if (m_mads_state.braking.current) {
allowed = false;
} else {
}
}
// Process control request if conditions allow. Gate the write on system_enabled so that
// stock safety builds (MADS disabled) never flip the global on.
if (allowed && m_mads_state.system_enabled && m_mads_state.controls_requested_lateral && !controls_allowed_lateral) {
m_mads_state.controls_requested_lateral = false;
controls_allowed_lateral = true;
m_mads_state.current_disengage.active_reason = MADS_DISENGAGE_REASON_NONE;
m_mads_state.current_disengage.pending_reasons = MADS_DISENGAGE_REASON_NONE;
}
}
inline void mads_heartbeat_engaged_check(void) {
if (controls_allowed_lateral && !heartbeat_engaged_mads) {
heartbeat_engaged_mads_mismatches += 1U;
if (heartbeat_engaged_mads_mismatches >= 3U) {
mads_exit_controls(MADS_DISENGAGE_REASON_HEARTBEAT_ENGAGED_MISMATCH);
}
} else {
heartbeat_engaged_mads_mismatches = 0U;
}
}
// ===============================
// Function Implementations
// ===============================
inline void mads_set_alternative_experience(const int *mode) {
const bool mads_enabled = (*mode & ALT_EXP_ENABLE_MADS) != 0;
const bool disengage_lateral_on_brake = (*mode & ALT_EXP_MADS_DISENGAGE_LATERAL_ON_BRAKE) != 0;
const bool pause_lateral_on_brake = (*mode & ALT_EXP_MADS_PAUSE_LATERAL_ON_BRAKE) != 0;
mads_set_system_state(mads_enabled, disengage_lateral_on_brake, pause_lateral_on_brake);
}
extern inline void mads_set_system_state(const bool enabled, const bool disengage_lateral_on_brake, const bool pause_lateral_on_brake) {
m_mads_state_init();
m_mads_state.system_enabled = enabled;
m_mads_state.disengage_lateral_on_brake = disengage_lateral_on_brake;
m_mads_state.pause_lateral_on_brake = pause_lateral_on_brake;
}
inline void mads_exit_controls(const DisengageReason reason) {
// Always track this as a pending reason
m_mads_state.current_disengage.pending_reasons |= reason;
if (controls_allowed_lateral) {
m_mads_state.current_disengage.active_reason = reason;
m_mads_state.controls_requested_lateral = false;
controls_allowed_lateral = false;
}
}
inline void mads_state_update(const bool op_vehicle_moving, const bool op_acc_main, const bool op_allowed, const bool is_braking, const bool _steering_disengage) {
m_mads_state.is_vehicle_moving = op_vehicle_moving;
m_mads_state.acc_main.current = op_acc_main;
m_mads_state.op_controls_allowed.current = op_allowed;
m_mads_state.mads_button.current = mads_button_press;
m_mads_state.braking.current = is_braking;
m_mads_state.mads_steering_disengage.current = _steering_disengage;
m_update_binary_state(&m_mads_state.acc_main);
m_update_binary_state(&m_mads_state.op_controls_allowed);
m_update_binary_state(&m_mads_state.braking);
m_update_binary_state(&m_mads_state.mads_steering_disengage);
m_update_button_state(&m_mads_state.mads_button);
m_update_control_state();
}
@@ -0,0 +1,116 @@
/**
* Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
*
* This file is part of sunnypilot and is licensed under the MIT License.
* See the LICENSE.md file in the root directory for more details.
*/
#pragma once
// ===============================
// Type Definitions and Enums
// ===============================
typedef enum __attribute__((packed)) {
MADS_BUTTON_UNAVAILABLE = -1, ///< Button state cannot be determined
MADS_BUTTON_NOT_PRESSED = 0, ///< Button is not pressed
MADS_BUTTON_PRESSED = 1 ///< Button is pressed
} ButtonState;
typedef enum __attribute__((packed)) {
MADS_EDGE_NO_CHANGE = 0, ///< No state change detected
MADS_EDGE_RISING = 1, ///< State changed from false to true
MADS_EDGE_FALLING = 2 ///< State changed from true to false
} EdgeTransition;
typedef enum __attribute__((packed)) {
MADS_DISENGAGE_REASON_NONE = 0, ///< No disengagement
MADS_DISENGAGE_REASON_BRAKE = 1, ///< Brake pedal pressed
MADS_DISENGAGE_REASON_LAG = 2, ///< System lag detected
MADS_DISENGAGE_REASON_BUTTON = 4, ///< User button press
MADS_DISENGAGE_REASON_ACC_MAIN_OFF = 8, ///< ACC system turned off
MADS_DISENGAGE_REASON_NON_PCM_ACC_MAIN_DESYNC = 16, ///< ACC sync error
MADS_DISENGAGE_REASON_HEARTBEAT_ENGAGED_MISMATCH = 32, ///< Heartbeat mismatch
MADS_DISENGAGE_REASON_STEERING_DISENGAGE = 64, ///< Steering disengage
} DisengageReason;
// ===============================
// Constants and Defines
// ===============================
#define ALT_EXP_ENABLE_MADS 1024
#define ALT_EXP_MADS_DISENGAGE_LATERAL_ON_BRAKE 2048
#define ALT_EXP_MADS_PAUSE_LATERAL_ON_BRAKE 4096
#define MISMATCH_DEFAULT_THRESHOLD 25
// ===============================
// Data Structures
// ===============================
typedef struct {
DisengageReason active_reason; // The reason that actually disengaged controls
DisengageReason pending_reasons; // All conditions that would've prevented engagement while controls were disengaged
} DisengageState;
typedef struct {
ButtonState current;
ButtonState last;
EdgeTransition transition;
} ButtonStateTracking;
typedef struct {
EdgeTransition transition;
bool current : 1;
bool previous : 1;
} BinaryStateTracking;
typedef struct {
bool is_vehicle_moving : 1;
ButtonStateTracking mads_button;
BinaryStateTracking acc_main;
BinaryStateTracking op_controls_allowed;
BinaryStateTracking braking;
BinaryStateTracking mads_steering_disengage;
DisengageState current_disengage;
bool system_enabled : 1;
bool disengage_lateral_on_brake : 1;
bool pause_lateral_on_brake : 1;
bool controls_requested_lateral : 1;
} MADSState;
// ===============================
// Global Variables
// ===============================
extern ButtonState mads_button_press;
extern MADSState m_mads_state;
extern bool controls_allowed_lateral;
// state for mads controls_allowed_lateral timeout logic
extern bool heartbeat_engaged_mads;
extern uint32_t heartbeat_engaged_mads_mismatches;
// ===============================
// External Function Declarations (kept as needed)
// ===============================
extern void mads_set_system_state(bool enabled, bool disengage_lateral_on_brake, bool pause_lateral_on_brake);
extern void mads_set_alternative_experience(const int *mode);
extern void mads_state_update(bool op_vehicle_moving, bool op_acc_main, bool op_allowed, bool is_braking, bool steering_disengage);
extern void mads_exit_controls(DisengageReason reason);
extern void mads_heartbeat_engaged_check(void);
// ===============================
// Inline Function Implementations, must be included in the header file to comply with MISRA-C:2012 Rule 8.10
// These are really only used internally.
// ===============================
extern EdgeTransition m_get_edge_transition(bool current, bool last);
extern void m_mads_state_init(void);
extern void m_update_button_state(ButtonStateTracking *button_state);
extern void m_update_binary_state(BinaryStateTracking *state);
extern void m_update_control_state(void);
Whitespace-only changes.
File diff suppressed because it is too large. Load diff
@@ -0,0 +1,83 @@
"""
Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
This file is part of sunnypilot and is licensed under the MIT License.
See the LICENSE.md file in the root directory for more details.
"""
import numpy as np
import unittest
from opendbc.safety.tests.common import CANPackerSafety, CarSafetyTest
class GasInterceptorSafetyTest(CarSafetyTest):
INTERCEPTOR_THRESHOLD = 0
cnt_gas_cmd = 0
cnt_user_gas = 0
packer: CANPackerSafety
@classmethod
def setUpClass(cls):
if cls.__name__ == "GasInterceptorSafetyTest" or cls.__name__.endswith("Base"):
cls.safety = None
raise unittest.SkipTest
def _interceptor_gas_cmd(self, gas: int):
values: dict[str, float | int] = {"PEDAL_COUNTER": self.__class__.cnt_gas_cmd & 0xF}
if gas > 0:
values["GAS_COMMAND"] = gas * 255.
values["GAS_COMMAND2"] = gas * 255.
self.__class__.cnt_gas_cmd += 1
return self.packer.make_can_msg_safety("GAS_COMMAND", 0, values)
def _interceptor_user_gas(self, gas: int):
values = {"INTERCEPTOR_GAS": gas, "INTERCEPTOR_GAS2": gas,
"PEDAL_COUNTER": self.__class__.cnt_user_gas}
self.__class__.cnt_user_gas += 1
return self.packer.make_can_msg_safety("GAS_SENSOR", 0, values)
# Skip non-interceptor user gas tests
def test_prev_gas(self):
pass
def test_no_disengage_on_gas(self):
pass
def test_prev_gas_interceptor(self):
self._rx(self._interceptor_user_gas(0x0))
self.assertFalse(self.safety.get_gas_interceptor_prev())
self._rx(self._interceptor_user_gas(0x1000))
self.assertTrue(self.safety.get_gas_interceptor_prev())
self._rx(self._interceptor_user_gas(0x0))
def test_no_disengage_on_gas_interceptor(self):
self.safety.set_controls_allowed(True)
for g in range(0x1000):
self._rx(self._interceptor_user_gas(g))
# Test we allow lateral, but not longitudinal
self.assertTrue(self.safety.get_controls_allowed())
self.assertEqual(g <= self.INTERCEPTOR_THRESHOLD, self.safety.get_longitudinal_allowed())
# Make sure we can re-gain longitudinal actuation
self._rx(self._interceptor_user_gas(0))
self.assertTrue(self.safety.get_longitudinal_allowed())
def test_allow_engage_with_gas_interceptor_pressed(self):
self._rx(self._interceptor_user_gas(0x1000))
self.safety.set_controls_allowed(True)
self._rx(self._interceptor_user_gas(0x1000))
self.assertTrue(self.safety.get_controls_allowed())
self._rx(self._interceptor_user_gas(0))
def test_gas_interceptor_safety_check(self):
for gas in np.arange(0, 4000, 100):
for controls_allowed in [True, False]:
self.safety.set_controls_allowed(controls_allowed)
if controls_allowed:
send = True
else:
send = gas == 0
self.assertEqual(send, self._tx(self._interceptor_gas_cmd(gas)))
@@ -0,0 +1,296 @@
import unittest
from opendbc.sunnypilot.car.hyundai.values import HyundaiSafetyFlagsSP
import opendbc.safety.tests.common as common
from opendbc.safety.tests.libsafety import libsafety_py
from opendbc.safety.tests.common import make_msg
class Buttons:
NONE = 0
RESUME = 1
SET = 2
CANCEL = 4
PREV_BUTTON_SAMPLES = 8
ENABLE_BUTTONS = (Buttons.RESUME, Buttons.SET, Buttons.CANCEL)
class HyundaiButtonBase:
BUTTONS_TX_BUS = 0 # tx on this bus, rx on 0
SCC_BUS = 0 # rx on this bus
def test_button_sends(self):
"""
RES, SET and CANCEL buttons are allowed
- RES and SET allowed while controls allowed
- CANCEL allowed while cruise is enabled
"""
self.safety.set_controls_allowed(0)
self.assertFalse(self._tx(self._button_msg(Buttons.RESUME, bus=self.BUTTONS_TX_BUS)))
self.assertFalse(self._tx(self._button_msg(Buttons.SET, bus=self.BUTTONS_TX_BUS)))
self.safety.set_controls_allowed(1)
self.assertTrue(self._tx(self._button_msg(Buttons.RESUME, bus=self.BUTTONS_TX_BUS)))
self.assertTrue(self._tx(self._button_msg(Buttons.SET, bus=self.BUTTONS_TX_BUS)))
for enabled in (True, False):
self._rx(self._pcm_status_msg(enabled))
self.assertEqual(enabled, self._tx(self._button_msg(Buttons.CANCEL, bus=self.BUTTONS_TX_BUS)))
def test_enable_control_allowed_from_cruise(self):
"""
Hyundai non-longitudinal only enables on PCM rising edge and recent button press. Tests PCM enabling with:
- disallowed: No buttons
- disallowed: Buttons that don't enable cruise
- allowed: Buttons that do enable cruise
- allowed: Main button with all above combinations
"""
for main_button in (0, 1):
for btn in range(8):
for _ in range(PREV_BUTTON_SAMPLES): # reset
self._rx(self._button_msg(Buttons.NONE))
self._rx(self._pcm_status_msg(False))
self.assertFalse(self.safety.get_controls_allowed())
self._rx(self._button_msg(btn, main_button=main_button))
self._rx(self._pcm_status_msg(True))
controls_allowed = btn in ENABLE_BUTTONS or main_button
self.assertEqual(controls_allowed, self.safety.get_controls_allowed())
def test_sampling_cruise_buttons(self):
"""
Test that we allow controls on recent button press, but not as button leaves sliding window
"""
self._rx(self._button_msg(Buttons.SET))
for i in range(2 * PREV_BUTTON_SAMPLES):
self._rx(self._pcm_status_msg(False))
self.assertFalse(self.safety.get_controls_allowed())
self._rx(self._pcm_status_msg(True))
controls_allowed = i < PREV_BUTTON_SAMPLES
self.assertEqual(controls_allowed, self.safety.get_controls_allowed())
self._rx(self._button_msg(Buttons.NONE))
class HyundaiLongitudinalBase(common.LongitudinalAccelSafetyTest):
DISABLED_ECU_UDS_MSG: tuple[int, int]
DISABLED_ECU_ACTUATION_MSG: tuple[int, int]
@classmethod
def setUpClass(cls):
if cls.__name__ == "HyundaiLongitudinalBase":
cls.safety = None
raise unittest.SkipTest
# override these tests from CarSafetyTest, hyundai longitudinal uses button enable
def test_disable_control_allowed_from_cruise(self):
pass
def test_enable_control_allowed_from_cruise(self):
pass
def test_sampling_cruise_buttons(self):
pass
def test_cruise_engaged_prev(self):
pass
def test_button_sends(self):
pass
def _pcm_status_msg(self, enable):
raise Exception
def _accel_msg(self, accel, aeb_req=False, aeb_decel=0):
raise NotImplementedError
def _acc_state_msg(self, enable):
raise NotImplementedError
def _tx_acc_state_msg(self, enable):
raise NotImplementedError
def test_pcm_main_cruise_state_availability(self):
pass
def test_set_resume_buttons(self):
"""
SET and RESUME enter controls allowed on their falling edge.
"""
for btn_prev in range(8):
for btn_cur in range(8):
self._rx(self._button_msg(Buttons.NONE))
self.safety.set_controls_allowed(0)
for _ in range(10):
self._rx(self._button_msg(btn_prev))
self.assertFalse(self.safety.get_controls_allowed())
# should enter controls allowed on falling edge and not transitioning to cancel
should_enable = btn_cur != btn_prev and \
btn_cur != Buttons.CANCEL and \
btn_prev in (Buttons.RESUME, Buttons.SET)
self._rx(self._button_msg(btn_cur))
self.assertEqual(should_enable, self.safety.get_controls_allowed())
def test_cancel_button(self):
self.safety.set_controls_allowed(1)
self._rx(self._button_msg(Buttons.CANCEL))
self.assertFalse(self.safety.get_controls_allowed())
def test_main_cruise_button(self):
"""Test that main cruise button correctly toggles acc_main_on state"""
default_safety_mode = self.safety.get_current_safety_mode()
default_safety_param = self.safety.get_current_safety_param()
default_safety_param_sp = self.safety.get_current_safety_param_sp()
for enable_mads in (True, False):
with self.subTest("enable_mads", mads_enabled=enable_mads):
for main_cruise_toggleable in (True, False):
with self.subTest("main_cruise_toggleable", main_cruise_toggleable=main_cruise_toggleable):
main_cruise_toggleable_flag = HyundaiSafetyFlagsSP.LONG_MAIN_CRUISE_TOGGLEABLE if main_cruise_toggleable else 0
self.safety.set_current_safety_param_sp(default_safety_param_sp | main_cruise_toggleable_flag)
self.safety.set_safety_hooks(default_safety_mode, default_safety_param)
# Test initial state
self.safety.set_mads_params(enable_mads, False, False)
self.assertFalse(self.safety.get_acc_main_on())
self._rx(self._main_cruise_button_msg(0))
self._rx(self._main_cruise_button_msg(1))
self.assertEqual(enable_mads and main_cruise_toggleable, self.safety.get_controls_allowed_lateral())
self._rx(self._main_cruise_button_msg(0))
self.assertEqual(enable_mads and main_cruise_toggleable, self.safety.get_controls_allowed_lateral())
self._rx(self._main_cruise_button_msg(1))
self.assertFalse(self.safety.get_controls_allowed_lateral())
for _ in range(10):
self._rx(self._main_cruise_button_msg(1))
self.assertFalse(self.safety.get_controls_allowed_lateral())
self.safety.set_current_safety_param_sp(default_safety_param_sp)
def test_acc_main_sync_mismatches_reset(self):
"""Test that acc_main_on_mismatches resets properly on rising edge of main button"""
default_safety_mode = self.safety.get_current_safety_mode()
default_safety_param = self.safety.get_current_safety_param()
default_safety_param_sp = self.safety.get_current_safety_param_sp()
for enable_mads in (True, False):
with self.subTest("enable_mads", mads_enabled=enable_mads):
main_cruise_toggleable_flag = HyundaiSafetyFlagsSP.LONG_MAIN_CRUISE_TOGGLEABLE
self.safety.set_current_safety_param_sp(default_safety_param_sp | main_cruise_toggleable_flag)
self.safety.set_safety_hooks(default_safety_mode, default_safety_param)
self.safety.set_mads_params(enable_mads, False, False)
# Initial state
self._rx(self._main_cruise_button_msg(0))
self.assertFalse(self.safety.get_acc_main_on())
# Set up mismatch condition
self._rx(self._main_cruise_button_msg(1)) # Press button - acc_main_on = True
self._rx(self._main_cruise_button_msg(0)) # Release button
self._tx(self._tx_acc_state_msg(False)) # acc_main_on_tx = False
self.assertTrue(self.safety.get_acc_main_on())
self.assertEqual(1, self.safety.get_acc_main_on_mismatches())
# Rising edge of acc_main_on should reset
self._rx(self._main_cruise_button_msg(1)) # Press button again
self._rx(self._main_cruise_button_msg(0)) # Release button
self._tx(self._tx_acc_state_msg(False)) # acc_main_on_tx = False
self.assertFalse(self.safety.get_acc_main_on())
self.assertEqual(0, self.safety.get_acc_main_on_mismatches())
self.safety.set_current_safety_param_sp(default_safety_param_sp)
def test_acc_main_sync_mismatch_counter(self):
"""Test mismatch counter behavior and disengagement"""
default_safety_mode = self.safety.get_current_safety_mode()
default_safety_param = self.safety.get_current_safety_param()
default_safety_param_sp = self.safety.get_current_safety_param_sp()
for enable_mads in (True, False):
with self.subTest("enable_mads", mads_enabled=enable_mads):
main_cruise_toggleable_flag = HyundaiSafetyFlagsSP.LONG_MAIN_CRUISE_TOGGLEABLE
self.safety.set_current_safety_param_sp(default_safety_param_sp | main_cruise_toggleable_flag)
self.safety.set_safety_hooks(default_safety_mode, default_safety_param)
self.safety.set_mads_params(enable_mads, False, False)
self.safety.set_controls_allowed_lateral(True)
# Start with matched states
self._rx(self._main_cruise_button_msg(0))
self._tx(self._tx_acc_state_msg(False))
self.assertEqual(0, self.safety.get_acc_main_on_mismatches())
# Create mismatch by enabling acc_main_on
self._rx(self._main_cruise_button_msg(1)) # Press button
self._rx(self._main_cruise_button_msg(0)) # Release button
self._tx(self._tx_acc_state_msg(False)) # acc_main_on_tx stays false
self.assertTrue(self.safety.get_acc_main_on())
self.assertEqual(1, self.safety.get_acc_main_on_mismatches())
# Second mismatch
self._tx(self._tx_acc_state_msg(False))
self.assertTrue(self.safety.get_acc_main_on())
self.assertEqual(2, self.safety.get_acc_main_on_mismatches())
# Third mismatch should trigger disengagement
self._tx(self._tx_acc_state_msg(False))
self.assertFalse(self.safety.get_acc_main_on())
self.assertFalse(self.safety.get_controls_allowed_lateral())
# Counter should reset after disengagement
self._tx(self._tx_acc_state_msg(False))
self.assertEqual(0, self.safety.get_acc_main_on_mismatches())
self.safety.set_current_safety_param_sp(default_safety_param_sp)
def test_acc_main_sync_mismatch_recovery(self):
default_safety_mode = self.safety.get_current_safety_mode()
default_safety_param = self.safety.get_current_safety_param()
default_safety_param_sp = self.safety.get_current_safety_param_sp()
"""Test that mismatch counter resets when states resync"""
for enable_mads in (True, False):
with self.subTest("enable_mads", mads_enabled=enable_mads):
main_cruise_toggleable_flag = HyundaiSafetyFlagsSP.LONG_MAIN_CRUISE_TOGGLEABLE
self.safety.set_current_safety_param_sp(default_safety_param_sp | main_cruise_toggleable_flag)
self.safety.set_safety_hooks(default_safety_mode, default_safety_param)
self.safety.set_mads_params(enable_mads, False, False)
# Create initial mismatch
self._rx(self._main_cruise_button_msg(1)) # Press button
self._rx(self._main_cruise_button_msg(0)) # Release button
self._tx(self._tx_acc_state_msg(False)) # acc_main_on_tx = False
self.assertEqual(1, self.safety.get_acc_main_on_mismatches())
# Sync states
self._tx(self._tx_acc_state_msg(True)) # Match acc_main_on_tx to acc_main_on
self.assertEqual(0, self.safety.get_acc_main_on_mismatches())
self.safety.set_current_safety_param_sp(default_safety_param_sp)
def test_tester_present_allowed(self, ecu_disable: bool = True):
"""
Ensure tester present diagnostic message is allowed to keep ECU knocked out
for longitudinal control.
"""
addr, bus = self.DISABLED_ECU_UDS_MSG
for should_tx, msg in ((True, b"\x02\x3E\x80\x00\x00\x00\x00\x00"),
(False, b"\x03\xAA\xAA\x00\x00\x00\x00\x00")):
tester_present = libsafety_py.make_CANPacket(addr, bus, msg)
self.assertEqual(should_tx and ecu_disable, self._tx(tester_present))
def test_disabled_ecu_alive(self):
"""
If the ECU knockout failed, make sure the relay malfunction is shown
"""
addr, bus = self.DISABLED_ECU_ACTUATION_MSG
self.assertFalse(self.safety.get_relay_malfunction())
self._rx(make_msg(bus, addr, 8))
self.assertTrue(self.safety.get_relay_malfunction())
Whitespace-only changes.
@@ -0,0 +1,12 @@
#include <stdio.h>
#include <stdint.h>
#include <stdlib.h>
#define ALLOW_DEBUG
// TODO: time should just be passed into the hooks we expose
uint32_t timer_cnt = 0;
uint32_t microsecond_timer_get(void);
uint32_t microsecond_timer_get(void) {
return timer_cnt;
}
@@ -0,0 +1,171 @@
import os
import subprocess
import tempfile
from pathlib import Path
from cffi import FFI
from opendbc.safety import LEN_TO_DLC
libsafety_dir = os.path.dirname(os.path.abspath(__file__))
def _build_libsafety(release: bool = False) -> str:
"""Compile libsafety.so to a temp file and return its path."""
root = str(Path(libsafety_dir).parents[3])
safety_c = os.path.join(libsafety_dir, "safety.c")
cflags = [
'-Wall', '-Wextra', '-Werror', '-nostdlib', '-fno-builtin',
'-std=gnu11', '-Wfatal-errors', '-Wno-pointer-to-int-cast',
'-g', '-O0', '-fno-omit-frame-pointer',
]
ldflags = [
'-fsanitize=undefined', '-fno-sanitize-recover=undefined',
]
if not release:
cflags += ['-DALLOW_DEBUG', '-fprofile-arcs', '-ftest-coverage']
ldflags += ['-fprofile-arcs', '-ftest-coverage']
fd, safety_os = tempfile.mkstemp(suffix='.os', dir=libsafety_dir)
os.close(fd)
fd, libsafety_so = tempfile.mkstemp(suffix='.so')
os.close(fd)
subprocess.check_call(['cc', '-fPIC', *cflags, '-I', root, '-c', safety_c, '-o', safety_os])
subprocess.check_call(['cc', '-shared', safety_os, '-o', libsafety_so, *ldflags])
return libsafety_so
ffi = FFI()
ffi.cdef("""
typedef struct {
unsigned char fd : 1;
unsigned char bus : 3;
unsigned char data_len_code : 4;
unsigned char rejected : 1;
unsigned char returned : 1;
unsigned char extended : 1;
unsigned int addr : 29;
unsigned char checksum;
unsigned char data[64];
} CANPacket_t;
""", packed=True)
class CANPacket:
pass
ffi.cdef("""
bool safety_rx_hook(CANPacket_t *msg);
bool safety_tx_hook(CANPacket_t *msg);
int safety_fwd_hook(int bus_num, int addr);
int set_safety_hooks(uint16_t mode, uint16_t param);
void set_controls_allowed(bool c);
bool get_controls_allowed(void);
bool get_longitudinal_allowed(void);
void set_alternative_experience(int mode);
int get_alternative_experience(void);
void set_relay_malfunction(bool c);
bool get_relay_malfunction(void);
bool get_gas_pressed_prev(void);
void set_gas_pressed_prev(bool);
bool get_brake_pressed_prev(void);
bool get_regen_braking_prev(void);
bool get_steering_disengage_prev(void);
bool get_acc_main_on(void);
float get_vehicle_speed_min(void);
float get_vehicle_speed_max(void);
int get_current_safety_mode(void);
int get_current_safety_param(void);
void set_torque_meas(int min, int max);
int get_torque_meas_min(void);
int get_torque_meas_max(void);
void set_torque_driver(int min, int max);
int get_torque_driver_min(void);
int get_torque_driver_max(void);
void set_desired_torque_last(int t);
void set_rt_torque_last(int t);
void set_desired_angle_last(int t);
int get_desired_angle_last();
void set_angle_meas(int min, int max);
int get_angle_meas_min(void);
int get_angle_meas_max(void);
void set_desired_curvature_last(int t);
int get_desired_curvature_last();
void set_curvature_meas(int min, int max);
int get_curvature_meas_min(void);
int get_curvature_meas_max(void);
bool get_cruise_engaged_prev(void);
void set_cruise_engaged_prev(bool engaged);
bool get_vehicle_moving(void);
void set_timer(uint32_t t);
void safety_tick_current_safety_config();
bool safety_config_valid();
void init_tests(void);
void set_honda_fwd_brake(bool c);
bool get_honda_fwd_brake(void);
void set_honda_alt_brake_msg(bool c);
void set_honda_bosch_long(bool c);
int get_honda_hw(void);
void mutation_set_active_mutant(int id);
int mutation_get_active_mutant(void);
void ignition_can_hook(const CANPacket_t *msg);
bool get_ignition_can(void);
void set_ignition_can(bool c);
bool get_controls_allowed_lateral(void);
bool get_controls_requested_lateral(void);
void set_current_safety_param_sp(uint16_t param);
uint16_t get_current_safety_param_sp(void);
bool get_enable_mads(void);
bool get_disengage_lateral_on_brake(void);
bool get_pause_lateral_on_brake(void);
void set_mads_button_press(int mads_button_press);
void set_controls_allowed_lateral(bool c);
void set_controls_requested_lateral(bool c);
bool get_mads_acc_main(void);
void set_acc_main_on(bool c);
int get_mads_button_press(void);
void mads_set_current_disengage_reason(int reason);
int mads_get_current_disengage_reason(void);
int get_temp_debug(void);
uint32_t get_acc_main_on_mismatches(void);
void set_mads_params(bool enable_mads, bool disengage_lateral_on_brake, bool pause_lateral_on_brake);
void mads_apply_alternative_experience(int mode);
void tick_mads_state(bool vm, bool acc_main, bool op_allowed, bool braking, bool steering_disengage);
void set_heartbeat_engaged_mads(bool c);
void mads_heartbeat_engaged_check(void);
void set_steering_disengage(bool c);
int get_gas_interceptor_prev(void);
""")
class LibSafety:
pass
libsafety: LibSafety
def load(path):
global libsafety
libsafety = ffi.dlopen(str(path))
def __getattr__(name):
if name == "libsafety":
load(_build_libsafety())
return libsafety
raise AttributeError(name)
def make_CANPacket(addr: int, bus: int, dat):
ret = ffi.new('CANPacket_t *')
ret[0].extended = 1 if addr >= 0x800 else 0
ret[0].addr = addr
ret[0].data_len_code = LEN_TO_DLC[len(dat)]
ret[0].bus = bus
ret[0].data = bytes(dat)
return ret
@@ -0,0 +1,344 @@
#include <stdio.h>
#include <stdint.h>
#include <stdlib.h>
// TODO: time should just be passed into the hooks we expose
uint32_t timer_cnt = 0;
uint32_t microsecond_timer_get(void);
uint32_t microsecond_timer_get(void) {
return timer_cnt;
}
#include "opendbc/safety/can.h"
#include "opendbc/safety/safety.h"
#include "opendbc/safety/ignition.h"
void safety_tick_current_safety_config() {
safety_tick(&current_safety_config);
}
bool safety_config_valid() {
if (current_safety_config.rx_checks_len <= 0) {
printf("missing RX checks\n");
return false;
}
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;
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);
return false;
}
}
return true;
}
void set_controls_allowed(bool c){
controls_allowed = c;
}
void set_alternative_experience(int mode){
alternative_experience = mode;
}
void mads_apply_alternative_experience(int mode){
mads_set_alternative_experience(&mode);
}
void tick_mads_state(bool vm, bool acc_main, bool op_allowed, bool braking, bool steering_disengage){
mads_state_update(vm, acc_main, op_allowed, braking, steering_disengage);
}
void set_relay_malfunction(bool c){
relay_malfunction = c;
}
void set_ignition_can(bool c){
ignition_can = c;
}
bool get_controls_allowed(void){
return controls_allowed;
}
bool get_ignition_can(void){
return ignition_can;
}
int get_alternative_experience(void){
return alternative_experience;
}
bool get_relay_malfunction(void){
return relay_malfunction;
}
bool get_gas_pressed_prev(void){
return gas_pressed_prev;
}
void set_gas_pressed_prev(bool c){
gas_pressed_prev = c;
}
bool get_brake_pressed_prev(void){
return brake_pressed_prev;
}
bool get_regen_braking_prev(void){
return regen_braking_prev;
}
bool get_steering_disengage_prev(void){
return steering_disengage_prev;
}
bool get_cruise_engaged_prev(void){
return cruise_engaged_prev;
}
void set_cruise_engaged_prev(bool engaged){
cruise_engaged_prev = engaged;
}
bool get_vehicle_moving(void){
return vehicle_moving;
}
bool get_acc_main_on(void){
return acc_main_on;
}
float get_vehicle_speed_min(void){
return vehicle_speed.min / VEHICLE_SPEED_FACTOR;
}
float get_vehicle_speed_max(void){
return vehicle_speed.max / VEHICLE_SPEED_FACTOR;
}
int get_current_safety_mode(void){
return current_safety_mode;
}
int get_current_safety_param(void){
return current_safety_param;
}
void set_timer(uint32_t t){
timer_cnt = t;
}
void set_torque_meas(int min, int max){
torque_meas.min = min;
torque_meas.max = max;
}
int get_torque_meas_min(void){
return torque_meas.min;
}
int get_torque_meas_max(void){
return torque_meas.max;
}
void set_torque_driver(int min, int max){
torque_driver.min = min;
torque_driver.max = max;
}
int get_torque_driver_min(void){
return torque_driver.min;
}
int get_torque_driver_max(void){
return torque_driver.max;
}
void set_rt_torque_last(int t){
rt_torque_last = t;
}
void set_desired_torque_last(int t){
desired_torque_last = t;
}
void set_desired_angle_last(int t){
desired_angle_last = t;
}
int get_desired_angle_last(void){
return desired_angle_last;
}
void set_angle_meas(int min, int max){
angle_meas.min = min;
angle_meas.max = max;
}
int get_angle_meas_min(void){
return angle_meas.min;
}
int get_angle_meas_max(void){
return angle_meas.max;
}
void set_desired_curvature_last(int t){
curvature_state.desired_last = t;
}
int get_desired_curvature_last(void){
return curvature_state.desired_last;
}
void set_curvature_meas(int min, int max){
curvature_state.meas.min = min;
curvature_state.meas.max = max;
}
int get_curvature_meas_min(void){
return curvature_state.meas.min;
}
int get_curvature_meas_max(void){
return curvature_state.meas.max;
}
// ***** car specific helpers *****
void set_honda_alt_brake_msg(bool c){
honda_alt_brake_msg = c;
}
void set_honda_bosch_long(bool c){
honda_bosch_long = c;
}
int get_honda_hw(void) {
return honda_hw;
}
void set_honda_fwd_brake(bool c){
honda_fwd_brake = c;
}
bool get_honda_fwd_brake(void){
return honda_fwd_brake;
}
static MADSState *get_mads_state(void) {
return &m_mads_state;
}
bool get_controls_allowed_lateral(void){
return controls_allowed_lateral;
}
bool get_controls_requested_lateral(void){
return get_mads_state()->controls_requested_lateral;
}
bool get_enable_mads(void){
return get_mads_state()->system_enabled;
}
bool get_disengage_lateral_on_brake(void){
return get_mads_state()->disengage_lateral_on_brake;
}
bool get_pause_lateral_on_brake(void){
return get_mads_state()->pause_lateral_on_brake;
}
void set_acc_main_on(bool c){
acc_main_on = c;
}
void set_current_safety_param_sp(uint16_t param){
current_safety_param_sp = param;
}
uint16_t get_current_safety_param_sp(void){
return current_safety_param_sp;
}
void set_mads_button_press(int c){
mads_button_press = c;
}
int get_mads_button_press(void){
return mads_button_press;
}
void set_controls_allowed_lateral(bool c){
controls_allowed_lateral = c;
}
bool get_mads_acc_main(void){
return m_mads_state.acc_main.current;
}
int mads_get_current_disengage_reason(void) {
return get_mads_state()->current_disengage.active_reason;
}
void mads_set_current_disengage_reason(int reason) {
m_mads_state.current_disengage.active_reason = reason;
}
void set_controls_requested_lateral(bool c){
m_mads_state.controls_requested_lateral = c;
}
void set_mads_params(bool enable_mads, bool disengage_lateral_on_brake, bool pause_lateral_on_brake){
alternative_experience = 0;
if (enable_mads) {
alternative_experience |= ALT_EXP_ENABLE_MADS;
if (disengage_lateral_on_brake) {
alternative_experience |= ALT_EXP_MADS_DISENGAGE_LATERAL_ON_BRAKE;
} else if (pause_lateral_on_brake) {
alternative_experience |= ALT_EXP_MADS_PAUSE_LATERAL_ON_BRAKE;
} else {
}
}
mads_set_alternative_experience(&alternative_experience);
}
void set_heartbeat_engaged_mads(bool c){
heartbeat_engaged_mads = c;
}
void set_steering_disengage(bool c){
steering_disengage = c;
}
int get_gas_interceptor_prev(void){
return gas_interceptor_prev;
}
void init_tests(void){
safety_mode_cnt = 2U; // avoid ignoring relay_malfunction logic
alternative_experience = 0;
current_safety_param_sp = 0;
set_timer(0);
ts_steer_req_mismatch_last = 0;
valid_steer_req_count = 0;
invalid_steer_req_count = 0;
// assumes autopark on safety mode init to avoid a fault. get rid of that for testing
tesla_autopark = false;
ignition_can = false;
ignition_can_cnt = 0U;
// reset MADS state to prevent leaking between tests
mads_set_system_state(false, false, false);
mads_button_press = MADS_BUTTON_UNAVAILABLE;
heartbeat_engaged_mads = false;
heartbeat_engaged_mads_mismatches = 0U;
}
@@ -0,0 +1,652 @@
from opendbc.testing import parameterized
import abc
import unittest
from opendbc.safety.tests.libsafety import libsafety_py
class MadsSafetyTestBase(unittest.TestCase):
safety: libsafety_py.LibSafety
@abc.abstractmethod
def _lkas_button_msg(self, enabled):
raise NotImplementedError
@abc.abstractmethod
def _acc_state_msg(self, enabled):
raise NotImplementedError
def setUp(self):
super().setUp()
self.safety.set_controls_allowed(False)
self.safety.set_mads_button_press(-1)
self.safety.set_controls_allowed_lateral(False)
self.safety.set_controls_requested_lateral(False)
self.safety.set_acc_main_on(False)
self.safety.set_mads_params(False, False, False)
self.safety.set_heartbeat_engaged_mads(True)
def test_heartbeat_engaged_mads_check(self):
"""Test MADS heartbeat engaged check behavior"""
for boolean in (True, False):
# If boolean is True, the heartbeat is engaged and should remain engaged, otherwise it should disengage.
with self.subTest(heartbeat_engaged=boolean, should_remain_engaged=boolean):
# Setup initial conditions
self.safety.set_mads_params(True, False, False) # Enable MADS
self.safety.set_controls_allowed_lateral(True)
self.assertTrue(self.safety.get_controls_allowed_lateral())
# Set heartbeat engaged state based on test case
self.safety.set_heartbeat_engaged_mads(boolean)
# Call the heartbeat check function multiple times
# We know from the implementation that it takes 3 mismatches to disengage
for _ in range(4): # More than 3 times to ensure we pass the threshold
self.safety.mads_heartbeat_engaged_check()
# Verify engagement state matches expectation
self.assertEqual(self.safety.get_controls_allowed_lateral(), boolean,
f"Expected controls_allowed_lateral to be [{boolean}] but got [{self.safety.get_controls_allowed_lateral()}]")
def test_enable_control_allowed_with_mads_button(self):
"""Toggle MADS with MADS button"""
try:
self._lkas_button_msg(False)
except NotImplementedError as err:
raise unittest.SkipTest("Skipping test because MADS button is not supported") from err
for enable_mads in (True, False):
with self.subTest("enable_mads", mads_enabled=enable_mads):
self.safety.set_mads_params(enable_mads, False, False)
self.assertEqual(enable_mads, self.safety.get_enable_mads())
self._rx(self._lkas_button_msg(True))
self._rx(self._lkas_button_msg(False))
self.assertEqual(enable_mads, self.safety.get_controls_allowed_lateral())
def test_enable_control_allowed_with_manual_acc_main_on_state(self):
try:
self._acc_state_msg(False)
except NotImplementedError as err:
raise unittest.SkipTest("Skipping test because _acc_state_msg is not implemented for this car") from err
for enable_mads in (True, False):
with self.subTest("enable_mads", mads_enabled=enable_mads):
self.safety.set_mads_params(enable_mads, False, False)
self._rx(self._acc_state_msg(True))
self.assertEqual(enable_mads, self.safety.get_controls_allowed_lateral())
def test_enable_control_allowed_with_manual_mads_button_state(self):
for enable_mads in (True, False):
with self.subTest("enable_mads", mads_enabled=enable_mads):
for mads_button_press in (-1, 0, 1):
with self.subTest("mads_button_press", button_state=mads_button_press):
self.safety.set_mads_params(enable_mads, False, False)
self.safety.set_mads_button_press(mads_button_press)
self._rx(self._speed_msg(0))
self.assertEqual(enable_mads and mads_button_press == 1, self.safety.get_controls_allowed_lateral())
def test_enable_control_allowed_from_acc_main_on(self):
"""Test that lateral controls are allowed when ACC main is enabled and disabled when ACC main is disabled"""
for enable_mads in (True, False):
with self.subTest("enable_mads", mads_enabled=enable_mads):
for acc_main_on in (True, False):
with self.subTest("initial_acc_main", initial_acc_main=acc_main_on):
self.safety.set_controls_allowed(False)
self.safety.set_mads_params(enable_mads, False, False)
# Set initial state
self.safety.set_acc_main_on(acc_main_on)
self._rx(self._speed_msg(0))
expected_lat = enable_mads and acc_main_on
self.assertEqual(expected_lat, self.safety.get_controls_allowed_lateral(),
f"Expected lat: [{expected_lat}] when acc_main_on goes to [{acc_main_on}]")
# Test transition to opposite state
self.safety.set_acc_main_on(not acc_main_on)
self._rx(self._speed_msg(0))
expected_lat = enable_mads and not acc_main_on
self.assertEqual(expected_lat, self.safety.get_controls_allowed_lateral(),
f"Expected lat: [{expected_lat}] when acc_main_on goes from [{acc_main_on}] to [{not acc_main_on}]")
# Test transition back to initial state
self.safety.set_acc_main_on(acc_main_on)
self._rx(self._speed_msg(0))
expected_lat = enable_mads and acc_main_on
self.assertEqual(expected_lat, self.safety.get_controls_allowed_lateral(),
f"Expected lat: [{expected_lat}] when acc_main_on goes from [{not acc_main_on}] to [{acc_main_on}]")
def test_mads_with_acc_main_on(self):
for enable_mads in (True, False):
with self.subTest("enable_mads", mads_enabled=enable_mads):
self.safety.set_mads_params(enable_mads, False, False)
self.safety.set_acc_main_on(True)
self._rx(self._speed_msg(0))
self.assertEqual(enable_mads, self.safety.get_controls_allowed_lateral())
self.safety.set_acc_main_on(False)
self._rx(self._speed_msg(0))
self.assertFalse(self.safety.get_controls_allowed_lateral())
def test_pause_lateral_on_brake_setup(self):
for enable_mads in (True, False):
with self.subTest("enable_mads", enable_mads=enable_mads):
for pause_lateral_on_brake in (True, False):
with self.subTest("pause_lateral_on_brake", pause_lateral_on_brake=pause_lateral_on_brake):
self.safety.set_mads_params(enable_mads, False, pause_lateral_on_brake)
self.assertEqual(enable_mads and pause_lateral_on_brake, self.safety.get_pause_lateral_on_brake())
def test_pause_lateral_on_brake(self):
self.safety.set_mads_params(True, False, True)
self._rx(self._user_brake_msg(False))
self.safety.set_controls_requested_lateral(True)
self.safety.set_controls_allowed_lateral(True)
self._rx(self._user_brake_msg(True))
# Test we pause lateral
self.assertFalse(self.safety.get_controls_allowed_lateral())
# Make sure we can re-gain lateral actuation
self._rx(self._user_brake_msg(False))
self.assertTrue(self.safety.get_controls_allowed_lateral())
def test_no_pause_lateral_on_brake(self):
self.safety.set_mads_params(True, False, False)
self._rx(self._user_brake_msg(False))
self.safety.set_controls_requested_lateral(True)
self.safety.set_controls_allowed_lateral(True)
self._rx(self._user_brake_msg(True))
self.assertTrue(self.safety.get_controls_allowed_lateral())
@parameterized("engage_method", ["mads_button", "acc_main_on"])
def test_engage_with_brake_pressed(self, engage_method):
if engage_method == "mads_button":
try:
self._lkas_button_msg(False)
except NotImplementedError as err:
raise unittest.SkipTest("Skipping test because MADS button is not supported") from err
elif engage_method == "acc_main_on":
try:
self._acc_state_msg(False)
except NotImplementedError as err:
raise unittest.SkipTest("Skipping test because ACC main is not supported") from err
for enable_mads in (True, False):
with self.subTest("enable_mads", enable_mads=enable_mads):
for pause_lateral_on_brake in (True, False):
with self.subTest("pause_lateral_on_brake", pause_lateral_on_brake=pause_lateral_on_brake):
with self.subTest(engage_method):
self.safety.set_mads_params(enable_mads, False, pause_lateral_on_brake)
# Brake press rising edge
self._rx(self._user_brake_msg(True))
if engage_method == "mads_button":
self._rx(self._lkas_button_msg(True))
elif engage_method == "acc_main_on":
self.safety.set_acc_main_on(True)
self.assertTrue(self.safety.get_acc_main_on())
else:
raise ValueError(f"Invalid engage_method: {engage_method}")
self._rx(self._speed_msg(0))
self.assertEqual(enable_mads and not pause_lateral_on_brake, self.safety.get_controls_allowed_lateral())
# Continuous braking after the first frame of brake press rising edge
for _ in range(400):
self.assertEqual(enable_mads and not pause_lateral_on_brake, self.safety.get_controls_allowed_lateral())
def test_pause_lateral_on_brake_with_pressed_and_released(self):
for enable_mads in (True, False):
with self.subTest("enable_mads", enable_mads=enable_mads):
for pause_lateral_on_brake in (True, False):
with self.subTest("pause_lateral_on_brake", pause_lateral_on_brake=pause_lateral_on_brake):
self.safety.set_mads_params(enable_mads, False, pause_lateral_on_brake)
# Set controls_allowed_lateral rising edge
self.safety.set_controls_requested_lateral(True)
self._rx(self._speed_msg(0))
self.assertEqual(enable_mads, self.safety.get_controls_allowed_lateral())
# User brake press, validate controls_allowed_lateral is false
self._rx(self._user_brake_msg(True))
self.assertEqual(enable_mads and not pause_lateral_on_brake, self.safety.get_controls_allowed_lateral())
# User brake release, validate controls_allowed_lateral is true
self._rx(self._user_brake_msg(False))
self.assertEqual(enable_mads, self.safety.get_controls_allowed_lateral())
def test_pause_lateral_on_brake_persistent_control_allowed_off(self):
self.safety.set_mads_params(True, False, True)
self.safety.set_controls_requested_lateral(True)
# Vehicle moving, validate controls_allowed_lateral is true
for _ in range(10):
self._rx(self._speed_msg(10))
self.assertTrue(self.safety.get_controls_allowed_lateral())
# User braked, vehicle slowed down in 10 frames, then stopped for 10 frames
# Validate controls_allowed_lateral is false
self._rx(self._user_brake_msg(True))
for _ in range(10):
self._rx(self._speed_msg(5))
self.assertFalse(self.safety.get_controls_allowed_lateral())
for _ in range(10):
self._rx(self._speed_msg(0))
self.assertFalse(self.safety.get_controls_allowed_lateral())
def test_enable_lateral_control_with_controls_allowed_rising_edge(self):
for enable_mads in (True, False):
with self.subTest("enable_mads", enable_mads=enable_mads):
self.safety.set_mads_params(enable_mads, False, False)
self.safety.set_controls_allowed(False)
self._rx(self._speed_msg(0))
self.safety.set_controls_allowed(True)
self._rx(self._speed_msg(0))
self.assertTrue(self.safety.get_controls_allowed())
def test_enable_control_allowed_with_mads_button_and_disable_with_main_cruise(self):
"""Tests main cruise and MADS button state transitions.
Sequence:
1. Main cruise off -> on
2. MADS button engage
3. Main cruise off
"""
try:
self._lkas_button_msg(False)
except NotImplementedError as err:
raise unittest.SkipTest("Skipping test because MADS button is not supported") from err
try:
self._acc_state_msg(False)
except NotImplementedError as err:
raise unittest.SkipTest("Skipping test because _acc_state_msg is not implemented for this car") from err
for enable_mads in (True, False):
with self.subTest("enable_mads", enable_mads=enable_mads):
self.safety.set_mads_params(enable_mads, False, False)
self._rx(self._lkas_button_msg(True))
self._rx(self._lkas_button_msg(False))
self.assertEqual(enable_mads, self.safety.get_controls_allowed_lateral())
self._rx(self._acc_state_msg(True))
self.assertEqual(enable_mads, self.safety.get_controls_allowed_lateral())
self._rx(self._acc_state_msg(False))
self.assertFalse(self.safety.get_controls_allowed_lateral())
def test_brake_disengage_with_control_request(self):
"""Tests behavior when controls are requested while brake is engaged
Sequence:
1. Enable MADS with pause lateral on brake
2. Brake to pause lateral control
3. Set control request while braking
4. Release brake
5. Verify controls become allowed
"""
self.safety.set_mads_params(True, False, True) # enable MADS with pause lateral on brake
# Initial state
self.safety.set_controls_allowed_lateral(True)
self._rx(self._speed_msg(0))
self.assertTrue(self.safety.get_controls_allowed_lateral())
# Brake press disengages lateral
self._rx(self._user_brake_msg(True))
self.assertFalse(self.safety.get_controls_allowed_lateral())
# Request controls while braking
self.safety.set_controls_requested_lateral(True)
self.assertFalse(self.safety.get_controls_allowed_lateral())
# Release brake - should enable since controls were requested
self._rx(self._user_brake_msg(False))
self.assertTrue(self.safety.get_controls_allowed_lateral())
def test_brake_disengage_with_acc_main_off(self):
"""Tests behavior when ACC main is turned off while brake is engaged
Sequence:
1. Enable MADS with pause lateral on brake
2. Brake to pause lateral control
3. Turn ACC main off while braking
4. Release brake
5. Verify controls remain disengaged
"""
self.safety.set_mads_params(True, False, True) # enable MADS with pause lateral on brake
# Initial state - enable with ACC main
self.safety.set_acc_main_on(True)
self._rx(self._speed_msg(0))
self.assertTrue(self.safety.get_controls_allowed_lateral())
# Brake press disengages lateral
self._rx(self._user_brake_msg(True))
self.assertFalse(self.safety.get_controls_allowed_lateral())
# Turn ACC main off while braking
self.safety.set_acc_main_on(False)
self._rx(self._speed_msg(0))
self.assertFalse(self.safety.get_controls_allowed_lateral())
# Release brake - should remain disabled since ACC main is off
self._rx(self._user_brake_msg(False))
self.assertFalse(self.safety.get_controls_allowed_lateral())
def test_steering_disengage_with_control_request(self):
self.safety.set_mads_params(True, False, False)
self.safety.set_controls_allowed_lateral(True)
self._rx(self._speed_msg(0))
self.assertTrue(self.safety.get_controls_allowed_lateral())
self.safety.set_steering_disengage(True)
self._rx(self._speed_msg(0))
self.assertFalse(self.safety.get_controls_allowed_lateral())
def test_disengage_on_brake(self):
for disengage_on_brake in (True, False):
self.safety.set_mads_params(True, disengage_on_brake, False)
self.safety.set_controls_allowed_lateral(True)
self._rx(self._speed_msg(0))
self.assertTrue(self.safety.get_controls_allowed_lateral())
self._rx(self._user_brake_msg(True))
self.assertEqual(not disengage_on_brake, self.safety.get_controls_allowed_lateral())
self._rx(self._user_brake_msg(False))
self.assertEqual(not disengage_on_brake, self.safety.get_controls_allowed_lateral())
def test_heartbeat_engaged_mads_exact_threshold(self):
"""Test that exactly 3 heartbeat mismatches triggers disengage (not 2 or 4)"""
self.safety.set_mads_params(True, False, False)
self.safety.set_controls_allowed_lateral(True)
self.safety.set_heartbeat_engaged_mads(False)
# After 2 mismatches: still engaged
for _ in range(2):
self.safety.mads_heartbeat_engaged_check()
self.assertTrue(self.safety.get_controls_allowed_lateral(),
"Should still be engaged after 2 mismatches")
# 3rd mismatch: disengaged
self.safety.mads_heartbeat_engaged_check()
self.assertFalse(self.safety.get_controls_allowed_lateral(),
"Should disengage after exactly 3 mismatches")
def test_heartbeat_engaged_mads_reset_on_match(self):
"""Test that mismatch counter resets when heartbeat matches"""
self.safety.set_mads_params(True, False, False)
self.safety.set_controls_allowed_lateral(True)
self.safety.set_heartbeat_engaged_mads(False)
# 2 mismatches
for _ in range(2):
self.safety.mads_heartbeat_engaged_check()
self.assertTrue(self.safety.get_controls_allowed_lateral())
# Match resets counter
self.safety.set_heartbeat_engaged_mads(True)
self.safety.mads_heartbeat_engaged_check()
self.assertTrue(self.safety.get_controls_allowed_lateral())
# Need 3 more mismatches from scratch to disengage
self.safety.set_heartbeat_engaged_mads(False)
for _ in range(2):
self.safety.mads_heartbeat_engaged_check()
self.assertTrue(self.safety.get_controls_allowed_lateral(),
"Counter should have reset; 2 mismatches after reset should not disengage")
def test_mads_button_not_engaged_without_press(self):
"""Test that MADS button in idle state does not engage lateral control"""
try:
self._lkas_button_msg(False)
except NotImplementedError as err:
raise unittest.SkipTest("MADS button not supported") from err
self.safety.set_mads_params(True, False, False)
# Only send idle/release — should NOT engage
self._rx(self._lkas_button_msg(False))
self._rx(self._speed_msg(0))
self.assertFalse(self.safety.get_controls_allowed_lateral(),
"Button idle/release alone should not engage lateral control")
def test_mads_params_individual_flags(self):
"""Test that each MADS param flag is independently wired correctly.
Kills mutation mutants for ALT_EXP boundary values (1024/2048/4096).
"""
for enable, disengage, pause in [
(False, False, False),
(True, False, False),
(True, True, False),
(True, False, True),
]:
with self.subTest(enable=enable, disengage=disengage, pause=pause):
self.safety.set_mads_params(enable, disengage, pause)
self.assertEqual(enable, self.safety.get_enable_mads())
self.assertEqual(enable and disengage, self.safety.get_disengage_lateral_on_brake())
self.assertEqual(enable and pause, self.safety.get_pause_lateral_on_brake())
def test_lat_active_only_with_mads_enabled(self):
"""lat_active reflects MADS lateral state only when MADS is enabled"""
for enable_mads in (True, False):
with self.subTest(enable_mads=enable_mads):
self.safety.set_mads_params(enable_mads, False, False)
self.safety.set_controls_allowed(False)
# Engage via ACC main
self.safety.set_acc_main_on(True)
self._rx(self._speed_msg(0))
self.assertEqual(enable_mads, self.safety.get_controls_allowed_lateral())
self.assertEqual(enable_mads, self.safety.get_controls_allowed_lateral())
self.assertFalse(self.safety.get_longitudinal_allowed())
def test_longitudinal_allowed_independent_of_mads(self):
"""longitudinal_allowed depends only on controls_allowed, not MADS"""
for enable_mads in (True, False):
with self.subTest(enable_mads=enable_mads):
self.safety.set_mads_params(enable_mads, False, False)
self.safety.set_controls_allowed(True)
self.assertTrue(self.safety.get_longitudinal_allowed())
self.safety.set_controls_allowed(False)
self.assertFalse(self.safety.get_longitudinal_allowed())
def test_lat_active_with_controls_allowed_and_mads(self):
"""Lateral TX allow = controls_allowed OR controls_allowed_lateral (MADS). Verify each leg."""
self.safety.set_mads_params(True, False, False)
self.safety.set_controls_allowed(False)
# MADS lat only
self.safety.set_acc_main_on(True)
self._rx(self._speed_msg(0))
self.assertTrue(self.safety.get_controls_allowed_lateral())
self.assertFalse(self.safety.get_controls_allowed())
# controls_allowed only (disable MADS lat)
self.safety.set_acc_main_on(False)
self._rx(self._speed_msg(0))
self.safety.set_controls_allowed(True)
self.assertTrue(self.safety.get_controls_allowed() or self.safety.get_controls_allowed_lateral())
self.assertFalse(self.safety.get_controls_allowed_lateral())
# Both active
self.safety.set_acc_main_on(True)
self._rx(self._speed_msg(0))
self.assertTrue(self.safety.get_controls_allowed() or self.safety.get_controls_allowed_lateral())
self.assertTrue(self.safety.get_controls_allowed())
self.assertTrue(self.safety.get_controls_allowed_lateral())
def test_lat_active_false_when_mads_disabled_and_controls_not_allowed(self):
"""lat_active is False when both MADS and controls_allowed are off"""
self.safety.set_mads_params(False, False, False)
self.safety.set_controls_allowed(False)
self.assertFalse(self.safety.get_controls_allowed_lateral())
self.assertFalse(self.safety.get_longitudinal_allowed())
def test_mads_lateral_only_controls_split(self):
"""Verify lat/long control states are independent in MADS lateral-only mode.
When MADS lat is active but controls_allowed is off, only lateral should be active."""
self.safety.set_mads_params(True, False, False)
self.safety.set_controls_allowed(False)
# Engage MADS lateral via ACC main
self.safety.set_acc_main_on(True)
self._rx(self._speed_msg(0))
# Lateral active, longitudinal not
self.assertTrue(self.safety.get_controls_allowed_lateral())
self.assertTrue(self.safety.get_controls_allowed_lateral())
self.assertFalse(self.safety.get_controls_allowed())
self.assertFalse(self.safety.get_longitudinal_allowed())
# Engage cruise (controls_allowed) — both active
self.safety.set_controls_allowed(True)
self.assertTrue(self.safety.get_controls_allowed_lateral())
self.assertTrue(self.safety.get_longitudinal_allowed())
# Disengage cruise — back to lateral only
self.safety.set_controls_allowed(False)
self.assertTrue(self.safety.get_controls_allowed_lateral())
self.assertFalse(self.safety.get_longitudinal_allowed())
# Disengage MADS lateral — nothing active
self.safety.set_acc_main_on(False)
self._rx(self._speed_msg(0))
self.assertFalse(self.safety.get_controls_allowed_lateral())
self.assertFalse(self.safety.get_longitudinal_allowed())
def test_mads_brake_disengage_lateral_only_mode(self):
"""In MADS lateral-only mode with disengage-on-brake, brake kills lateral while long stays off."""
self.safety.set_mads_params(True, True, False) # disengage lateral on brake
# MADS lateral only (cruise not engaged)
self.safety.set_acc_main_on(True)
self._rx(self._speed_msg(0))
self.assertFalse(self.safety.get_controls_allowed())
self.assertTrue(self.safety.get_controls_allowed_lateral())
self.assertTrue(self.safety.get_controls_allowed_lateral())
self.assertFalse(self.safety.get_longitudinal_allowed())
# Brake: MADS lateral disengages, controls_allowed stays off
self._rx(self._user_brake_msg(True))
self.assertFalse(self.safety.get_controls_allowed_lateral())
self.assertFalse(self.safety.get_controls_allowed())
self.assertFalse(self.safety.get_controls_allowed_lateral())
self.assertFalse(self.safety.get_longitudinal_allowed())
def test_lateral_defaults_false_on_safety_init(self):
"""controlsAllowedLateral must be false after safety init, even with MADS system enabled."""
self.safety.set_mads_params(True, False, False)
# After init, system_enabled is True but controls_allowed_lateral must be False
self.assertFalse(self.safety.get_controls_allowed_lateral())
self.assertFalse(self.safety.get_controls_allowed_lateral())
self.assertFalse(self.safety.get_controls_allowed())
# Requires explicit user action to enable
self.safety.set_acc_main_on(True)
self._rx(self._speed_msg(0))
self.assertTrue(self.safety.get_controls_allowed_lateral())
self.assertTrue(self.safety.get_controls_allowed_lateral())
def test_safety_tick_clears_lateral(self):
"""Message lag via safety_tick clears controls_allowed_lateral, causing is_lat_active to go false."""
self.safety.set_mads_params(True, False, False)
self.safety.set_acc_main_on(True)
self._rx(self._speed_msg(0))
self.assertTrue(self.safety.get_controls_allowed_lateral())
self.assertTrue(self.safety.get_controls_allowed_lateral())
# Simulate message lag — safety_tick clears both controls_allowed and controls_allowed_lateral
self.safety.set_timer(int(2e6))
self.safety.safety_tick_current_safety_config()
self.assertFalse(self.safety.get_controls_allowed_lateral())
self.assertFalse(self.safety.get_controls_allowed_lateral())
self.assertFalse(self.safety.get_controls_allowed())
def test_heartbeat_agreement_disengage_on_brake(self):
"""With disengage-on-brake, after brake the Python side sends enabled=False.
Verify no heartbeat mismatch since both sides agree lateral is off."""
self.safety.set_mads_params(True, True, False) # disengage lateral on brake
self._rx(self._user_brake_msg(False))
self.safety.set_controls_allowed_lateral(True)
self.assertTrue(self.safety.get_controls_allowed_lateral())
# Heartbeat matches — both active
self.safety.set_heartbeat_engaged_mads(True)
self.safety.mads_heartbeat_engaged_check()
self.assertTrue(self.safety.get_controls_allowed_lateral())
# Brake: safety clears controls_allowed_lateral
self._rx(self._user_brake_msg(True))
self.assertFalse(self.safety.get_controls_allowed_lateral())
# Python side: disengage mode → state=disabled → enabled=False → heartbeat sends False
self.safety.set_heartbeat_engaged_mads(False)
for _ in range(4):
self.safety.mads_heartbeat_engaged_check()
# No mismatch: controls_allowed_lateral=False, so check doesn't trigger
self.assertFalse(self.safety.get_controls_allowed_lateral())
def test_heartbeat_agreement_pause_on_brake(self):
"""With pause-on-brake, after brake the Python side sends enabled=True (paused is enabled).
Verify no heartbeat mismatch and re-enable on brake release."""
self.safety.set_mads_params(True, False, True) # pause lateral on brake
self._rx(self._user_brake_msg(False))
self.safety.set_controls_requested_lateral(True)
self.safety.set_controls_allowed_lateral(True)
self.assertTrue(self.safety.get_controls_allowed_lateral())
# Brake: safety pauses — controls_allowed_lateral goes False
self._rx(self._user_brake_msg(True))
self.assertFalse(self.safety.get_controls_allowed_lateral())
# Python side: pause mode → state=paused → enabled=True → heartbeat sends True
self.safety.set_heartbeat_engaged_mads(True)
for _ in range(4):
self.safety.mads_heartbeat_engaged_check()
# No mismatch: controls_allowed_lateral=False, check condition (lat && !hb) = False
self.assertFalse(self.safety.get_controls_allowed_lateral())
# Brake release: safety re-enables lateral
self._rx(self._user_brake_msg(False))
self.assertTrue(self.safety.get_controls_allowed_lateral())
# Heartbeat still True (Python side: paused→enabled, enabled=True)
self.safety.set_heartbeat_engaged_mads(True)
for _ in range(4):
self.safety.mads_heartbeat_engaged_check()
# No mismatch: both agree lateral is active
self.assertTrue(self.safety.get_controls_allowed_lateral())
def test_mads_button_rising_edge_from_not_pressed(self):
self.safety.set_mads_params(True, False, False)
self.safety.set_mads_button_press(0)
self.safety.tick_mads_state(False, False, False, False, False)
self.assertFalse(self.safety.get_controls_allowed_lateral())
self.safety.set_mads_button_press(1)
self.safety.tick_mads_state(False, False, False, False, False)
self.assertTrue(self.safety.get_controls_allowed_lateral())
def test_alt_exp_bit_discrimination(self):
self.safety.mads_apply_alternative_experience(1)
self.assertFalse(self.safety.get_enable_mads())
self.assertFalse(self.safety.get_disengage_lateral_on_brake())
self.assertFalse(self.safety.get_pause_lateral_on_brake())
@@ -0,0 +1,456 @@
Cppcheck checkers list from test_misra.sh:
TEST variant options:
--enable=all --enable=unusedFunction --addon=misra -DCANFD /opendbc/safety/main.c
Critical errors
---------------
No critical errors encountered.
Note: There might still have been non-critical bailouts which might lead to false negatives.
Open source checkers
--------------------
Yes Check64BitPortability::pointerassignment
Yes CheckAssert::assertWithSideEffects
Yes CheckAutoVariables::assignFunctionArg
Yes CheckAutoVariables::autoVariables
Yes CheckAutoVariables::checkVarLifetime
No CheckBool::checkAssignBoolToFloat require:style,c++
Yes CheckBool::checkAssignBoolToPointer
No CheckBool::checkBitwiseOnBoolean require:style,inconclusive
Yes CheckBool::checkComparisonOfBoolExpressionWithInt
No CheckBool::checkComparisonOfBoolWithBool require:style,c++
No CheckBool::checkComparisonOfBoolWithInt require:warning,c++
No CheckBool::checkComparisonOfFuncReturningBool require:style,c++
Yes CheckBool::checkIncrementBoolean
Yes CheckBool::pointerArithBool
Yes CheckBool::returnValueOfFunctionReturningBool
Yes CheckBufferOverrun::analyseWholeProgram
Yes CheckBufferOverrun::argumentSize
Yes CheckBufferOverrun::arrayIndex
Yes CheckBufferOverrun::arrayIndexThenCheck
Yes CheckBufferOverrun::bufferOverflow
Yes CheckBufferOverrun::negativeArraySize
Yes CheckBufferOverrun::objectIndex
Yes CheckBufferOverrun::pointerArithmetic
No CheckBufferOverrun::stringNotZeroTerminated require:warning,inconclusive
Yes CheckClass::analyseWholeProgram
No CheckClass::checkConst require:style,inconclusive
No CheckClass::checkConstructors require:style,warning
No CheckClass::checkCopyConstructors require:warning
No CheckClass::checkDuplInheritedMembers require:warning
No CheckClass::checkExplicitConstructors require:style
No CheckClass::checkMemset
No CheckClass::checkMissingOverride require:style,c++03
No CheckClass::checkReturnByReference require:performance
No CheckClass::checkSelfInitialization
No CheckClass::checkThisUseAfterFree require:warning
No CheckClass::checkUnsafeClassRefMember require:warning,safeChecks
No CheckClass::checkUselessOverride require:style
No CheckClass::checkVirtualFunctionCallInConstructor require:warning
No CheckClass::initializationListUsage require:performance
No CheckClass::initializerListOrder require:style,inconclusive
No CheckClass::operatorEqRetRefThis require:style
No CheckClass::operatorEqToSelf require:warning
No CheckClass::privateFunctions require:style
No CheckClass::thisSubtraction require:warning
No CheckClass::virtualDestructor
Yes CheckCondition::alwaysTrueFalse
Yes CheckCondition::assignIf
Yes CheckCondition::checkAssignmentInCondition
Yes CheckCondition::checkBadBitmaskCheck
Yes CheckCondition::checkCompareValueOutOfTypeRange
Yes CheckCondition::checkDuplicateConditionalAssign
Yes CheckCondition::checkIncorrectLogicOperator
Yes CheckCondition::checkInvalidTestForOverflow
Yes CheckCondition::checkModuloAlwaysTrueFalse
Yes CheckCondition::checkPointerAdditionResultNotNull
Yes CheckCondition::clarifyCondition
Yes CheckCondition::comparison
Yes CheckCondition::duplicateCondition
Yes CheckCondition::multiCondition
Yes CheckCondition::multiCondition2
No CheckExceptionSafety::checkCatchExceptionByValue require:style
No CheckExceptionSafety::checkRethrowCopy require:style
No CheckExceptionSafety::deallocThrow require:warning
No CheckExceptionSafety::destructors require:warning
No CheckExceptionSafety::nothrowThrows
No CheckExceptionSafety::rethrowNoCurrentException
No CheckExceptionSafety::unhandledExceptionSpecification require:style,inconclusive
Yes CheckFunctions::checkIgnoredReturnValue
Yes CheckFunctions::checkMathFunctions
Yes CheckFunctions::checkMissingReturn
Yes CheckFunctions::checkProhibitedFunctions
Yes CheckFunctions::invalidFunctionUsage
Yes CheckFunctions::memsetInvalid2ndParam
Yes CheckFunctions::memsetZeroBytes
No CheckFunctions::returnLocalStdMove require:performance,c++11
Yes CheckFunctions::useStandardLibrary
No CheckIO::checkCoutCerrMisusage require:c
Yes CheckIO::checkFileUsage
Yes CheckIO::checkWrongPrintfScanfArguments
Yes CheckIO::invalidScanf
Yes CheckLeakAutoVar::check
No CheckMemoryLeakInClass::check
Yes CheckMemoryLeakInFunction::checkReallocUsage
Yes CheckMemoryLeakNoVar::check
No CheckMemoryLeakNoVar::checkForUnsafeArgAlloc
Yes CheckMemoryLeakStructMember::check
Yes CheckNullPointer::analyseWholeProgram
Yes CheckNullPointer::arithmetic
Yes CheckNullPointer::nullConstantDereference
Yes CheckNullPointer::nullPointer
No CheckOther::checkAccessOfMovedVariable require:c++11,warning
Yes CheckOther::checkCastIntToCharAndBack
Yes CheckOther::checkCharVariable
Yes CheckOther::checkComparePointers
Yes CheckOther::checkComparisonFunctionIsAlwaysTrueOrFalse
Yes CheckOther::checkConstPointer
No CheckOther::checkConstVariable require:style,c++
No CheckOther::checkDuplicateBranch require:style,inconclusive
Yes CheckOther::checkDuplicateExpression
Yes CheckOther::checkEvaluationOrder
Yes CheckOther::checkFuncArgNamesDifferent
No CheckOther::checkIncompleteArrayFill require:warning,portability,inconclusive
Yes CheckOther::checkIncompleteStatement
No CheckOther::checkInterlockedDecrement require:windows-platform
Yes CheckOther::checkInvalidFree
Yes CheckOther::checkKnownArgument
Yes CheckOther::checkKnownPointerToBool
No CheckOther::checkMisusedScopedObject require:style,c++
Yes CheckOther::checkModuloOfOne
Yes CheckOther::checkNanInArithmeticExpression
Yes CheckOther::checkNegativeBitwiseShift
Yes CheckOther::checkOverlappingWrite
No CheckOther::checkPassByReference require:performance,c++
Yes CheckOther::checkRedundantAssignment
No CheckOther::checkRedundantCopy require:c++,performance,inconclusive
Yes CheckOther::checkRedundantPointerOp
Yes CheckOther::checkShadowVariables
Yes CheckOther::checkSignOfUnsignedVariable
No CheckOther::checkSuspiciousCaseInSwitch require:warning,inconclusive
No CheckOther::checkSuspiciousSemicolon require:warning,inconclusive
Yes CheckOther::checkUnreachableCode
Yes CheckOther::checkUnusedLabel
Yes CheckOther::checkVarFuncNullUB
Yes CheckOther::checkVariableScope
Yes CheckOther::checkZeroDivision
Yes CheckOther::clarifyCalculation
Yes CheckOther::clarifyStatement
Yes CheckOther::invalidPointerCast
Yes CheckOther::redundantBitwiseOperationInSwitch
Yes CheckOther::suspiciousFloatingPointCast
No CheckOther::warningOldStylePointerCast require:style,c++
No CheckPostfixOperator::postfixOperator require:performance
Yes CheckSizeof::checkSizeofForArrayParameter
Yes CheckSizeof::checkSizeofForNumericParameter
Yes CheckSizeof::checkSizeofForPointerSize
Yes CheckSizeof::sizeofCalculation
Yes CheckSizeof::sizeofFunction
Yes CheckSizeof::sizeofVoid
Yes CheckSizeof::sizeofsizeof
No CheckSizeof::suspiciousSizeofCalculation require:warning,inconclusive
No CheckStl::checkDereferenceInvalidIterator require:warning
No CheckStl::checkDereferenceInvalidIterator2
No CheckStl::checkFindInsert require:performance
No CheckStl::checkMutexes require:warning
No CheckStl::erase
No CheckStl::eraseIteratorOutOfBounds
No CheckStl::if_find require:warning,performance
No CheckStl::invalidContainer
No CheckStl::iterators
No CheckStl::knownEmptyContainer require:style
No CheckStl::misMatchingContainerIterator
No CheckStl::misMatchingContainers
No CheckStl::missingComparison require:warning
No CheckStl::negativeIndex
No CheckStl::outOfBounds
No CheckStl::outOfBoundsIndexExpression
No CheckStl::redundantCondition require:style
No CheckStl::size require:performance,c++03
No CheckStl::stlBoundaries
No CheckStl::stlOutOfBounds
No CheckStl::string_c_str
No CheckStl::useStlAlgorithm require:style
No CheckStl::uselessCalls require:performance,warning
Yes CheckString::checkAlwaysTrueOrFalseStringCompare
Yes CheckString::checkIncorrectStringCompare
Yes CheckString::checkSuspiciousStringCompare
Yes CheckString::overlappingStrcmp
Yes CheckString::sprintfOverlappingData
Yes CheckString::strPlusChar
Yes CheckString::stringLiteralWrite
Yes CheckType::checkFloatToIntegerOverflow
Yes CheckType::checkIntegerOverflow
Yes CheckType::checkLongCast
Yes CheckType::checkSignConversion
Yes CheckType::checkTooBigBitwiseShift
Yes CheckUninitVar::analyseWholeProgram
Yes CheckUninitVar::check
Yes CheckUninitVar::valueFlowUninit
Yes CheckUnusedFunctions::check
Yes CheckUnusedVar::checkFunctionVariableUsage
Yes CheckUnusedVar::checkStructMemberUsage
Yes CheckVaarg::va_list_usage
Yes CheckVaarg::va_start_argument
Premium checkers
----------------
Not available, Cppcheck Premium is not used
Autosar
-------
Not available, Cppcheck Premium is not used
Cert C
------
Not available, Cppcheck Premium is not used
Cert C++
--------
Not available, Cppcheck Premium is not used
Misra C 2012
------------
No Misra C 2012: Dir 1.1
No Misra C 2012: Dir 2.1
No Misra C 2012: Dir 3.1
No Misra C 2012: Dir 4.1
No Misra C 2012: Dir 4.2
No Misra C 2012: Dir 4.3
No Misra C 2012: Dir 4.4
No Misra C 2012: Dir 4.5
No Misra C 2012: Dir 4.6 amendment:3
No Misra C 2012: Dir 4.7
No Misra C 2012: Dir 4.8
No Misra C 2012: Dir 4.9 amendment:3
No Misra C 2012: Dir 4.10
No Misra C 2012: Dir 4.11 amendment:3
No Misra C 2012: Dir 4.12
No Misra C 2012: Dir 4.13
No Misra C 2012: Dir 4.14 amendment:2
No Misra C 2012: Dir 4.15 amendment:3
No Misra C 2012: Dir 5.1 amendment:4
No Misra C 2012: Dir 5.2 amendment:4
No Misra C 2012: Dir 5.3 amendment:4
Yes Misra C 2012: 1.1
Yes Misra C 2012: 1.2
Yes Misra C 2012: 1.3
Yes Misra C 2012: 1.4 amendment:2
No Misra C 2012: 1.5 amendment:3 require:premium
Yes Misra C 2012: 2.1
Yes Misra C 2012: 2.2
Yes Misra C 2012: 2.3
Yes Misra C 2012: 2.4
Yes Misra C 2012: 2.5
Yes Misra C 2012: 2.6
Yes Misra C 2012: 2.7
Yes Misra C 2012: 2.8
Yes Misra C 2012: 3.1
Yes Misra C 2012: 3.2
Yes Misra C 2012: 4.1
Yes Misra C 2012: 4.2
Yes Misra C 2012: 5.1
Yes Misra C 2012: 5.2
Yes Misra C 2012: 5.3
Yes Misra C 2012: 5.4
Yes Misra C 2012: 5.5
Yes Misra C 2012: 5.6
Yes Misra C 2012: 5.7
Yes Misra C 2012: 5.8
Yes Misra C 2012: 5.9
Yes Misra C 2012: 6.1
Yes Misra C 2012: 6.2
No Misra C 2012: 6.3
Yes Misra C 2012: 7.1
Yes Misra C 2012: 7.2
Yes Misra C 2012: 7.3
Yes Misra C 2012: 7.4
No Misra C 2012: 7.5
No Misra C 2012: 7.6
Yes Misra C 2012: 8.1
Yes Misra C 2012: 8.2
No Misra C 2012: 8.3
Yes Misra C 2012: 8.4
Yes Misra C 2012: 8.5
Yes Misra C 2012: 8.6
Yes Misra C 2012: 8.7
Yes Misra C 2012: 8.8
Yes Misra C 2012: 8.9
Yes Misra C 2012: 8.10
Yes Misra C 2012: 8.11
Yes Misra C 2012: 8.12
Yes Misra C 2012: 8.13
Yes Misra C 2012: 8.14
No Misra C 2012: 8.15
No Misra C 2012: 8.16
No Misra C 2012: 8.17
Yes Misra C 2012: 9.1
Yes Misra C 2012: 9.2
Yes Misra C 2012: 9.3
Yes Misra C 2012: 9.4
Yes Misra C 2012: 9.5
No Misra C 2012: 9.6
No Misra C 2012: 9.7
Yes Misra C 2012: 10.1
Yes Misra C 2012: 10.2
Yes Misra C 2012: 10.3
Yes Misra C 2012: 10.4
Yes Misra C 2012: 10.5
Yes Misra C 2012: 10.6
Yes Misra C 2012: 10.7
Yes Misra C 2012: 10.8
Yes Misra C 2012: 11.1
Yes Misra C 2012: 11.2
Yes Misra C 2012: 11.3
Yes Misra C 2012: 11.4
Yes Misra C 2012: 11.5
Yes Misra C 2012: 11.6
Yes Misra C 2012: 11.7
Yes Misra C 2012: 11.8
Yes Misra C 2012: 11.9
No Misra C 2012: 11.10
Yes Misra C 2012: 12.1
Yes Misra C 2012: 12.2
Yes Misra C 2012: 12.3
Yes Misra C 2012: 12.4
Yes Misra C 2012: 12.5 amendment:1
No Misra C 2012: 12.6 amendment:4 require:premium
Yes Misra C 2012: 13.1
No Misra C 2012: 13.2
Yes Misra C 2012: 13.3
Yes Misra C 2012: 13.4
Yes Misra C 2012: 13.5
Yes Misra C 2012: 13.6
Yes Misra C 2012: 14.1
Yes Misra C 2012: 14.2
Yes Misra C 2012: 14.3
Yes Misra C 2012: 14.4
Yes Misra C 2012: 15.1
Yes Misra C 2012: 15.2
Yes Misra C 2012: 15.3
Yes Misra C 2012: 15.4
Yes Misra C 2012: 15.5
Yes Misra C 2012: 15.6
Yes Misra C 2012: 15.7
Yes Misra C 2012: 16.1
Yes Misra C 2012: 16.2
Yes Misra C 2012: 16.3
Yes Misra C 2012: 16.4
Yes Misra C 2012: 16.5
Yes Misra C 2012: 16.6
Yes Misra C 2012: 16.7
Yes Misra C 2012: 17.1
Yes Misra C 2012: 17.2
Yes Misra C 2012: 17.3
No Misra C 2012: 17.4
Yes Misra C 2012: 17.5
Yes Misra C 2012: 17.6
Yes Misra C 2012: 17.7
Yes Misra C 2012: 17.8
No Misra C 2012: 17.9
No Misra C 2012: 17.10
No Misra C 2012: 17.11
No Misra C 2012: 17.12
No Misra C 2012: 17.13
Yes Misra C 2012: 18.1
Yes Misra C 2012: 18.2
Yes Misra C 2012: 18.3
Yes Misra C 2012: 18.4
Yes Misra C 2012: 18.5
Yes Misra C 2012: 18.6
Yes Misra C 2012: 18.7
Yes Misra C 2012: 18.8
No Misra C 2012: 18.9
No Misra C 2012: 18.10
Yes Misra C 2012: 19.1
Yes Misra C 2012: 19.2
Yes Misra C 2012: 20.1
Yes Misra C 2012: 20.2
Yes Misra C 2012: 20.3
Yes Misra C 2012: 20.4
Yes Misra C 2012: 20.5
Yes Misra C 2012: 20.6
Yes Misra C 2012: 20.7
Yes Misra C 2012: 20.8
Yes Misra C 2012: 20.9
Yes Misra C 2012: 20.10
Yes Misra C 2012: 20.11
Yes Misra C 2012: 20.12
Yes Misra C 2012: 20.13
Yes Misra C 2012: 20.14
Yes Misra C 2012: 21.1
Yes Misra C 2012: 21.2
Yes Misra C 2012: 21.3
Yes Misra C 2012: 21.4
Yes Misra C 2012: 21.5
Yes Misra C 2012: 21.6
Yes Misra C 2012: 21.7
Yes Misra C 2012: 21.8
Yes Misra C 2012: 21.9
Yes Misra C 2012: 21.10
Yes Misra C 2012: 21.11
Yes Misra C 2012: 21.12
Yes Misra C 2012: 21.13 amendment:1
Yes Misra C 2012: 21.14 amendment:1
Yes Misra C 2012: 21.15 amendment:1
Yes Misra C 2012: 21.16 amendment:1
Yes Misra C 2012: 21.17 amendment:1
Yes Misra C 2012: 21.18 amendment:1
Yes Misra C 2012: 21.19 amendment:1
Yes Misra C 2012: 21.20 amendment:1
Yes Misra C 2012: 21.21 amendment:3
No Misra C 2012: 21.22 amendment:3 require:premium
No Misra C 2012: 21.23 amendment:3 require:premium
No Misra C 2012: 21.24 amendment:3 require:premium
No Misra C 2012: 21.25 amendment:4 require:premium
No Misra C 2012: 21.26 amendment:4 require:premium
Yes Misra C 2012: 22.1
Yes Misra C 2012: 22.2
Yes Misra C 2012: 22.3
Yes Misra C 2012: 22.4
Yes Misra C 2012: 22.5
Yes Misra C 2012: 22.6
Yes Misra C 2012: 22.7 amendment:1
Yes Misra C 2012: 22.8 amendment:1
Yes Misra C 2012: 22.9 amendment:1
Yes Misra C 2012: 22.10 amendment:1
No Misra C 2012: 22.11 amendment:4 require:premium
No Misra C 2012: 22.12 amendment:4 require:premium
No Misra C 2012: 22.13 amendment:4 require:premium
No Misra C 2012: 22.14 amendment:4 require:premium
No Misra C 2012: 22.15 amendment:4 require:premium
No Misra C 2012: 22.16 amendment:4 require:premium
No Misra C 2012: 22.17 amendment:4 require:premium
No Misra C 2012: 22.18 amendment:4 require:premium
No Misra C 2012: 22.19 amendment:4 require:premium
No Misra C 2012: 22.20 amendment:4 require:premium
No Misra C 2012: 23.1 amendment:3 require:premium
No Misra C 2012: 23.2 amendment:3 require:premium
No Misra C 2012: 23.3 amendment:3 require:premium
No Misra C 2012: 23.4 amendment:3 require:premium
No Misra C 2012: 23.5 amendment:3 require:premium
No Misra C 2012: 23.6 amendment:3 require:premium
No Misra C 2012: 23.7 amendment:3 require:premium
No Misra C 2012: 23.8 amendment:3 require:premium
Misra C++ 2008
--------------
Not available, Cppcheck Premium is not used
Misra C++ 2023
--------------
Not available, Cppcheck Premium is not used
@@ -0,0 +1,156 @@
1.1
1.2 X (Addon)
1.3 X (Cppcheck)
2.1 X (Cppcheck)
2.2 X (Addon)
2.3 X (Addon)
2.4 X (Addon)
2.5 X (Addon)
2.6 X (Cppcheck)
2.7 X (Addon)
3.1 X (Addon)
3.2
4.1 X (Addon)
4.2 X (Addon)
5.1 X (Addon)
5.2 X (Addon)
5.3 X (Cppcheck)
5.4 X (Addon)
5.5 X (Addon)
5.6 X (Addon)
5.7 X (Addon)
5.8 X (Addon)
5.9 X (Addon)
6.1 X (Addon)
6.2 X (Addon)
7.1 X (Addon)
7.2 X (Addon)
7.3 X (Addon)
7.4 X (Addon)
8.1 X (Addon)
8.2 X (Addon)
8.3 X (Cppcheck)
8.4 X (Addon)
8.5 X (Addon)
8.6 X (Addon)
8.7 X (Addon)
8.8 X (Addon)
8.9 X (Addon)
8.10 X (Addon)
8.11 X (Addon)
8.12 X (Addon)
8.13 X (Cppcheck)
8.14 X (Addon)
9.1 X (Cppcheck)
9.2 X (Addon)
9.3 X (Addon)
9.4 X (Addon)
9.5 X (Addon)
10.1 X (Addon)
10.2 X (Addon)
10.3 X (Addon)
10.4 X (Addon)
10.5 X (Addon)
10.6 X (Addon)
10.7 X (Addon)
10.8 X (Addon)
11.1 X (Addon)
11.2 X (Addon)
11.3 X (Addon)
11.4 X (Addon)
11.5 X (Addon)
11.6 X (Addon)
11.7 X (Addon)
11.8 X (Addon)
11.9 X (Addon)
12.1 X (Addon)
12.2 X (Addon)
12.3 X (Addon)
12.4 X (Addon)
13.1 X (Addon)
13.2 X (Cppcheck)
13.3 X (Addon)
13.4 X (Addon)
13.5 X (Addon)
13.6 X (Addon)
14.1 X (Addon)
14.2 X (Addon)
14.3 X (Cppcheck)
14.4 X (Addon)
15.1 X (Addon)
15.2 X (Addon)
15.3 X (Addon)
15.4 X (Addon)
15.5 X (Addon)
15.6 X (Addon)
15.7 X (Addon)
16.1 X (Addon)
16.2 X (Addon)
16.3 X (Addon)
16.4 X (Addon)
16.5 X (Addon)
16.6 X (Addon)
16.7 X (Addon)
17.1 X (Addon)
17.2 X (Addon)
17.3 X (Addon)
17.4 X (Cppcheck)
17.5 X (Cppcheck)
17.6 X (Addon)
17.7 X (Addon)
17.8 X (Addon)
18.1 X (Cppcheck)
18.2 X (Cppcheck)
18.3 X (Cppcheck)
18.4 X (Addon)
18.5 X (Addon)
18.6 X (Cppcheck)
18.7 X (Addon)
18.8 X (Addon)
19.1 X (Cppcheck)
19.2 X (Addon)
20.1 X (Addon)
20.2 X (Addon)
20.3 X (Addon)
20.4 X (Addon)
20.5 X (Addon)
20.6 X (Cppcheck)
20.7 X (Addon)
20.8 X (Addon)
20.9 X (Addon)
20.10 X (Addon)
20.11 X (Addon)
20.12 X (Addon)
20.13 X (Addon)
20.14 X (Addon)
21.1 X (Addon)
21.2 X (Addon)
21.3 X (Addon)
21.4 X (Addon)
21.5 X (Addon)
21.6 X (Addon)
21.7 X (Addon)
21.8 X (Addon)
21.9 X (Addon)
21.10 X (Addon)
21.11 X (Addon)
21.12 X (Addon)
21.13 X (Cppcheck)
21.14 X (Addon)
21.15 X (Addon)
21.16 X (Addon)
21.17 X (Cppcheck)
21.18 X (Cppcheck)
21.19 X (Addon)
21.20 X (Addon)
21.21 X (Addon)
22.1 X (Cppcheck)
22.2 X (Cppcheck)
22.3 X (Cppcheck)
22.4 X (Cppcheck)
22.5 X (Addon)
22.6 X (Cppcheck)
22.7 X (Addon)
22.8 X (Addon)
22.9 X (Addon)
22.10 X (Addon)
@@ -0,0 +1,17 @@
#include "opendbc/safety/safety.h"
// this file is checked by cppcheck
extern uint32_t microsecond_timer_get(void);
// Ignore misra-c2012-8.7 as these functions are only called from libsafety
SAFETY_UNUSED(heartbeat_engaged);
SAFETY_UNUSED(safety_rx_hook);
SAFETY_UNUSED(safety_tx_hook);
SAFETY_UNUSED(safety_fwd_hook);
SAFETY_UNUSED(safety_tick);
SAFETY_UNUSED(set_safety_hooks);
SAFETY_UNUSED(mads_heartbeat_engaged_check);
SAFETY_UNUSED(mads_set_alternative_experience);
SAFETY_UNUSED(get_acc_main_on_mismatches);
@@ -0,0 +1,21 @@
# Advisory: casting from void pointer to type pointer is ok. Done by STM libraries as well
misra-c2012-11.4
# Advisory: casting from void pointer to type pointer is ok. Done by STM libraries as well
misra-c2012-11.5
# Advisory: as stated in the Misra document, use of goto statements in accordance to 15.2 and 15.3 is ok
misra-c2012-15.1
# Advisory: union types can be used
misra-c2012-19.2
# Advisory: The # and ## preprocessor operators should not be used
misra-c2012-20.10
# needed since not all of these suppressions are applicable to all builds
unmatchedSuppression
# All interrupt handlers are defined, including ones we don't use
unusedFunction:*/interrupt_handlers*.h
# all of the below suppressions are from new checks introduced after updating
# cppcheck from 2.5 -> 2.13. they are listed here to separate the update from
# fixing the violations and all are intended to be removed soon after
misra-c2012-2.5 # unused macros. a few legit, rest aren't common between F4/H7 builds. should we do this in the unusedFunction pass?
+74
View File
@@ -0,0 +1,74 @@
#!/usr/bin/env bash
set -e
DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" >/dev/null 2>&1 && pwd )"
cd $DIR
source ../../../../setup.sh
GREEN="\e[1;32m"
YELLOW="\e[1;33m"
RED="\e[1;31m"
NC='\033[0m'
: "${CPPCHECK_DIR:=$(python3 -c "import cppcheck; print(cppcheck.DIR)")}"
# ensure checked in coverage table is up to date
python3 $CPPCHECK_DIR/addons/misra.py -generate-table > coverage_table
if ! git diff --quiet coverage_table; then
echo -e "${YELLOW}MISRA coverage table doesn't match. Update and commit:${NC}"
exit 3
fi
cd $BASEDIR
CHECKLIST=$(mktemp)
echo "Cppcheck checkers list from test_misra.sh:" > $CHECKLIST
cppcheck() {
# get all gcc defines: arm-none-eabi-gcc -dM -E - < /dev/null
# cppcheck does not provide __has_include_next, which is used by Clang's stdint.h
COMMON_DEFINES="-D__GNUC__=9 -D__has_include_next(x)=0"
# note that cppcheck build cache results in inconsistent results as of v2.13.0
OUTPUT=$(mktemp)
echo -e "\n\n\n\n\nTEST variant options:" >> $CHECKLIST
echo -e ""${@//$BASEDIR/}"\n\n" >> $CHECKLIST # (absolute path removed)
OPENDBC_ROOT=${OPENDBC_ROOT:-$BASEDIR}
$CPPCHECK_DIR/cppcheck --inline-suppr -I $OPENDBC_ROOT \
-I "$(cc -print-file-name=include)" \
--suppress=missingIncludeSystem \
--suppress=*:*include/* \
--suppressions-list=$DIR/suppressions.txt \
--error-exitcode=2 --check-level=exhaustive --safety \
--platform=arm32-wchar_t4 $COMMON_DEFINES --checkers-report=$CHECKLIST.tmp \
--std=c11 "$@" 2>&1 | tee $OUTPUT
cat $CHECKLIST.tmp >> $CHECKLIST
rm $CHECKLIST.tmp
# cppcheck bug: some MISRA errors won't result in the error exit code,
# so check the output (https://trac.cppcheck.net/ticket/12440#no1)
if grep -e "misra violation" -e "error" -e "style: " $OUTPUT > /dev/null; then
printf "${RED}** FAILED: MISRA violations found!${NC}\n"
exit 1
fi
}
OPTS=" --enable=all --enable=unusedFunction --addon=misra"
printf "\n${GREEN}** Safety **${NC}\n"
cppcheck $OPTS $BASEDIR/opendbc/safety/tests/misra/main.c
printf "\n${GREEN}Success!${NC} took $SECONDS seconds\n"
# ensure list of checkers is up to date
if [ -z "$OPENDBC_ROOT" ]; then
cd $DIR
if ! git diff --quiet $CHECKLIST; then
echo -e "\n${YELLOW}WARNING: Cppcheck checkers.txt report has changed. Review and commit...${NC}"
mv $CHECKLIST $DIR/checkers.txt
exit 4
fi
fi
@@ -0,0 +1,71 @@
#!/usr/bin/env python3
import os
import glob
import unittest
import shutil
import subprocess
import tempfile
import random
HERE = os.path.abspath(os.path.dirname(__file__))
ROOT = os.path.join(HERE, "../../../../")
IGNORED_PATHS = (
'opendbc/safety/main.c',
'opendbc/safety/tests/',
)
mutations = [
# no mutation, should pass
(None, None, lambda s: s, False),
]
patterns = [
("misra-c2012-10.3", lambda s: s + "\nvoid test(float len) { for (float j = 0; j < len; j++) {;} }\n"),
("misra-c2012-13.3", lambda s: s + "\nvoid test(int tmp) { int tmp2 = tmp++ + 2; if (tmp2) {;}}\n"),
("misra-c2012-13.4", lambda s: s + "\nint test(int x, int y) { return (x=2) && (y=2); }\n"),
("misra-c2012-13.5", lambda s: s + "\nvoid test(int tmp) { if (true && tmp++) {;} }\n"),
("misra-c2012-13.6", lambda s: s + "\nvoid test(int tmp) { if (sizeof(tmp++)) {;} }\n"),
("misra-c2012-14.2", lambda s: s + "\nvoid test(int cnt) { for (cnt=0;;cnt++) {;} }\n"),
("misra-c2012-14.4", lambda s: s + "\nvoid test(int len) { if (len - 8) {;} }\n"),
("misra-c2012-16.4", lambda s: s + "\nvoid test(int temp) {switch (temp) { case 1: ; }}\n"),
("misra-c2012-20.4", lambda s: s + "\n#define auto 1\n"),
("misra-c2012-20.5", lambda s: s + "\n#define TEST 1\n#undef TEST\n"),
]
all_files = glob.glob('opendbc/safety/**', root_dir=ROOT, recursive=True)
files = sorted(f for f in all_files if f.endswith(('.c', '.h')) and not f.startswith(IGNORED_PATHS))
assert len(files) > 20, files
# fixed seed so every xdist worker collects the same test params
rng = random.Random(len(files))
for p in patterns:
mutations.append((rng.choice(files), *p, True))
# sample to keep CI fast, but always include the no-mutation case
mutations = [mutations[0]] + rng.sample(mutations[1:], min(2, len(mutations) - 1))
class TestMisraMutation(unittest.TestCase):
def test_misra_mutation(self):
for fn, rule, transform, should_fail in mutations:
with self.subTest(fn=fn, rule=rule, should_fail=should_fail):
with tempfile.TemporaryDirectory() as tmp:
shutil.copytree(ROOT, tmp, dirs_exist_ok=True,
ignore=shutil.ignore_patterns('.venv', '.git', '*.ctu-info', '.hypothesis'))
# apply patch
if fn is not None:
with open(os.path.join(tmp, fn), 'r+') as f:
content = f.read()
f.seek(0)
f.write(transform(content))
# run test
r = subprocess.run(f"OPENDBC_ROOT={tmp} opendbc/safety/tests/misra/test_misra.sh",
stdout=subprocess.PIPE, cwd=ROOT, shell=True, encoding='utf8')
print(r.stdout) # helpful for debugging failures
failed = r.returncode != 0
assert failed == should_fail
if should_fail:
assert rule in r.stdout, "MISRA test failed but not for the correct violation"
+635
View File
@@ -0,0 +1,635 @@
#!/usr/bin/env python3
import argparse
import io
import os
import re
import subprocess
import sys
import tempfile
import time
import unittest
from concurrent.futures import ProcessPoolExecutor, as_completed
from collections import Counter, namedtuple
from dataclasses import dataclass
from pathlib import Path
import tree_sitter_c as ts_c
import tree_sitter as ts
ROOT = Path(__file__).resolve().parents[3]
SAFETY_DIR = ROOT / "opendbc" / "safety"
SAFETY_TESTS_DIR = ROOT / "opendbc" / "safety" / "tests"
SAFETY_C_REL = Path("opendbc/safety/tests/libsafety/safety.c")
ANSI_RESET = "\033[0m"
ANSI_BOLD = "\033[1m"
ANSI_RED = "\033[31m"
ANSI_GREEN = "\033[32m"
ANSI_YELLOW = "\033[33m"
COMPARISON_OPERATOR_MAP = {
"==": "!=",
"!=": "==",
">": "<=",
">=": "<",
"<": ">=",
"<=": ">",
}
MUTATOR_FAMILIES = {
"increment": ("update_expression", {"++": "--"}),
"decrement": ("update_expression", {"--": "++"}),
"comparison": ("binary_expression", COMPARISON_OPERATOR_MAP),
"boundary": ("number_literal", {}),
"bitwise_assignment": ("assignment_expression", {"&=": "|=", "|=": "&=", "^=": "&="}),
"bitwise": ("binary_expression", {"&": "|", "|": "&", "^": "&"}),
"arithmetic_assignment": ("assignment_expression", {"+=": "-=", "-=": "+=", "*=": "/=", "/=": "*=", "%=": "*="}),
"arithmetic": ("binary_expression", {"+": "-", "-": "+", "*": "/", "/": "*", "%": "*"}),
"remove_negation": ("unary_expression", {"!": ""}),
}
_RawSite = namedtuple('_RawSite', 'expr_start expr_end op_start op_end line original_op mutated_op mutator')
@dataclass(frozen=True)
class MutationSite:
site_id: int
expr_start: int
expr_end: int
op_start: int
op_end: int
line: int
original_op: str
mutated_op: str
mutator: str
origin_file: Path
origin_line: int
@dataclass(frozen=True)
class MutantResult:
site: MutationSite
outcome: str # killed | survived | infra_error
test_sec: float
details: str
def colorize(text, color):
term = os.environ.get("TERM", "")
if not sys.stdout.isatty() or term in ("", "dumb") or "NO_COLOR" in os.environ:
return text
return f"{color}{text}{ANSI_RESET}"
def format_mutation(original_op, mutated_op):
return colorize(f"{original_op}->{mutated_op}", ANSI_RED)
def _parse_int_literal(token):
m = re.fullmatch(r"([0-9][0-9a-fA-FxX]*)([uUlL]*)", token)
if m is None:
return None
body, suffix = m.groups()
try:
value = int(body, 0)
except ValueError:
return None
base = "hex" if body.lower().startswith("0x") else "dec"
return value, base, suffix
def _site_key(site):
return (site.op_start, site.op_end, site.mutator)
def _is_in_constexpr_context(node):
"""Check if a node is inside a static or file-scope variable initializer."""
current = node.parent
while current is not None:
if current.type == "init_declarator":
decl = current.parent
if decl and decl.type == "declaration":
for child in decl.children:
if child.type == "storage_class_specifier" and child.text == b"static":
return True
if decl.parent and decl.parent.type == "translation_unit":
return True
current = current.parent
return False
def _prepare_for_parsing(txt):
"""Blank line markers and replace __typeof__() for tree-sitter. Preserves byte offsets."""
result = re.sub(
r'^[ \t]*#[ \t]+\d+[ \t]+"[^\n]*',
lambda m: " " * len(m.group()),
txt,
flags=re.MULTILINE,
)
# Replace __typeof__(...) with padded int (handle nested parens)
parts = []
i = 0
for m in re.finditer(r"(?:__typeof__|typeof)\s*\(", result):
if m.start() < i:
continue # skip nested typeof inside already-replaced region
parts.append(result[i:m.start()])
depth = 1
j = m.end()
while j < len(result) and depth > 0:
if result[j] == "(":
depth += 1
elif result[j] == ")":
depth -= 1
j += 1
parts.append("int" + " " * (j - m.start() - 3))
i = j
parts.append(result[i:])
return "".join(parts)
def enumerate_sites(input_source, preprocessed_file):
subprocess.run([
"cc", "-E", "-std=gnu11", "-nostdlib", "-fno-builtin", "-DALLOW_DEBUG",
f"-I{ROOT}", f"-I{ROOT / 'opendbc/safety/board'}",
str(input_source), "-o", str(preprocessed_file),
], cwd=ROOT, capture_output=True, check=True)
txt = preprocessed_file.read_text()
# Build line map from preprocessor directives
line_map = {}
current_map_file = None
current_map_line = None
directive_re = re.compile(r'^\s*#\s*(\d+)\s+"([^"]+)"')
for pp_line_num, pp_line in enumerate(txt.splitlines(keepends=True), start=1):
m = directive_re.match(pp_line)
if m:
current_map_line = int(m.group(1))
current_map_file = Path(m.group(2)).resolve()
continue
if current_map_file is not None and current_map_line is not None:
line_map[pp_line_num] = (current_map_file, current_map_line)
current_map_line += 1
# Parse with tree-sitter
parser = ts.Parser(ts.Language(ts_c.language()))
tree = parser.parse(_prepare_for_parsing(txt).encode())
# Build rule map
rule_map = {}
counts = {}
for mutator, (node_kind, op_map) in MUTATOR_FAMILIES.items():
counts[mutator] = 0
if mutator == "boundary":
continue
for original_op, mutated_op in op_map.items():
rule_map.setdefault((node_kind, original_op), []).append((mutator, original_op, mutated_op))
# Walk tree to find mutation sites
deduped = {}
build_incompatible_keys = set()
stack = [tree.root_node]
while stack:
node = stack.pop()
kind = node.type
# Boundary mutations: find number_literals inside comparison operands
if kind == "binary_expression":
cmp_op = node.child_by_field_name("operator")
if cmp_op and cmp_op.type in COMPARISON_OPERATOR_MAP:
lit_stack = []
for field in ("left", "right"):
operand = node.child_by_field_name(field)
if operand:
lit_stack.append(operand)
while lit_stack:
n = lit_stack.pop()
if n.type == "number_literal":
token = txt[n.start_byte:n.end_byte]
parsed = _parse_int_literal(token)
if parsed:
value, base, suffix = parsed
mutated = f"0x{value + 1:X}{suffix}" if base == "hex" else f"{value + 1}{suffix}"
line = n.start_point[0] + 1
bsite = _RawSite(n.start_byte, n.end_byte, n.start_byte, n.end_byte, line, token, mutated, "boundary")
key = _site_key(bsite)
deduped[key] = bsite
if _is_in_constexpr_context(n):
build_incompatible_keys.add(key)
lit_stack.extend(n.children)
# Operator mutations: any node with an operator child
op_child = node.child_by_field_name("operator")
if op_child:
for mutator, original_op, mutated_op in rule_map.get((kind, op_child.type), []):
line = node.start_point[0] + 1
site = _RawSite(node.start_byte, node.end_byte, op_child.start_byte, op_child.end_byte, line, original_op, mutated_op, mutator)
key = _site_key(site)
deduped[key] = site
if _is_in_constexpr_context(node):
build_incompatible_keys.add(key)
stack.extend(node.children)
sites = sorted(deduped.values(), key=lambda s: (s.op_start, s.mutator))
out = []
build_incompatible_site_ids = set()
for s in sites:
mapped = line_map.get(s.line)
if mapped is None:
continue
origin_file, origin_line = mapped
if SAFETY_DIR not in origin_file.parents and origin_file != SAFETY_DIR:
continue
site_id = len(out)
site = MutationSite(
site_id=site_id, expr_start=s.expr_start, expr_end=s.expr_end,
op_start=s.op_start, op_end=s.op_end, line=s.line,
original_op=s.original_op, mutated_op=s.mutated_op, mutator=s.mutator,
origin_file=origin_file, origin_line=origin_line,
)
if _site_key(s) in build_incompatible_keys:
build_incompatible_site_ids.add(site_id)
out.append(site)
counts[s.mutator] += 1
return out, counts, build_incompatible_site_ids, txt
def _build_core_tests(catalog):
"""Build test ordering for core (non-mode) files.
One test per unique method name from evenly-spaced modules,
ordered by how widely each method is shared. Methods inherited by many
classes exercise the most fundamental safety logic and run first.
"""
MAX_PER_METHOD = 5
method_freq = {}
method_by_module = {}
for name in sorted(catalog.keys()):
for test_id in catalog[name]:
method = test_id.rsplit(".", 1)[-1]
method_freq[method] = method_freq.get(method, 0) + 1
if method not in method_by_module:
method_by_module[method] = {}
if name not in method_by_module[method]:
method_by_module[method][name] = test_id
# Pick evenly-spaced modules for each method to maximize configuration diversity
method_ids = {}
for method, module_map in method_by_module.items():
modules = sorted(module_map.keys())
n = len(modules)
if n <= MAX_PER_METHOD:
method_ids[method] = [module_map[m] for m in modules]
else:
step = n / MAX_PER_METHOD
method_ids[method] = [module_map[modules[int(i * step)]] for i in range(MAX_PER_METHOD)]
# Round-robin: first instance of each method (by freq), then second, etc.
# This ensures diverse early coverage with failfast.
sorted_methods = sorted(method_freq, key=lambda m: -method_freq[m])
ordered = []
for round_idx in range(MAX_PER_METHOD):
for m in sorted_methods:
ids = method_ids.get(m, [])
if round_idx < len(ids):
ordered.append(ids[round_idx])
return ordered
def build_priority_tests(site, catalog, core_tests):
"""Build an ordered list of test IDs for a mutation site.
For mode files: all tests from the matching test_<mode>.py module.
For core files: uses the pre-computed core_tests ordering.
"""
src = site.origin_file
rel_parts = src.relative_to(ROOT).parts
is_mode = len(rel_parts) >= 4 and rel_parts[:3] == ("opendbc", "safety", "modes")
if is_mode:
mode_file = f"test_{src.stem}.py"
return list(catalog.get(mode_file, []))
return core_tests
def format_site_snippet(site, context_lines=2):
source = site.origin_file
text = source.read_text()
lines = text.splitlines()
display_ln = site.origin_line
line_idx = display_ln - 1
start = max(0, line_idx - context_lines)
end = min(len(lines), line_idx + context_lines + 1)
line_text = lines[line_idx]
rel_start = line_text.find(site.original_op)
if rel_start < 0:
rel_start = 0
rel_end = rel_start + len(site.original_op)
snippet_lines = []
width = len(str(end))
for idx in range(start, end):
num = idx + 1
prefix = ">" if idx == line_idx else " "
line = lines[idx]
if idx == line_idx:
marker = colorize(f"[[{site.original_op}->{site.mutated_op}]]", ANSI_RED)
line = f"{line[:rel_start]}{marker}{line[rel_end:]}"
snippet_lines.append(f"{prefix} {num:>{width}} | {line}")
return "\n".join(snippet_lines)
def render_progress(completed, total, killed, survived, infra, elapsed_sec):
bar_width = 30
filled = int((completed / total) * bar_width)
bar = "#" * filled + "-" * (bar_width - filled)
rate = completed / elapsed_sec if elapsed_sec > 0 else 0.0
remaining = total - completed
eta = (remaining / rate) if rate > 0 else 0.0
killed_text = colorize(f"k:{killed}", ANSI_GREEN)
survived_text = colorize(f"s:{survived}", ANSI_RED)
infra_text = colorize(f"i:{infra}", ANSI_YELLOW)
return f"[{bar}] {completed}/{total} {killed_text} {survived_text} {infra_text} mps:{rate:.2f} elapsed:{elapsed_sec:.1f}s eta:{eta:.1f}s"
def print_live_status(text, *, final=False):
if sys.stdout.isatty():
print("\r" + text, end="\n" if final else "", flush=True)
else:
print(text, flush=True)
def _discover_test_catalog():
loader = unittest.TestLoader()
catalog = {}
for test_file in sorted(SAFETY_TESTS_DIR.glob("test_*.py")):
module_name = ".".join(test_file.relative_to(ROOT).with_suffix("").parts)
suite = loader.loadTestsFromName(module_name)
catalog[test_file.name] = [t.id() for group in suite for t in group]
return catalog
def run_unittest(targets, lib_path, mutant_id, verbose):
from opendbc.safety.tests.libsafety import libsafety_py
libsafety_py.load(lib_path)
libsafety_py.libsafety.mutation_set_active_mutant(mutant_id)
if verbose:
print("Running unittest targets:", ", ".join(targets), flush=True)
loader = unittest.TestLoader()
stream = io.StringIO()
runner = unittest.TextTestRunner(stream=stream, verbosity=0, failfast=True)
suite = unittest.TestSuite()
for target in targets:
suite.addTests(loader.loadTestsFromName(target))
result = runner.run(suite)
if result.failures:
return result.failures[0][0].id()
if result.errors:
return result.errors[0][0].id()
return None
def _instrument_source(source, sites):
# Sort by start ascending, end descending (outermost first when same start)
sorted_sites = sorted(sites, key=lambda s: (s.expr_start, -s.expr_end))
# Build containment forest using a stack
roots = []
stack = []
for site in sorted_sites:
while stack and stack[-1][0].expr_end <= site.expr_start:
stack.pop()
node = [site, []]
if stack:
stack[-1][1].append(node)
else:
roots.append(node)
stack.append(node)
def build_replacement(site, children):
parts = []
pos = site.expr_start
op_rel = None
running_len = 0
for child_site, child_children in children:
seg = source[pos : child_site.expr_start]
if op_rel is None and site.op_start >= pos and site.op_start < child_site.expr_start:
op_rel = running_len + (site.op_start - pos)
parts.append(seg)
running_len += len(seg)
child_repl = build_replacement(child_site, child_children)
parts.append(child_repl)
running_len += len(child_repl)
pos = child_site.expr_end
seg = source[pos : site.expr_end]
if op_rel is None and site.op_start >= pos:
op_rel = running_len + (site.op_start - pos)
parts.append(seg)
expr_text = "".join(parts)
op_len = site.op_end - site.op_start
assert op_rel is not None and expr_text[op_rel : op_rel + op_len] == site.original_op, (
f"Operator mismatch (site_id={site.site_id}): expected {site.original_op!r} at offset {op_rel}"
)
mutated_expr = f"{expr_text[:op_rel]}{site.mutated_op}{expr_text[op_rel + op_len :]}"
return f"((__mutation_active_id == {site.site_id}) ? ({mutated_expr}) : ({expr_text}))"
result_parts = []
pos = 0
for site, children in roots:
result_parts.append(source[pos : site.expr_start])
result_parts.append(build_replacement(site, children))
pos = site.expr_end
result_parts.append(source[pos:])
return "".join(result_parts)
def compile_mutated_library(preprocessed_source, sites, output_so):
instrumented = _instrument_source(preprocessed_source, sites)
prelude = """
static int __mutation_active_id = -1;
void mutation_set_active_mutant(int id) { __mutation_active_id = id; }
int mutation_get_active_mutant(void) { return __mutation_active_id; }
"""
marker_re = re.compile(r'^\s*#\s+\d+\s+"[^\n]*\n?', re.MULTILINE)
instrumented = prelude + marker_re.sub("", instrumented)
mutation_source = output_so.with_suffix(".c")
mutation_source.write_text(instrumented)
subprocess.run([
"cc", "-shared", "-fPIC", "-w", "-fno-builtin", "-std=gnu11",
"-g0", "-O0", "-DALLOW_DEBUG",
str(mutation_source), "-o", str(output_so),
], cwd=ROOT, check=True)
def eval_mutant(site, targets, lib_path, verbose):
try:
t0 = time.perf_counter()
failed_test = run_unittest(targets, lib_path, mutant_id=site.site_id, verbose=verbose)
duration = time.perf_counter() - t0
if failed_test is not None:
return MutantResult(site, "killed", duration, "")
return MutantResult(site, "survived", duration, "")
except Exception as exc:
return MutantResult(site, "infra_error", 0.0, str(exc))
def main():
parser = argparse.ArgumentParser(description="Run strict safety mutation")
parser.add_argument("-j", type=int, default=max((os.cpu_count() or 1) - 1, 1), help="parallel mutants to run")
parser.add_argument("--max-mutants", type=int, default=0, help="optional limit for debugging (0 means all)")
parser.add_argument("--list-only", action="store_true", help="list discovered candidates and exit")
parser.add_argument("--verbose", action="store_true", help="print extra debug output")
args = parser.parse_args()
start = time.perf_counter()
with tempfile.TemporaryDirectory(prefix="mutation-op-run-") as run_tmp_dir:
preprocessed_file = Path(run_tmp_dir) / "safety_preprocessed.c"
sites, mutator_counts, build_incompatible_ids, preprocessed_source = enumerate_sites(ROOT / SAFETY_C_REL, preprocessed_file)
assert len(sites) > 0
if args.max_mutants > 0:
sites = sites[: args.max_mutants]
mutator_summary = ", ".join(f"{name} ({c})" for name in MUTATOR_FAMILIES if (c := mutator_counts.get(name, 0)) > 0)
print(f"Found {len(sites)} unique candidates: {mutator_summary}", flush=True)
if args.list_only:
for site in sites:
mutation = format_mutation(site.original_op, site.mutated_op)
print(f" #{site.site_id:03d} {site.origin_file.relative_to(ROOT)}:{site.origin_line} [{site.mutator}] {mutation}")
return 0
print(f"Running {len(sites)} mutants with {args.j} workers", flush=True)
discovered_count = len(sites)
selected_site_ids = {s.site_id for s in sites}
build_incompatible_ids &= selected_site_ids
pruned_compile_sites = len(build_incompatible_ids)
if pruned_compile_sites > 0:
sites = [s for s in sites if s.site_id not in build_incompatible_ids]
print(f"Pruned {pruned_compile_sites} build-incompatible mutants from constant-expression initializers", flush=True)
if not sites:
print("Failed to build mutation library: all sites were pruned as build-incompatible", flush=True)
return 2
mutation_lib = Path(run_tmp_dir) / "libsafety_mutation.so"
compile_mutated_library(preprocessed_source, sites, mutation_lib)
# Discover all tests by importing modules in the main process.
# Forked workers inherit these imports, eliminating per-worker import cost.
catalog = _discover_test_catalog()
# Baseline smoke check
baseline_ids = catalog.get("test_defaults.py", [])[:5]
baseline_failed = run_unittest(baseline_ids, mutation_lib, mutant_id=-1, verbose=args.verbose)
if baseline_failed is not None:
print("Baseline smoke failed with mutant_id=-1; aborting to avoid false kill signals.", flush=True)
print(f" failed_test: {baseline_failed}", flush=True)
return 2
# Pre-compute test targets per mutation site
core_tests = _build_core_tests(catalog)
site_targets = {site.site_id: build_priority_tests(site, catalog, core_tests) for site in sites}
results = []
counts = Counter()
with ProcessPoolExecutor(max_workers=args.j) as pool:
future_map = {
pool.submit(eval_mutant, site, site_targets[site.site_id], mutation_lib, args.verbose): site for site in sites
}
print_live_status(render_progress(0, len(sites), 0, 0, 0, 0.0))
try:
for fut in as_completed(future_map):
try:
res = fut.result()
except Exception:
site = future_map[fut]
res = MutantResult(site, "killed", 0.0, "worker process crashed")
results.append(res)
counts[res.outcome] += 1
elapsed_now = time.perf_counter() - start
done = len(results) == len(sites)
print_live_status(render_progress(len(results), len(sites), counts["killed"], counts["survived"],
counts["infra_error"], elapsed_now), final=done)
except Exception:
# Pool broken — mark all unfinished mutants as killed (crash = behavioral change detected)
completed_ids = {r.site.site_id for r in results}
for site in sites:
if site.site_id not in completed_ids:
results.append(MutantResult(site, "killed", 0.0, "pool broken"))
counts["killed"] += 1
elapsed_now = time.perf_counter() - start
print_live_status(render_progress(len(results), len(sites), counts["killed"], counts["survived"], counts["infra_error"], elapsed_now), final=True)
survivors = sorted((r for r in results if r.outcome == "survived"), key=lambda r: r.site.site_id)
if survivors:
print("", flush=True)
print(colorize("Surviving mutants", ANSI_RED), flush=True)
for res in survivors:
loc = f"{res.site.origin_file.relative_to(ROOT)}:{res.site.origin_line}"
mutation = format_mutation(res.site.original_op, res.site.mutated_op)
print(f"- #{res.site.site_id} {loc} [{res.site.mutator}] {mutation}", flush=True)
print(format_site_snippet(res.site), flush=True)
infra_results = sorted((r for r in results if r.outcome == "infra_error"), key=lambda r: r.site.site_id)
if infra_results:
print("", flush=True)
print(colorize("Infra errors", ANSI_YELLOW), flush=True)
for res in infra_results:
loc = f"{res.site.origin_file.relative_to(ROOT)}:{res.site.origin_line}"
detail = res.details.splitlines()[0] if res.details else "unknown error"
print(f"- #{res.site.site_id} {loc}: {detail}", flush=True)
elapsed = time.perf_counter() - start
total_test_sec = sum(r.test_sec for r in results)
print("", flush=True)
print(colorize("Mutation summary", ANSI_BOLD), flush=True)
print(f" discovered: {discovered_count}", flush=True)
print(f" pruned_build_incompatible: {pruned_compile_sites}", flush=True)
print(f" total: {len(sites)}", flush=True)
print(f" killed: {colorize(str(counts['killed']), ANSI_GREEN)}", flush=True)
print(f" survived: {colorize(str(counts['survived']), ANSI_RED)}", flush=True)
print(f" infra_error: {colorize(str(counts['infra_error']), ANSI_YELLOW)}", flush=True)
print(f" test_time_sum: {total_test_sec:.2f}s", flush=True)
print(f" avg_test_per_mutant: {total_test_sec / len(results):.3f}s", flush=True)
print(f" mutants_per_second: {len(sites) / elapsed:.2f}", flush=True)
print(f" elapsed: {elapsed:.2f}s", flush=True)
if counts["infra_error"] > 0:
return 2
# TODO: fix these surviving mutants and delete this block
known_survivors = {
("opendbc/safety/helpers.h", 40, "arithmetic"),
("opendbc/safety/lateral.h", 106, "boundary"),
("opendbc/safety/lateral.h", 189, "boundary"),
("opendbc/safety/lateral.h", 213, "boundary"),
("opendbc/safety/lateral.h", 214, "boundary"),
("opendbc/safety/lateral.h", 364, "arithmetic"),
("opendbc/safety/longitudinal.h", 38, "remove_negation"),
}
survivors = [r for r in survivors if (str(r.site.origin_file.relative_to(ROOT)), r.site.origin_line, r.site.mutator) not in known_survivors]
if survivors:
return 1
return 0
if __name__ == "__main__":
raise SystemExit(main())
Whitespace-only changes.
@@ -0,0 +1,113 @@
from opendbc.car.ford.values import FordSafetyFlags
from opendbc.car.hyundai.values import HyundaiSafetyFlags
from opendbc.car.toyota.values import ToyotaSafetyFlags
from opendbc.car.structs import CarParams
from opendbc.safety.tests.libsafety import libsafety_py
def to_signed(d, bits):
ret = d
if d >= (1 << (bits - 1)):
ret = d - (1 << bits)
return ret
def is_steering_msg(mode, param, addr):
ret = False
if mode in (CarParams.SafetyModel.hondaNidec, CarParams.SafetyModel.hondaBosch):
ret = (addr == 0xE4) or (addr == 0x194) or (addr == 0x33D) or (addr == 0x33DA) or (addr == 0x33DB)
elif mode == CarParams.SafetyModel.toyota:
ret = addr == (0x191 if param & ToyotaSafetyFlags.LTA else 0x2E4)
elif mode == CarParams.SafetyModel.gm:
ret = addr == 384
elif mode in (CarParams.SafetyModel.hyundai, CarParams.SafetyModel.hyundaiLegacy):
ret = addr == 832
elif mode == CarParams.SafetyModel.hyundaiCanfd:
ret = addr == (0x110 if param & HyundaiSafetyFlags.CANFD_LKA_STEER_MSG_ALT else
0x50 if param & HyundaiSafetyFlags.CANFD_LKA_STEER_MSG else
0x12A)
elif mode == CarParams.SafetyModel.chrysler:
ret = addr == 0x292
elif mode == CarParams.SafetyModel.subaru:
ret = addr == 0x122
elif mode == CarParams.SafetyModel.ford:
ret = addr == 0x3d6 if param & FordSafetyFlags.CANFD else addr == 0x3d3
elif mode == CarParams.SafetyModel.nissan:
ret = addr == 0x169
elif mode == CarParams.SafetyModel.rivian:
ret = addr == 0x120
elif mode == CarParams.SafetyModel.tesla:
ret = addr == 0x488
return ret
def get_steer_value(mode, param, msg):
# TODO: use CANParser
torque, angle = 0, 0
if mode in (CarParams.SafetyModel.hondaNidec, CarParams.SafetyModel.hondaBosch):
torque = (msg.data[0] << 8) | msg.data[1]
torque = to_signed(torque, 16)
elif mode == CarParams.SafetyModel.toyota:
if param & ToyotaSafetyFlags.LTA:
angle = (msg.data[1] << 8) | msg.data[2]
angle = to_signed(angle, 16)
else:
torque = (msg.data[1] << 8) | (msg.data[2])
torque = to_signed(torque, 16)
elif mode == CarParams.SafetyModel.gm:
torque = ((msg.data[0] & 0x7) << 8) | msg.data[1]
torque = to_signed(torque, 11)
elif mode in (CarParams.SafetyModel.hyundai, CarParams.SafetyModel.hyundaiLegacy):
torque = (((msg.data[3] & 0x7) << 8) | msg.data[2]) - 1024
elif mode == CarParams.SafetyModel.hyundaiCanfd:
torque = ((msg.data[5] >> 1) | (msg.data[6] & 0xF) << 7) - 1024
elif mode == CarParams.SafetyModel.chrysler:
torque = (((msg.data[0] & 0x7) << 8) | msg.data[1]) - 1024
elif mode == CarParams.SafetyModel.subaru:
torque = ((msg.data[3] & 0x1F) << 8) | msg.data[2]
torque = -to_signed(torque, 13)
elif mode == CarParams.SafetyModel.ford:
if param & FordSafetyFlags.CANFD:
angle = ((msg.data[2] << 3) | (msg.data[3] >> 5)) - 1000
else:
angle = ((msg.data[0] << 3) | (msg.data[1] >> 5)) - 1000
elif mode == CarParams.SafetyModel.nissan:
angle = (msg.data[0] << 10) | (msg.data[1] << 2) | (msg.data[2] >> 6)
angle = -angle + (1310 * 100)
elif mode == CarParams.SafetyModel.rivian:
torque = ((msg.data[2] << 3) | (msg.data[3] >> 5)) - 1024
elif mode == CarParams.SafetyModel.tesla:
angle = (((msg.data[0] & 0x7F) << 8) | (msg.data[1])) - 16384 # ceil(1638.35/0.1)
return torque, angle
def package_can_msg(msg):
return libsafety_py.make_CANPacket(msg.address, msg.src % 4, msg.dat)
def init_segment(safety, msgs, mode, param):
sendcan = (msg for msg in msgs if msg.which() == 'sendcan')
steering_msgs = (can for msg in sendcan for can in msg.sendcan if is_steering_msg(mode, param, can.address))
msg = next(steering_msgs, None)
if msg is None:
print("no steering msgs found!")
return
msg = package_can_msg(msg)
torque, angle = get_steer_value(mode, param, msg)
if torque != 0:
safety.set_controls_allowed(1)
safety.set_controls_allowed_lateral(1)
safety.set_desired_torque_last(torque)
safety.set_rt_torque_last(torque)
safety.set_torque_meas(torque, torque)
safety.set_torque_driver(torque, torque)
elif angle != 0:
safety.set_controls_allowed(1)
safety.set_controls_allowed_lateral(1)
safety.set_desired_angle_last(angle)
safety.set_angle_meas(angle, angle)
safety.set_desired_curvature_last(angle)
safety.set_curvature_meas(angle, angle)
assert safety.safety_tx_hook(msg), "failed to initialize safety for segment"
@@ -0,0 +1,175 @@
#!/usr/bin/env python3
import argparse
import os
from collections import Counter, defaultdict
from tqdm import tqdm
from opendbc.safety import ALTERNATIVE_EXPERIENCE
from opendbc.safety.tests.libsafety import libsafety_py
from opendbc.car.carlog import carlog
from opendbc.safety.tests.safety_replay.helpers import package_can_msg, init_segment
# Define debug variables and their getter methods
DEBUG_VARS = {
'controls_allowed': lambda safety: safety.get_controls_allowed(),
'controls_allowed_lateral': lambda safety: safety.get_controls_allowed_lateral(),
'longitudinal_tx_allowed': lambda safety: safety.get_longitudinal_allowed(),
'controls_requested_lateral': lambda safety: safety.get_controls_requested_lateral(),
'current_disengage_reason': lambda safety: safety.mads_get_current_disengage_reason(),
'stock_acc_main': lambda safety: safety.get_acc_main_on(),
'mads_acc_main': lambda safety: safety.get_mads_acc_main(),
}
# replay a drive to check for safety violations
def replay_drive(msgs, safety_mode, param, alternative_experience, param_sp):
safety = libsafety_py.libsafety
msgs.sort(key=lambda m: m.logMonoTime)
safety.set_current_safety_param_sp(param_sp)
err = safety.set_safety_hooks(safety_mode, param)
assert err == 0, "invalid safety mode: %d" % safety_mode
safety.set_alternative_experience(alternative_experience)
_enable_mads = bool(alternative_experience & ALTERNATIVE_EXPERIENCE.ENABLE_MADS)
_disengage_lateral_on_brake = bool(alternative_experience & ALTERNATIVE_EXPERIENCE.MADS_DISENGAGE_LATERAL_ON_BRAKE)
_pause_lateral_on_brake = bool(alternative_experience & ALTERNATIVE_EXPERIENCE.MADS_PAUSE_LATERAL_ON_BRAKE)
safety.set_mads_params(_enable_mads, _disengage_lateral_on_brake, _pause_lateral_on_brake)
print("alternative experience:")
print(f" enable mads: {_enable_mads}")
print(f" disengage lateral on brake: {_disengage_lateral_on_brake}")
print(f" pause lateral on brake: {_pause_lateral_on_brake}")
init_segment(safety, msgs, safety_mode, param)
rx_tot, rx_invalid, tx_tot, tx_blocked, tx_controls, tx_controls_lateral, tx_controls_blocked, tx_controls_lateral_blocked = 0, 0, 0, 0, 0, 0, 0, 0
mads_mismatch = 0
safety_tick_rx_invalid = False
blocked_addrs = Counter()
invalid_addrs = set()
# Track last good state for each address
last_good_states = defaultdict(lambda: {
'timestamp': None,
**{var: None for var in DEBUG_VARS}
})
can_msgs = [m for m in msgs if m.which() in ('can', 'sendcan')]
start_t = can_msgs[0].logMonoTime
end_t = can_msgs[-1].logMonoTime
for msg in tqdm(can_msgs):
safety.set_timer((msg.logMonoTime // 1000) % 0xFFFFFFFF)
# skip start and end of route, warm up/down period
if msg.logMonoTime - start_t > 1e9 and end_t - msg.logMonoTime > 1e9:
safety.safety_tick_current_safety_config()
safety_tick_rx_invalid |= not safety.safety_config_valid() or safety_tick_rx_invalid
if msg.which() == 'sendcan':
for canmsg in msg.sendcan:
_msg = package_can_msg(canmsg)
sent = safety.safety_tx_hook(_msg)
# mismatch: with MADS enabled, stock controls_allowed went true but MADS didn't follow on lateral
if safety.get_enable_mads() and safety.get_controls_allowed() and not safety.get_controls_allowed_lateral():
mads_mismatch += 1
print(f"controls_allowed but not controls_allowed_lateral [{mads_mismatch}]")
print(f"msg:{canmsg.address} ({hex(canmsg.address)})")
for var, getter in DEBUG_VARS.items():
print(f" {var}: {getter(safety)}")
if not sent:
tx_blocked += 1
tx_controls_blocked += safety.get_controls_allowed()
tx_controls_lateral_blocked += safety.get_controls_allowed_lateral()
blocked_addrs[canmsg.address] += 1
carlog.debug("blocked bus %d msg %d at %f" % (canmsg.src, canmsg.address, (msg.logMonoTime - start_t) / 1e9))
if "DEBUG" in os.environ:
last_good = last_good_states[canmsg.address]
print(f"\nBlocked message at {(msg.logMonoTime - start_t) / 1e9:.3f}s:")
print(f"Address: {hex(canmsg.address)} (bus {canmsg.src})")
print("Current state:")
for var, getter in DEBUG_VARS.items():
print(f" {var}: {getter(safety)}")
if last_good['timestamp'] is not None:
print(f"\nLast good state ({last_good['timestamp']:.3f}s):")
for var in DEBUG_VARS:
print(f" {var}: {last_good[var]}")
else:
print("\nNo previous good state found for this address")
print("-" * 80)
else: # Update last good state if message is allowed
last_good_states[canmsg.address].update({
'timestamp': (msg.logMonoTime - start_t) / 1e9,
**{var: getter(safety) for var, getter in DEBUG_VARS.items()}
})
tx_controls += safety.get_controls_allowed()
tx_controls_lateral += safety.get_controls_allowed_lateral()
tx_tot += 1
elif msg.which() == 'can':
# ignore msgs we sent
for canmsg in filter(lambda m: m.src < 128, msg.can):
safety.safety_fwd_hook(canmsg.src, canmsg.address)
_msg = package_can_msg(canmsg)
recv = safety.safety_rx_hook(_msg)
if not recv:
rx_invalid += 1
invalid_addrs.add(canmsg.address)
rx_tot += 1
print("\nRX")
print("total rx msgs:", rx_tot)
print("invalid rx msgs:", rx_invalid)
print("safety tick rx invalid:", safety_tick_rx_invalid)
print("invalid addrs:", invalid_addrs)
print("\nTX")
print("total openpilot msgs:", tx_tot)
print("total msgs with controls_allowed:", tx_controls)
print("total msgs with controls_allowed_lateral:", tx_controls_lateral)
print("blocked msgs:", tx_blocked)
print("blocked with controls_allowed:", tx_controls_blocked)
print("blocked with controls_allowed_lateral:", tx_controls_lateral_blocked)
print("blocked addrs:", blocked_addrs)
print("\nMADS")
print("mads enabled:", safety.get_enable_mads())
print("controls_allowed_lateral (mads):", safety.get_controls_allowed_lateral())
print("longitudinal_tx_allowed (controls_allowed && !gas_pressed):", safety.get_longitudinal_allowed())
print("mads mismatch (mads_enabled && controls_allowed && !controls_allowed_lateral):", mads_mismatch)
return tx_controls_blocked == 0 and tx_controls_lateral_blocked == 0 and rx_invalid == 0 and \
not safety_tick_rx_invalid and mads_mismatch == 0
if __name__ == "__main__":
from openpilot.tools.lib.logreader import LogReader
parser = argparse.ArgumentParser(description="Replay CAN messages from a route or segment through a safety mode",
formatter_class=argparse.ArgumentDefaultsHelpFormatter)
parser.add_argument("route_or_segment_name", nargs='+')
parser.add_argument("--mode", type=int, help="Override the safety mode from the log")
parser.add_argument("--param", type=int, help="Override the safety param from the log")
parser.add_argument("--alternative-experience", type=int, help="Override the alternative experience from the log")
parser.add_argument("--param-sp", type=int, help="Override the sunnypilot safety param from the log")
args = parser.parse_args()
lr = LogReader(args.route_or_segment_name[0])
if None in (args.mode, args.param, args.alternative_experience, args.param_sp):
CP = lr.first('carParams')
CP_SP = lr.first('carParamsSP')
if args.mode is None:
args.mode = CP.safetyConfigs[-1].safetyModel.raw
if args.param is None:
args.param = CP.safetyConfigs[-1].safetyParam
if args.alternative_experience is None:
args.alternative_experience = CP.alternativeExperience
if args.param_sp is None:
_param_sp = CP_SP.safetyParam if hasattr(CP_SP, 'safetyParam') else 0
args.param_sp = _param_sp
print(f"replaying {args.route_or_segment_name[0]} with safety mode {args.mode}, param {args.param}, alternative experience {args.alternative_experience}, " +
f"param_sp {args.param_sp}")
replay_drive(list(lr), args.mode, args.param, args.alternative_experience, args.param_sp)
+38
View File
@@ -0,0 +1,38 @@
#!/usr/bin/env bash
set -e
DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" >/dev/null && pwd)"
cd $DIR
source ../../../setup.sh
# reset coverage data
rm -f ./libsafety/*.gcda
# run safety tests and generate coverage data
python -m unittest discover -s .
# NOTE: we accept that these tools will have slight differences,
# and in return, we get to use the stock toolchain instead of
# installing LLVM on all users' machines
if [ "$(uname)" = "Darwin" ]; then
GCOV_EXEC="llvm-cov gcov"
else
GCOV_EXEC="gcov"
fi
# generate and open report
if [ "$1" == "--report" ]; then
mkdir -p coverage-out
gcovr -r ../ --gcov-executable "$GCOV_EXEC" --html-nested coverage-out/index.html
sensible-browser coverage-out/index.html
fi
# test coverage
GCOV="gcovr -r $DIR/../ --gcov-executable \"$GCOV_EXEC\" -d --fail-under-line=100 -e ^libsafety"
if ! GCOV_OUTPUT="$(eval $GCOV)"; then
echo -e "FAILED:\n$GCOV_OUTPUT"
exit 1
else
echo "SUCCESS: All checked files have 100% coverage!"
fi
+60
View File
@@ -0,0 +1,60 @@
#!/usr/bin/env python3
import unittest
from opendbc.car.structs import CarParams
import opendbc.safety.tests.common as common
from opendbc.safety.tests.libsafety import libsafety_py
from opendbc.safety.tests.common import CANPackerSafety
class TestBody(common.SafetyTest):
TX_MSGS = [[0x250, 0], [0x251, 0],
[0x1, 0], [0x1, 1], [0x1, 2], [0x1, 3]]
FWD_BUS_LOOKUP = {}
def setUp(self):
self.packer = CANPackerSafety("comma_body")
self.safety = libsafety_py.libsafety
self.safety.set_safety_hooks(CarParams.SafetyModel.body, 0)
self.safety.init_tests()
def _motors_data_msg(self, speed_l, speed_r):
values = {"SPEED_L": speed_l, "SPEED_R": speed_r}
return self.packer.make_can_msg_safety("MOTORS_DATA", 0, values)
def _torque_cmd_msg(self, torque_l, torque_r):
values = {"TORQUE_L": torque_l, "TORQUE_R": torque_r}
return self.packer.make_can_msg_safety("TORQUE_CMD", 0, values)
def _max_motor_rpm_cmd_msg(self, max_rpm_l, max_rpm_r):
values = {"MAX_RPM_L": max_rpm_l, "MAX_RPM_R": max_rpm_r}
return self.packer.make_can_msg_safety("MAX_MOTOR_RPM_CMD", 0, values)
def test_rx_hook(self):
self.assertFalse(self.safety.get_controls_allowed())
# controls allowed when we get MOTORS_DATA message
self.assertTrue(self._rx(self._torque_cmd_msg(0, 0)))
self.assertFalse(self.safety.get_controls_allowed())
self.assertTrue(self._rx(self._motors_data_msg(0, 0)))
self.assertTrue(self.safety.get_controls_allowed())
def test_tx_hook(self):
self.assertFalse(self._tx(self._torque_cmd_msg(0, 0)))
self.safety.set_controls_allowed(True)
self.assertTrue(self._tx(self._torque_cmd_msg(0, 0)))
def test_can_flasher(self):
# CAN flasher always allowed
self.safety.set_controls_allowed(False)
self.assertTrue(self._tx(common.make_msg(0, 0x1, 8)))
# 0xdeadfaceU allowed for CAN flashing mode
self.assertTrue(self._tx(common.make_msg(0, 0x250, dat=b'\xce\xfa\xad\xde\x1e\x0b\xb0\x0a')))
self.assertFalse(self._tx(common.make_msg(0, 0x250, dat=b'\xce\xfa\xad\xde\x1e\x0b\xb0'))) # not correct data/len
self.assertFalse(self._tx(common.make_msg(0, 0x251, dat=b'\xce\xfa\xad\xde\x1e\x0b\xb0\x0a'))) # wrong address
if __name__ == "__main__":
unittest.main()
+159
View File
@@ -0,0 +1,159 @@
#!/usr/bin/env python3
import unittest
from opendbc.car.chrysler.values import ChryslerSafetyFlags
from opendbc.car.structs import CarParams
from opendbc.safety.tests.libsafety import libsafety_py
import opendbc.safety.tests.common as common
from opendbc.safety.tests.common import CANPackerSafety
class TestChryslerSafety(common.CarSafetyTest, common.MotorTorqueSteeringSafetyTest):
TX_MSGS = [[0x23B, 0], [0x292, 0], [0x2A6, 0], [0x2D9, 0]]
RELAY_MALFUNCTION_ADDRS = {0: (0x292, 0x2A6, 0x2D9)}
FWD_BLACKLISTED_ADDRS = {2: [0x292, 0x2A6, 0x2D9]}
MAX_RATE_UP = 3
MAX_RATE_DOWN = 3
MAX_TORQUE_LOOKUP = [0], [261]
MAX_RT_DELTA = 112
MAX_TORQUE_ERROR = 80
LKAS_ACTIVE_VALUE = 1
DAS_BUS = 0
def setUp(self):
self.packer = CANPackerSafety("chrysler_pacifica_2017_hybrid_generated")
self.safety = libsafety_py.libsafety
self.safety.set_safety_hooks(CarParams.SafetyModel.chrysler, 0)
self.safety.init_tests()
def _button_msg(self, cancel=False, resume=False, accel=False, decel=False):
values = {"ACC_Cancel": cancel, "ACC_Resume": resume, "ACC_Accel": accel, "ACC_Decel": decel}
return self.packer.make_can_msg_safety("CRUISE_BUTTONS", self.DAS_BUS, values)
def _pcm_status_msg(self, enable):
values = {"ACC_ACTIVE": enable}
return self.packer.make_can_msg_safety("DAS_3", self.DAS_BUS, values)
def _speed_msg(self, speed):
values = {"SPEED_LEFT": speed, "SPEED_RIGHT": speed}
return self.packer.make_can_msg_safety("SPEED_1", 0, values)
def _user_gas_msg(self, gas):
values = {"Accelerator_Position": gas}
return self.packer.make_can_msg_safety("ECM_5", 0, values)
def _user_brake_msg(self, brake):
values = {"Brake_Pedal_State": 1 if brake else 0}
return self.packer.make_can_msg_safety("ESP_1", 0, values)
def _torque_meas_msg(self, torque):
values = {"EPS_TORQUE_MOTOR": torque}
return self.packer.make_can_msg_safety("EPS_2", 0, values)
def _torque_cmd_msg(self, torque, steer_req=1):
values = {"STEERING_TORQUE": torque, "LKAS_CONTROL_BIT": self.LKAS_ACTIVE_VALUE if steer_req else 0}
return self.packer.make_can_msg_safety("LKAS_COMMAND", 0, values)
def test_buttons(self):
for controls_allowed in (True, False):
self.safety.set_controls_allowed(controls_allowed)
# resume/accel/decel only while controls allowed
self.assertEqual(controls_allowed, self._tx(self._button_msg(resume=True)))
self.assertEqual(controls_allowed, self._tx(self._button_msg(accel=True)))
self.assertEqual(controls_allowed, self._tx(self._button_msg(decel=True)))
# can always cancel
self.assertTrue(self._tx(self._button_msg(cancel=True)))
# invalid: more than one button pressed
combos = [
# 2 buttons
{"cancel": True, "resume": True},
{"cancel": True, "accel": True},
{"cancel": True, "decel": True},
{"resume": True, "accel": True},
{"resume": True, "decel": True},
{"accel": True, "decel": True},
# 3 buttons
{"cancel": True, "resume": True, "accel": True},
{"cancel": True, "resume": True, "decel": True},
{"cancel": True, "accel": True, "decel": True},
{"resume": True, "accel": True, "decel": True},
# all 4 buttons
{"cancel": True, "resume": True, "accel": True, "decel": True},
]
for combo in combos:
with self.subTest(combo=combo):
self.assertFalse(self._tx(self._button_msg(**combo)))
def _lkas_button_msg(self, enabled):
values = {"TOGGLE_LKAS": enabled}
return self.packer.make_can_msg_safety("TRACTION_BUTTON", 0, values)
class TestChryslerRamDTSafety(TestChryslerSafety):
TX_MSGS = [[0xB1, 2], [0xA6, 0], [0xFA, 0]]
RELAY_MALFUNCTION_ADDRS = {0: (0xA6, 0xFA)}
FWD_BLACKLISTED_ADDRS = {2: [0xA6, 0xFA]}
MAX_RATE_UP = 6
MAX_RATE_DOWN = 6
MAX_TORQUE_LOOKUP = [0], [350]
DAS_BUS = 2
LKAS_ACTIVE_VALUE = 2
def setUp(self):
self.packer = CANPackerSafety("chrysler_ram_dt_generated")
self.safety = libsafety_py.libsafety
self.safety.set_safety_hooks(CarParams.SafetyModel.chrysler, ChryslerSafetyFlags.RAM_DT)
self.safety.init_tests()
def _speed_msg(self, speed):
values = {"Vehicle_Speed": speed}
return self.packer.make_can_msg_safety("ESP_8", 0, values)
def _lkas_button_msg(self, enabled):
values = {"LKAS_Button": enabled}
return self.packer.make_can_msg_safety("Center_Stack_2", 0, values)
class TestChryslerRamHDSafety(TestChryslerSafety):
TX_MSGS = [[0x275, 0], [0x276, 0], [0x23A, 2]]
RELAY_MALFUNCTION_ADDRS = {0: (0x276, 0x275)}
FWD_BLACKLISTED_ADDRS = {2: [0x275, 0x276]}
MAX_TORQUE_LOOKUP = [0], [361]
MAX_RATE_UP = 14
MAX_RATE_DOWN = 14
MAX_RT_DELTA = 182
DAS_BUS = 2
LKAS_ACTIVE_VALUE = 2
def setUp(self):
self.packer = CANPackerSafety("chrysler_ram_hd_generated")
self.safety = libsafety_py.libsafety
self.safety.set_safety_hooks(CarParams.SafetyModel.chrysler, ChryslerSafetyFlags.RAM_HD)
self.safety.init_tests()
def _speed_msg(self, speed):
values = {"Vehicle_Speed": speed}
return self.packer.make_can_msg_safety("ESP_8", 0, values)
def _lkas_button_msg(self, enabled):
values = {"LKAS_Button": enabled}
return self.packer.make_can_msg_safety("Center_Stack_2", 0, values)
if __name__ == "__main__":
unittest.main()
@@ -0,0 +1,75 @@
#!/usr/bin/env python3
import unittest
from opendbc.car.structs import CarParams
from opendbc.safety.tests.libsafety import libsafety_py
import opendbc.safety.tests.common as common
from opendbc.safety.tests.common import CANPackerSafety
class TestChryslerCuswSafety(common.CarSafetyTest, common.MotorTorqueSteeringSafetyTest):
TX_MSGS = [[0x1F6, 0], [0x2FA, 0], [0x5DC, 0]]
STANDSTILL_THRESHOLD = 0
RELAY_MALFUNCTION_ADDRS = {0: (0x1F6, 0x5DC)}
FWD_BLACKLISTED_ADDRS = {2: [0x1F6, 0x5DC]}
MAX_RATE_UP = 4
MAX_RATE_DOWN = 4
MAX_TORQUE_LOOKUP = [0], [250]
MAX_RT_DELTA = 150
MAX_TORQUE_ERROR = 80
def setUp(self):
self.packer = CANPackerSafety("chrysler_cusw")
self.safety = libsafety_py.libsafety
self.safety.set_safety_hooks(CarParams.SafetyModel.chryslerCusw, 0)
self.safety.init_tests()
def _button_msg(self, cancel=False, resume=False):
values = {"ACC_Cancel": cancel, "ACC_Resume": resume}
return self.packer.make_can_msg_safety("CRUISE_BUTTONS", 0, values)
def _pcm_status_msg(self, enable):
values = {"ACC_ACTIVE": 1 if enable else 0}
return self.packer.make_can_msg_safety("ACC_CONTROL", 0, values)
def _speed_msg(self, speed):
values = {"VEHICLE_SPEED": speed}
return self.packer.make_can_msg_safety("BRAKE_1", 0, values)
def _user_gas_msg(self, gas):
values = {"GAS_HUMAN": gas}
return self.packer.make_can_msg_safety("ACCEL_GAS", 0, values)
def _user_brake_msg(self, brake):
values = {"DRIVER_BRAKE_SWITCH": 1 if brake else 0}
return self.packer.make_can_msg_safety("BRAKE_3", 0, values)
def _torque_meas_msg(self, torque):
values = {"TORQUE_MOTOR": torque}
return self.packer.make_can_msg_safety("EPS_STATUS", 0, values)
def _torque_cmd_msg(self, torque, steer_req=1):
values = {"STEERING_TORQUE": torque, "LKAS_CONTROL_BIT": steer_req}
return self.packer.make_can_msg_safety("LKAS_COMMAND", 0, values)
def test_buttons(self):
for controls_allowed in (True, False):
self.safety.set_controls_allowed(controls_allowed)
# resume only while controls allowed
self.assertEqual(controls_allowed, self._tx(self._button_msg(resume=True)))
# can always cancel
self.assertTrue(self._tx(self._button_msg(cancel=True)))
def test_rx_hook(self):
for count in range(20):
self.assertTrue(self._rx(self._speed_msg(0)), f"{count=}")
self.assertTrue(self._rx(self._user_brake_msg(False)), f"{count=}")
self.assertTrue(self._rx(self._torque_meas_msg(0)), f"{count=}")
self.assertTrue(self._rx(self._user_gas_msg(0)), f"{count=}")
self.assertTrue(self._rx(self._pcm_status_msg(False)), f"{count=}")
if __name__ == "__main__":
unittest.main()
+74
View File
@@ -0,0 +1,74 @@
#!/usr/bin/env python3
import unittest
import opendbc.safety.tests.common as common
from opendbc.car.structs import CarParams
from opendbc.safety.tests.libsafety import libsafety_py
class TestDefaultRxHookBase(common.SafetyTest):
FWD_BUS_LOOKUP = {}
def test_rx_hook(self):
# default rx hook allows all msgs
for bus in range(4):
for addr in self.SCANNED_ADDRS:
self.assertTrue(self._rx(common.make_msg(bus, addr, 8)), f"failed RX {addr=}")
class TestNoOutput(TestDefaultRxHookBase):
TX_MSGS = []
def setUp(self):
self.safety = libsafety_py.libsafety
self.safety.set_safety_hooks(CarParams.SafetyModel.noOutput, 0)
self.safety.init_tests()
class TestSilent(TestNoOutput):
"""SILENT uses same hooks as NOOUTPUT"""
def setUp(self):
self.safety = libsafety_py.libsafety
self.safety.set_safety_hooks(CarParams.SafetyModel.silent, 0)
self.safety.init_tests()
class TestAllOutput(TestDefaultRxHookBase):
# Allow all messages
TX_MSGS = [[addr, bus] for addr in common.SafetyTest.SCANNED_ADDRS
for bus in range(4)]
def setUp(self):
self.safety = libsafety_py.libsafety
self.safety.set_safety_hooks(CarParams.SafetyModel.allOutput, 0)
self.safety.init_tests()
def test_spam_can_buses(self):
# asserts tx allowed for all scanned addrs
for bus in range(4):
for addr in self.SCANNED_ADDRS:
should_tx = [addr, bus] in self.TX_MSGS
self.assertEqual(should_tx, self._tx(common.make_msg(bus, addr, 8)), f"allowed TX {addr=} {bus=}")
def test_default_controls_not_allowed(self):
# controls always allowed
self.assertTrue(self.safety.get_controls_allowed())
def test_tx_hook_on_wrong_safety_mode(self):
# No point, since we allow all messages
pass
class TestAllOutputPassthrough(TestAllOutput):
FWD_BLACKLISTED_ADDRS = {}
FWD_BUS_LOOKUP = {0: 2, 2: 0}
def setUp(self):
self.safety = libsafety_py.libsafety
self.safety.set_safety_hooks(CarParams.SafetyModel.allOutput, 1)
self.safety.init_tests()
if __name__ == "__main__":
unittest.main()
+52
View File
@@ -0,0 +1,52 @@
#!/usr/bin/env python3
import unittest
import opendbc.safety.tests.common as common
from opendbc.car.structs import CarParams
from opendbc.safety import DLC_TO_LEN
from opendbc.safety.tests.libsafety import libsafety_py
from opendbc.safety.tests.test_defaults import TestDefaultRxHookBase
GM_CAMERA_DIAG_ADDR = 0x24B
class TestElm327(TestDefaultRxHookBase):
TX_MSGS = [[addr, bus] for addr in [GM_CAMERA_DIAG_ADDR, *range(0x600, 0x800),
*range(0x18DA00F1, 0x18DB00F1, 0x100), # 29-bit UDS physical addressing
*[0x18DB33F1], # 29-bit UDS functional address
] for bus in range(4)]
FWD_BUS_LOOKUP = {}
def setUp(self):
self.safety = libsafety_py.libsafety
self.safety.set_safety_hooks(CarParams.SafetyModel.elm327, 0)
self.safety.init_tests()
def test_tx_hook(self):
# ensure we can transmit arbitrary data on allowed addresses
for bus in range(4):
for addr in self.SCANNED_ADDRS:
should_tx = [addr, bus] in self.TX_MSGS
self.assertEqual(should_tx, self._tx(common.make_msg(bus, addr, 8)))
# ELM only allows 8 byte UDS/KWP messages under ISO 15765-4
for msg_len in DLC_TO_LEN:
should_tx = msg_len == 8
self.assertEqual(should_tx, self._tx(common.make_msg(0, 0x700, msg_len)))
# TODO: perform this check for all addresses
# 4 to 15 are reserved ISO-TP frame types (https://en.wikipedia.org/wiki/ISO_15765-2)
for byte in range(0xff):
should_tx = (byte >> 4) <= 3
self.assertEqual(should_tx, self._tx(common.make_msg(0, GM_CAMERA_DIAG_ADDR, dat=bytes([byte] * 8))))
# test GM camera diagnostic address with malformed length
self.assertEqual(False, self._tx(common.make_msg(0, GM_CAMERA_DIAG_ADDR, dat=bytes([0x00] * 7))))
def test_tx_hook_on_wrong_safety_mode(self):
# No point, since we allow many diagnostic addresses
pass
if __name__ == "__main__":
unittest.main()
+542
View File
@@ -0,0 +1,542 @@
#!/usr/bin/env python3
import numpy as np
import random
import unittest
import opendbc.safety.tests.common as common
from opendbc.car.lateral import MAX_LATERAL_ACCEL, MAX_LATERAL_JERK
from opendbc.car.ford.values import FordSafetyFlags
from opendbc.car.structs import CarParams
from opendbc.safety.tests.libsafety import libsafety_py
from opendbc.safety.tests.common import CANPackerSafety
MSG_EngBrakeData = 0x165 # RX from PCM, for driver brake pedal and cruise state
MSG_EngVehicleSpThrottle = 0x204 # RX from PCM, for driver throttle input
MSG_BrakeSysFeatures = 0x415 # RX from ABS, for vehicle speed
MSG_EngVehicleSpThrottle2 = 0x202 # RX from PCM, for second vehicle speed
MSG_Yaw_Data_FD1 = 0x91 # RX from RCM, for yaw rate
MSG_Steering_Data_FD1 = 0x083 # TX by OP, various driver switches and LKAS/CC buttons
MSG_ACCDATA = 0x186 # TX by OP, ACC controls
MSG_ACCDATA_3 = 0x18A # TX by OP, ACC/TJA user interface
MSG_Lane_Assist_Data1 = 0x3CA # TX by OP, Lane Keep Assist
MSG_LateralMotionControl = 0x3D3 # TX by OP, Lateral Control message
MSG_LateralMotionControl2 = 0x3D6 # TX by OP, alternate Lateral Control message
MSG_IPMA_Data = 0x3D8 # TX by OP, IPMA and LKAS user interface
def checksum(msg):
addr, dat, bus = msg
ret = bytearray(dat)
if addr == MSG_Yaw_Data_FD1:
chksum = dat[0] + dat[1] # VehRol_W_Actl
chksum += dat[2] + dat[3] # VehYaw_W_Actl
chksum += dat[5] # VehRollYaw_No_Cnt
chksum += dat[6] >> 6 # VehRolWActl_D_Qf
chksum += (dat[6] >> 4) & 0x3 # VehYawWActl_D_Qf
chksum = 0xff - (chksum & 0xff)
ret[4] = chksum
elif addr == MSG_BrakeSysFeatures:
chksum = dat[0] + dat[1] # Veh_V_ActlBrk
chksum += (dat[2] >> 2) & 0xf # VehVActlBrk_No_Cnt
chksum += dat[2] >> 6 # VehVActlBrk_D_Qf
chksum = 0xff - (chksum & 0xff)
ret[3] = chksum
elif addr == MSG_EngVehicleSpThrottle2:
chksum = (dat[2] >> 3) & 0xf # VehVActlEng_No_Cnt
chksum += (dat[4] >> 5) & 0x3 # VehVActlEng_D_Qf
chksum += dat[6] + dat[7] # Veh_V_ActlEng
chksum = 0xff - (chksum & 0xff)
ret[1] = chksum
return addr, ret, bus
class Buttons:
CANCEL = 0
RESUME = 1
TJA_TOGGLE = 2
# Ford safety has four different configurations tested here:
# * CAN with openpilot longitudinal
# * CAN FD with stock longitudinal
# * CAN FD with openpilot longitudinal
class TestFordSafetyBase(common.CarSafetyTest):
STANDSTILL_THRESHOLD = 1
RELAY_MALFUNCTION_ADDRS = {0: (MSG_ACCDATA_3, MSG_Lane_Assist_Data1, MSG_LateralMotionControl,
MSG_LateralMotionControl2, MSG_IPMA_Data)}
FWD_BLACKLISTED_ADDRS = {2: [MSG_ACCDATA_3, MSG_Lane_Assist_Data1, MSG_LateralMotionControl,
MSG_LateralMotionControl2, MSG_IPMA_Data]}
STEER_MESSAGE = 0
# Curvature control limits
DEG_TO_CAN = 50000 # CAN units per rad/m
MAX_CURVATURE = 0.02 # rad/m, 1000 CAN units
MAX_CURVATURE_ERROR = 0.002 # rad/m, 100 CAN units
CURVATURE_ERROR_MIN_SPEED = 10.0 # m/s
LATERAL_FREQUENCY = 20 # Hz, for per-frame jerk limit
cnt_speed = 0
cnt_speed_2 = 0
cnt_yaw_rate = 0
cnt_lat_ctl = 0
packer: CANPackerSafety
safety: libsafety_py.LibSafety
def _get_max_curvature_can(self, speed):
fudged_speed = max(speed - 1.0, 1.0)
return int(MAX_LATERAL_ACCEL / (fudged_speed * fudged_speed) * self.DEG_TO_CAN) + 1
def _get_max_curvature_delta_can(self, speed):
fudged_speed = max(speed - 1.0, 1.0)
return int(MAX_LATERAL_JERK / (fudged_speed * fudged_speed) / self.LATERAL_FREQUENCY * self.DEG_TO_CAN) + 1
def _set_prev_desired_angle(self, t):
t = round(t * self.DEG_TO_CAN)
self.safety.set_desired_curvature_last(t)
def _reset_curvature_measurement(self, curvature, speed):
for _ in range(6):
self._rx(self._speed_msg(speed))
self._rx(self._speed_msg_2(speed))
self._rx(self._yaw_rate_msg(curvature, speed))
# Driver brake pedal
def _user_brake_msg(self, brake: bool):
# brake pedal and cruise state share same message, so we have to send
# the other signal too
enable = self.safety.get_controls_allowed()
values = {
"BpedDrvAppl_D_Actl": 2 if brake else 1,
"CcStat_D_Actl": 5 if enable else 0,
}
return self.packer.make_can_msg_safety("EngBrakeData", 0, values)
# ABS vehicle speed
def _speed_msg(self, speed: float, quality_flag=True):
values = {"Veh_V_ActlBrk": speed * 3.6, "VehVActlBrk_D_Qf": 3 if quality_flag else 0, "VehVActlBrk_No_Cnt": self.cnt_speed % 16}
self.__class__.cnt_speed += 1
return self.packer.make_can_msg_safety("BrakeSysFeatures", 0, values, fix_checksum=checksum)
# PCM vehicle speed
def _speed_msg_2(self, speed: float, quality_flag=True):
# Ford relies on speed for driver curvature limiting, so it checks two sources
values = {"Veh_V_ActlEng": speed * 3.6, "VehVActlEng_D_Qf": 3 if quality_flag else 0, "VehVActlEng_No_Cnt": self.cnt_speed_2 % 16}
self.__class__.cnt_speed_2 += 1
return self.packer.make_can_msg_safety("EngVehicleSpThrottle2", 0, values, fix_checksum=checksum)
# Standstill state
def _vehicle_moving_msg(self, speed: float):
values = {"VehStop_D_Stat": 1 if speed <= self.STANDSTILL_THRESHOLD else random.choice((0, 2, 3))}
return self.packer.make_can_msg_safety("DesiredTorqBrk", 0, values)
# Current curvature
def _yaw_rate_msg(self, curvature: float, speed: float, quality_flag=True):
values = {"VehYaw_W_Actl": curvature * speed, "VehYawWActl_D_Qf": 3 if quality_flag else 0,
"VehRollYaw_No_Cnt": self.cnt_yaw_rate % 256}
self.__class__.cnt_yaw_rate += 1
return self.packer.make_can_msg_safety("Yaw_Data_FD1", 0, values, fix_checksum=checksum)
# Drive throttle input
def _user_gas_msg(self, gas: float):
values = {"ApedPos_Pc_ActlArb": gas}
return self.packer.make_can_msg_safety("EngVehicleSpThrottle", 0, values)
# Cruise status
def _pcm_status_msg(self, enable: bool):
# brake pedal and cruise state share same message, so we have to send
# the other signal too
brake = self.safety.get_brake_pressed_prev()
values = {
"BpedDrvAppl_D_Actl": 2 if brake else 1,
"CcStat_D_Actl": 5 if enable else 0,
}
return self.packer.make_can_msg_safety("EngBrakeData", 0, values)
# LKAS command
def _lkas_command_msg(self, action: int):
values = {
"LkaActvStats_D2_Req": action,
}
return self.packer.make_can_msg_safety("Lane_Assist_Data1", 0, values)
# LCA command
def _lat_ctl_msg(self, enabled: bool, path_offset: float, path_angle: float, curvature: float, curvature_rate: float,
increment_timer: bool = True):
if increment_timer:
self.safety.set_timer(self.cnt_lat_ctl * int(1e6 / self.LATERAL_FREQUENCY))
self.__class__.cnt_lat_ctl += 1
if self.STEER_MESSAGE == MSG_LateralMotionControl:
values = {
"LatCtl_D_Rq": 1 if enabled else 0,
"LatCtlPathOffst_L_Actl": path_offset, # Path offset [-5.12|5.11] meter
"LatCtlPath_An_Actl": path_angle, # Path angle [-0.5|0.5235] radians
"LatCtlCurv_NoRate_Actl": curvature_rate, # Curvature rate [-0.001024|0.00102375] 1/meter^2
"LatCtlCurv_No_Actl": curvature, # Curvature [-0.02|0.02094] 1/meter
}
return self.packer.make_can_msg_safety("LateralMotionControl", 0, values)
elif self.STEER_MESSAGE == MSG_LateralMotionControl2:
values = {
"LatCtl_D2_Rq": 1 if enabled else 0,
"LatCtlPathOffst_L_Actl": path_offset, # Path offset [-5.12|5.11] meter
"LatCtlPath_An_Actl": path_angle, # Path angle [-0.5|0.5235] radians
"LatCtlCrv_NoRate2_Actl": curvature_rate, # Curvature rate [-0.001024|0.001023] 1/meter^2
"LatCtlCurv_No_Actl": curvature, # Curvature [-0.02|0.02094] 1/meter
}
return self.packer.make_can_msg_safety("LateralMotionControl2", 0, values)
# Cruise control buttons
def _acc_button_msg(self, button: int, bus: int):
values = {
"CcAslButtnCnclPress": 1 if button == Buttons.CANCEL else 0,
"CcAsllButtnResPress": 1 if button == Buttons.RESUME else 0,
"TjaButtnOnOffPress": 1 if button == Buttons.TJA_TOGGLE else 0,
}
return self.packer.make_can_msg_safety("Steering_Data_FD1", bus, values)
def test_rx_hook_speed_mismatch(self):
for speed in np.arange(0, 40, 0.5):
for speed_delta in np.arange(-5, 5, 0.1):
speed_2 = round(max(speed + speed_delta, 0), 1)
self._rx(self._speed_msg(speed))
self._rx(self._speed_msg_2(speed_2))
self.safety.set_controls_allowed(True)
self._tx(self._lat_ctl_msg(True, 0, 0, 0, 0))
within_delta = abs(speed - speed_2) <= common.MAX_SPEED_DELTA
self.assertEqual(self.safety.get_controls_allowed(), within_delta)
def test_rx_hook(self):
# checksum, counter, and quality flag checks
for quality_flag in [True, False]:
for msg_type in ["speed", "speed_2", "yaw"]:
self.safety.set_controls_allowed(True)
# send multiple times to verify counter checks
for _ in range(10):
if msg_type == "speed":
msg = self._speed_msg(0, quality_flag=quality_flag)
elif msg_type == "speed_2":
msg = self._speed_msg_2(0, quality_flag=quality_flag)
elif msg_type == "yaw":
msg = self._yaw_rate_msg(0, 0, quality_flag=quality_flag)
self.assertEqual(quality_flag, self._rx(msg))
self.assertEqual(quality_flag, self.safety.get_controls_allowed())
# Mess with checksum to make it fail, checksum is not checked for 2nd speed
msg[0].data[3] = 0 # Speed checksum & half of yaw signal
should_rx = msg_type == "speed_2" and quality_flag
self.assertEqual(should_rx, self._rx(msg))
self.assertEqual(should_rx, self.safety.get_controls_allowed())
def test_angle_measurements(self):
"""Tests rx hook correctly parses the curvature measurement from the vehicle speed and yaw rate"""
for speed in np.arange(0.5, 40, 0.5):
for curvature in np.arange(0, self.MAX_CURVATURE * 2, 2e-3):
self._rx(self._speed_msg(speed))
for c in (curvature, -curvature, 0, 0, 0, 0):
self._rx(self._yaw_rate_msg(c, speed))
self.assertEqual(self.safety.get_curvature_meas_min(), round(-curvature * self.DEG_TO_CAN))
self.assertEqual(self.safety.get_curvature_meas_max(), round(curvature * self.DEG_TO_CAN))
self._rx(self._yaw_rate_msg(0, speed))
self.assertEqual(self.safety.get_curvature_meas_min(), round(-curvature * self.DEG_TO_CAN))
self.assertEqual(self.safety.get_curvature_meas_max(), 0)
self._rx(self._yaw_rate_msg(0, speed))
self.assertEqual(self.safety.get_curvature_meas_min(), 0)
self.assertEqual(self.safety.get_curvature_meas_max(), 0)
def test_max_lateral_acceleration(self):
# Ford CAN FD can achieve a higher max lateral acceleration than CAN so we limit curvature based on speed
max_curvature_can = round(self.MAX_CURVATURE * self.DEG_TO_CAN)
for speed in np.arange(0, 40, 0.5):
max_can = min(self._get_max_curvature_can(speed), max_curvature_can)
for offset in (-5, -1, 0, 1, 5):
curvature_can = max_can + offset
curvature = curvature_can / self.DEG_TO_CAN
for sign in (-1, 1):
signed_curvature = sign * curvature
self.safety.set_controls_allowed(True)
self._set_prev_desired_angle(signed_curvature)
self._reset_curvature_measurement(signed_curvature, speed)
should_tx = abs(curvature_can) <= max_can
self.assertEqual(should_tx, self._tx(self._lat_ctl_msg(True, 0, 0, signed_curvature, 0)))
def test_steer_allowed(self):
path_offsets = np.arange(-5.12, 5.11, 2.5).round()
path_angles = np.arange(-0.5, 0.5235, 0.25).round(1)
curvature_rates = np.arange(-0.001024, 0.00102375, 0.001).round(3)
curvatures = np.arange(-0.02, 0.02094, 0.01).round(2)
for speed in (self.CURVATURE_ERROR_MIN_SPEED - 1,
self.CURVATURE_ERROR_MIN_SPEED + 1):
max_curvature_can = self._get_max_curvature_can(speed)
for controls_allowed in (True, False):
for steer_control_enabled in (True, False):
for path_offset in path_offsets:
for path_angle in path_angles:
for curvature_rate in curvature_rates:
for curvature in curvatures:
self.safety.set_controls_allowed(controls_allowed)
self._set_prev_desired_angle(curvature)
self._reset_curvature_measurement(curvature, speed)
should_tx = path_offset == 0 and path_angle == 0 and curvature_rate == 0
# when request bit is 0, only allow curvature of 0 since the signal range
# is not large enough to enforce it tracking measured
should_tx = should_tx and (controls_allowed if steer_control_enabled else curvature == 0)
should_tx = should_tx and abs(round(curvature * self.DEG_TO_CAN)) <= max_curvature_can
with self.subTest(controls_allowed=controls_allowed, steer_control_enabled=steer_control_enabled,
path_offset=float(path_offset), path_angle=float(path_angle), curvature_rate=float(curvature_rate),
curvature=float(curvature)):
self.assertEqual(should_tx, self._tx(self._lat_ctl_msg(steer_control_enabled, path_offset, path_angle, curvature, curvature_rate)))
def test_curvature_rate_limits(self):
"""Curvature command must satisfy the ISO 11270 lateral jerk limit per frame."""
self.safety.set_controls_allowed(True)
max_encoded_can = int(self.MAX_CURVATURE * self.DEG_TO_CAN)
for speed in np.arange(0, 40, 0.5):
max_can = self._get_max_curvature_can(speed)
max_delta_can = self._get_max_curvature_delta_can(speed)
base_can = max_delta_can * 2
if base_can + max_delta_can + 5 > min(max_can, max_encoded_can):
continue
# (should_tx, desired_curvature_can) relative to base_can
cases = [
(True, max_delta_can), # at jerk limit
(False, max_delta_can + 5), # over jerk limit
]
for sign in (-1, 1):
self._reset_curvature_measurement(sign * base_can / self.DEG_TO_CAN, speed)
for should_tx, delta_can in cases:
self.safety.set_desired_curvature_last(sign * base_can)
self.assertEqual(should_tx, self._tx(self._lat_ctl_msg(True, 0, 0, sign * (base_can + delta_can) / self.DEG_TO_CAN, 0)))
def test_curvature_error_limits(self):
# above CURVATURE_ERROR_MIN_SPEED, command must be within max_curvature_error of measured, below the check is skipped
self.safety.set_controls_allowed(True)
max_error_can = round(self.MAX_CURVATURE_ERROR * self.DEG_TO_CAN)
for speed in (self.CURVATURE_ERROR_MIN_SPEED - 1, self.CURVATURE_ERROR_MIN_SPEED + 1):
limit_command = speed > self.CURVATURE_ERROR_MIN_SPEED
self._reset_curvature_measurement(0, speed)
for sign in (-1, 1):
self.safety.set_desired_curvature_last(sign * max_error_can)
self.assertTrue(self._tx(self._lat_ctl_msg(True, 0, 0, sign * (max_error_can + 1) / self.DEG_TO_CAN, 0)))
self.safety.set_desired_curvature_last(sign * max_error_can)
self.assertEqual(not limit_command, self._tx(self._lat_ctl_msg(True, 0, 0, sign * (max_error_can + 2) / self.DEG_TO_CAN, 0)))
# newest speed sample gates the check
self._reset_curvature_measurement(0, self.CURVATURE_ERROR_MIN_SPEED - 1)
self._rx(self._speed_msg(self.CURVATURE_ERROR_MIN_SPEED + 1))
self.safety.set_desired_curvature_last(max_error_can)
self.assertFalse(self._tx(self._lat_ctl_msg(True, 0, 0, (max_error_can + 2) / self.DEG_TO_CAN, 0)))
def test_curvature_violation(self):
# If violation occurs, curvature cmd is blocked until reset to 0
self.safety.set_controls_allowed(True)
speed = 25.
max_delta_can = self._get_max_curvature_delta_can(speed)
self._reset_curvature_measurement(0, speed)
self._tx(self._lat_ctl_msg(True, 0, 0, 0, 0))
over_curvature = (max_delta_can + 5) / self.DEG_TO_CAN
for _ in range(20):
self.assertFalse(self._tx(self._lat_ctl_msg(True, 0, 0, over_curvature, 0)))
self.assertTrue(self._tx(self._lat_ctl_msg(True, 0, 0, 0, 0)))
# prev tracks the commanded curvature on a passing tx (not reset to 0 every frame)
self.assertTrue(self._tx(self._lat_ctl_msg(True, 0, 0, max_delta_can / self.DEG_TO_CAN, 0)))
self.assertTrue(self._tx(self._lat_ctl_msg(True, 0, 0, 2 * max_delta_can / self.DEG_TO_CAN, 0)))
def test_rt_limits(self):
# send rate is limited over a rolling 250ms window split into two half-interval buckets
self.safety.set_controls_allowed(True)
self._reset_curvature_measurement(0, 0)
max_rt_msgs = int(self.LATERAL_FREQUENCY * common.RT_INTERVAL / 1e6 * 1.2 + 1)
half = common.RT_INTERVAL // 2
# too many messages within one window is blocked
self.safety.set_timer(0)
for i in range(max_rt_msgs * 2):
self.assertEqual(i <= max_rt_msgs, self._tx(self._lat_ctl_msg(True, 0, 0, 0, 0, increment_timer=False)))
# shift the overflow into the previous bucket
self.safety.set_timer(half)
self.assertFalse(self._tx(self._lat_ctl_msg(True, 0, 0, 0, 0, increment_timer=False)))
# previous bucket still counts within the half interval
self.safety.set_timer(half + 2 * common.RT_INTERVAL // 5)
self.assertFalse(self._tx(self._lat_ctl_msg(True, 0, 0, 0, 0, increment_timer=False)))
self.assertFalse(self._tx(self._lat_ctl_msg(True, 0, 0, 0, 0, increment_timer=False)))
# both buckets clear after a full interval
self.safety.set_timer(half + common.RT_INTERVAL)
self.assertFalse(self._tx(self._lat_ctl_msg(True, 0, 0, 0, 0, increment_timer=False)))
self.safety.set_timer(half + 2 * common.RT_INTERVAL)
for _ in range(max_rt_msgs):
self.assertTrue(self._tx(self._lat_ctl_msg(True, 0, 0, 0, 0, increment_timer=False)))
def test_prevent_lkas_action(self):
self.safety.set_controls_allowed(1)
self.assertFalse(self._tx(self._lkas_command_msg(1)))
self.safety.set_controls_allowed(0)
self.assertFalse(self._tx(self._lkas_command_msg(1)))
def test_acc_buttons(self):
for allowed in (0, 1):
self.safety.set_controls_allowed(allowed)
for enabled in (True, False):
self._rx(self._pcm_status_msg(enabled))
self.assertTrue(self._tx(self._acc_button_msg(Buttons.TJA_TOGGLE, 2)))
for allowed in (0, 1):
self.safety.set_controls_allowed(allowed)
for bus in (0, 2):
self.assertEqual(allowed, self._tx(self._acc_button_msg(Buttons.RESUME, bus)))
for enabled in (True, False):
self._rx(self._pcm_status_msg(enabled))
for bus in (0, 2):
self.assertEqual(enabled, self._tx(self._acc_button_msg(Buttons.CANCEL, bus)))
def test_enable_control_allowed_from_acc_main_on(self):
for enable_mads in (True, False):
with self.subTest("enable_mads", mads_enabled=enable_mads):
for main_button_msg_valid in (True, False):
with self.subTest("main_button_msg_valid", state_valid=main_button_msg_valid):
self.safety.set_mads_params(enable_mads, False, False)
self._rx(self._pcm_status_msg(main_button_msg_valid))
self.assertEqual(enable_mads and main_button_msg_valid, self.safety.get_controls_allowed_lateral())
class TestFordCANFDStockSafety(TestFordSafetyBase):
STEER_MESSAGE = MSG_LateralMotionControl2
TX_MSGS = [
[MSG_Steering_Data_FD1, 0], [MSG_Steering_Data_FD1, 2], [MSG_ACCDATA_3, 0], [MSG_Lane_Assist_Data1, 0],
[MSG_LateralMotionControl2, 0], [MSG_IPMA_Data, 0],
]
RELAY_MALFUNCTION_ADDRS = {0: (MSG_ACCDATA_3, MSG_Lane_Assist_Data1, MSG_LateralMotionControl2,
MSG_IPMA_Data)}
FWD_BLACKLISTED_ADDRS = {2: [MSG_ACCDATA_3, MSG_Lane_Assist_Data1, MSG_LateralMotionControl2,
MSG_IPMA_Data]}
def setUp(self):
self.packer = CANPackerSafety("ford_lincoln_base_pt")
self.safety = libsafety_py.libsafety
self.safety.set_safety_hooks(CarParams.SafetyModel.ford, FordSafetyFlags.CANFD)
self.safety.init_tests()
class TestFordLongitudinalSafetyBase(TestFordSafetyBase):
MAX_ACCEL = 2.0 # accel is used for brakes, but openpilot can set positive values
MIN_ACCEL = -3.5
INACTIVE_ACCEL = 0.0
MAX_GAS = 2.0
MIN_GAS = -0.5
INACTIVE_GAS = -5.0
# ACC command
def _acc_command_msg(self, gas: float, brake: float, brake_actuation: bool, cmbb_deny: bool = False):
values = {
"AccPrpl_A_Rq": gas, # [-5|5.23] m/s^2
"AccPrpl_A_Pred": gas, # [-5|5.23] m/s^2
"AccBrkTot_A_Rq": brake, # [-20|11.9449] m/s^2
"AccBrkPrchg_B_Rq": 1 if brake_actuation else 0, # Pre-charge brake request: 0=No, 1=Yes
"AccBrkDecel_B_Rq": 1 if brake_actuation else 0, # Deceleration request: 0=Inactive, 1=Active
"CmbbDeny_B_Actl": 1 if cmbb_deny else 0, # [0|1] deny AEB actuation
}
return self.packer.make_can_msg_safety("ACCDATA", 0, values)
def test_stock_aeb(self):
# Test that CmbbDeny_B_Actl is never 1, it prevents the ABS module from actuating AEB requests from ACCDATA_2
for controls_allowed in (True, False):
self.safety.set_controls_allowed(controls_allowed)
for cmbb_deny in (True, False):
should_tx = not cmbb_deny
self.assertEqual(should_tx, self._tx(self._acc_command_msg(self.INACTIVE_GAS, self.INACTIVE_ACCEL, controls_allowed, cmbb_deny)))
should_tx = controls_allowed and not cmbb_deny
self.assertEqual(should_tx, self._tx(self._acc_command_msg(self.MAX_GAS, self.MAX_ACCEL, controls_allowed, cmbb_deny)))
def test_gas_safety_check(self):
for controls_allowed in (True, False):
self.safety.set_controls_allowed(controls_allowed)
for gas in np.concatenate((np.arange(self.MIN_GAS - 2, self.MAX_GAS + 2, 0.05), [self.INACTIVE_GAS])):
gas = round(gas, 2) # floats might not hit exact boundary conditions without rounding
should_tx = (controls_allowed and self.MIN_GAS <= gas <= self.MAX_GAS) or gas == self.INACTIVE_GAS
self.assertEqual(should_tx, self._tx(self._acc_command_msg(gas, self.INACTIVE_ACCEL, controls_allowed)))
def test_brake_safety_check(self):
brake_values = self._boundary_values([self.MIN_ACCEL, self.MAX_ACCEL, self.INACTIVE_ACCEL],
self.MIN_ACCEL - 2, self.MAX_ACCEL + 2, 0.05)
for controls_allowed in (True, False):
self.safety.set_controls_allowed(controls_allowed)
for brake_actuation in (True, False):
for brake in brake_values:
should_tx = (controls_allowed and self.MIN_ACCEL <= brake <= self.MAX_ACCEL) or brake == self.INACTIVE_ACCEL
should_tx = should_tx and (controls_allowed or not brake_actuation)
self.assertEqual(should_tx, self._tx(self._acc_command_msg(self.INACTIVE_GAS, brake, brake_actuation)))
class TestFordLongitudinalSafety(TestFordLongitudinalSafetyBase):
STEER_MESSAGE = MSG_LateralMotionControl
TX_MSGS = [
[MSG_Steering_Data_FD1, 0], [MSG_Steering_Data_FD1, 2], [MSG_ACCDATA, 0], [MSG_ACCDATA_3, 0], [MSG_Lane_Assist_Data1, 0],
[MSG_LateralMotionControl, 0], [MSG_IPMA_Data, 0],
]
RELAY_MALFUNCTION_ADDRS = {0: (MSG_ACCDATA, MSG_ACCDATA_3, MSG_Lane_Assist_Data1, MSG_LateralMotionControl,
MSG_IPMA_Data)}
FWD_BLACKLISTED_ADDRS = {2: [MSG_ACCDATA, MSG_ACCDATA_3, MSG_Lane_Assist_Data1, MSG_LateralMotionControl,
MSG_IPMA_Data]}
def setUp(self):
self.packer = CANPackerSafety("ford_lincoln_base_pt")
self.safety = libsafety_py.libsafety
# Make sure we enforce long safety even without long flag for CAN
self.safety.set_safety_hooks(CarParams.SafetyModel.ford, 0)
self.safety.init_tests()
class TestFordCANFDLongitudinalSafety(TestFordLongitudinalSafetyBase):
STEER_MESSAGE = MSG_LateralMotionControl2
TX_MSGS = [
[MSG_Steering_Data_FD1, 0], [MSG_Steering_Data_FD1, 2], [MSG_ACCDATA, 0], [MSG_ACCDATA_3, 0], [MSG_Lane_Assist_Data1, 0],
[MSG_LateralMotionControl2, 0], [MSG_IPMA_Data, 0],
]
RELAY_MALFUNCTION_ADDRS = {0: (MSG_ACCDATA, MSG_ACCDATA_3, MSG_Lane_Assist_Data1, MSG_LateralMotionControl2,
MSG_IPMA_Data)}
FWD_BLACKLISTED_ADDRS = {2: [MSG_ACCDATA, MSG_ACCDATA_3, MSG_Lane_Assist_Data1, MSG_LateralMotionControl2,
MSG_IPMA_Data]}
def setUp(self):
self.packer = CANPackerSafety("ford_lincoln_base_pt")
self.safety = libsafety_py.libsafety
self.safety.set_safety_hooks(CarParams.SafetyModel.ford, FordSafetyFlags.LONG_CONTROL | FordSafetyFlags.CANFD)
self.safety.init_tests()
if __name__ == "__main__":
unittest.main()
+276
View File
@@ -0,0 +1,276 @@
#!/usr/bin/env python3
import unittest
from opendbc.car.gm.values import GMSafetyFlags
from opendbc.car.structs import CarParams
from opendbc.safety.tests.libsafety import libsafety_py
import opendbc.safety.tests.common as common
from opendbc.safety.tests.common import CANPackerSafety
from opendbc.sunnypilot.car.gm.values_ext import GMSafetyFlagsSP
class Buttons:
UNPRESS = 1
RES_ACCEL = 2
DECEL_SET = 3
CANCEL = 6
class GmLongitudinalBase(common.CarSafetyTest, common.LongitudinalGasBrakeSafetyTest):
RELAY_MALFUNCTION_ADDRS = {0: (0x180, 0x2CB), 2: (0x184,)} # ASCMLKASteeringCmd, ASCMGasRegenCmd, PSCMStatus
MAX_POSSIBLE_BRAKE = 2 ** 12
MAX_BRAKE = 400
MAX_POSSIBLE_GAS = 4000 # reasonably excessive limits, not signal max
MIN_POSSIBLE_GAS = -4000
PCM_CRUISE = False # openpilot can control the PCM state if longitudinal
def _send_brake_msg(self, brake):
values = {"FrictionBrakeCmd": -brake}
return self.packer_chassis.make_can_msg_safety("EBCMFrictionBrakeCmd", self.BRAKE_BUS, values)
def _send_gas_msg(self, gas):
values = {"GasRegenCmd": gas}
return self.packer.make_can_msg_safety("ASCMGasRegenCmd", 0, values)
# override these tests from CarSafetyTest, GM longitudinal uses button enable
def _pcm_status_msg(self, enable):
raise NotImplementedError
def test_disable_control_allowed_from_cruise(self):
pass
def test_enable_control_allowed_from_cruise(self):
pass
def test_cruise_engaged_prev(self):
pass
def test_set_resume_buttons(self):
"""
SET and RESUME enter controls allowed on their falling and rising edges, respectively.
"""
for btn_prev in range(8):
for btn_cur in range(8):
with self.subTest(btn_prev=btn_prev, btn_cur=btn_cur):
self._rx(self._button_msg(btn_prev))
self.safety.set_controls_allowed(0)
for _ in range(10):
self._rx(self._button_msg(btn_cur))
should_enable = btn_cur != Buttons.DECEL_SET and btn_prev == Buttons.DECEL_SET
should_enable = should_enable or (btn_cur == Buttons.RES_ACCEL and btn_prev != Buttons.RES_ACCEL)
should_enable = should_enable and btn_cur != Buttons.CANCEL
self.assertEqual(should_enable, self.safety.get_controls_allowed())
def test_cancel_button(self):
self.safety.set_controls_allowed(1)
self._rx(self._button_msg(Buttons.CANCEL))
self.assertFalse(self.safety.get_controls_allowed())
class TestGmSafetyBase(common.CarSafetyTest, common.DriverTorqueSteeringSafetyTest):
STANDSTILL_THRESHOLD = 10 * 0.0311
# Ensures ASCM is off on ASCM cars, and relay is not malfunctioning for camera-ACC cars
RELAY_MALFUNCTION_ADDRS = {0: (0x180,), 2: (0x184,)} # ASCMLKASteeringCmd, PSCMStatus
BUTTONS_BUS = 0 # rx or tx
BRAKE_BUS = 0 # tx only
MAX_RATE_UP = 10
MAX_RATE_DOWN = 15
MAX_TORQUE_LOOKUP = [0], [300]
MAX_RT_DELTA = 128
DRIVER_TORQUE_ALLOWANCE = 65
DRIVER_TORQUE_FACTOR = 4
PCM_CRUISE = True # openpilot is tied to the PCM state if not longitudinal
EXTRA_SAFETY_PARAM = 0
def setUp(self):
self.packer = CANPackerSafety("gm_global_a_powertrain_generated")
self.packer_chassis = CANPackerSafety("gm_global_a_chassis")
self.safety = libsafety_py.libsafety
self.safety.set_safety_hooks(CarParams.SafetyModel.gm, 0)
self.safety.init_tests()
def _pcm_status_msg(self, enable):
if self.PCM_CRUISE:
values = {"CruiseState": enable}
return self.packer.make_can_msg_safety("AcceleratorPedal2", 0, values)
else:
raise NotImplementedError
def _speed_msg(self, speed):
values = {"%sWheelSpd" % s: speed for s in ["RL", "RR"]}
return self.packer.make_can_msg_safety("EBCMWheelSpdRear", 0, values)
def _user_brake_msg(self, brake):
# GM safety has a brake threshold of 8
values = {"BrakePedalPos": 8 if brake else 0}
return self.packer.make_can_msg_safety("ECMAcceleratorPos", 0, values)
def _user_gas_msg(self, gas):
values = {"AcceleratorPedal2": 1 if gas else 0}
if self.PCM_CRUISE:
# Fill CruiseState with expected value if the safety mode reads cruise state from gas msg
values["CruiseState"] = self.safety.get_controls_allowed()
return self.packer.make_can_msg_safety("AcceleratorPedal2", 0, values)
def _torque_driver_msg(self, torque):
# Safety tests assume driver torque is an int, use DBC factor
values = {"LKADriverAppldTrq": torque * 0.01}
return self.packer.make_can_msg_safety("PSCMStatus", 0, values)
def _torque_cmd_msg(self, torque, steer_req=1):
values = {"LKASteeringCmd": torque, "LKASteeringCmdActive": steer_req}
return self.packer.make_can_msg_safety("ASCMLKASteeringCmd", 0, values)
def _button_msg(self, buttons):
values = {"ACCButtons": buttons}
return self.packer.make_can_msg_safety("ASCMSteeringButton", self.BUTTONS_BUS, values)
class TestGmEVSafetyBase(TestGmSafetyBase):
EXTRA_SAFETY_PARAM = GMSafetyFlags.EV
# existence of _user_regen_msg adds regen tests
def _user_regen_msg(self, regen):
values = {"RegenPaddle": 2 if regen else 0}
return self.packer.make_can_msg_safety("EBCMRegenPaddle", 0, values)
class TestGmAscmSafety(GmLongitudinalBase, TestGmSafetyBase):
TX_MSGS = [[0x180, 0], [0x409, 0], [0x40A, 0], [0x2CB, 0], [0x370, 0], # pt bus
[0xA1, 1], [0x306, 1], [0x308, 1], [0x310, 1], # obs bus
[0x315, 2]] # ch bus
FWD_BLACKLISTED_ADDRS: dict[int, list[int]] = {}
RELAY_MALFUNCTION_ADDRS = {0: (0x180, 0x2CB)} # ASCMLKASteeringCmd, ASCMGasRegenCmd
FWD_BUS_LOOKUP: dict[int, int] = {}
BRAKE_BUS = 2
MAX_GAS = 1018
MIN_GAS = -650 # maximum regen
INACTIVE_GAS = -650
def setUp(self):
self.packer = CANPackerSafety("gm_global_a_powertrain_generated")
self.packer_chassis = CANPackerSafety("gm_global_a_chassis")
self.safety = libsafety_py.libsafety
self.safety.set_safety_hooks(CarParams.SafetyModel.gm, self.EXTRA_SAFETY_PARAM)
self.safety.init_tests()
class TestGmAscmEVSafety(TestGmAscmSafety, TestGmEVSafetyBase):
pass
class TestGmCameraSafetyBase(TestGmSafetyBase):
def _user_brake_msg(self, brake):
values = {"BrakePressed": brake}
return self.packer.make_can_msg_safety("ECMEngineStatus", 0, values)
class TestGmCameraSafety(TestGmCameraSafetyBase):
TX_MSGS = [[0x180, 0], # pt bus
[0x184, 2]] # camera bus
FWD_BLACKLISTED_ADDRS = {2: [0x180], 0: [0x184]} # block LKAS message and PSCMStatus
BUTTONS_BUS = 2 # tx only
def setUp(self):
self.packer = CANPackerSafety("gm_global_a_powertrain_generated")
self.packer_chassis = CANPackerSafety("gm_global_a_chassis")
self.safety = libsafety_py.libsafety
self.safety.set_safety_hooks(CarParams.SafetyModel.gm, GMSafetyFlags.HW_CAM | self.EXTRA_SAFETY_PARAM)
self.safety.init_tests()
def test_buttons(self):
# Only CANCEL button is allowed while cruise is enabled
self.safety.set_controls_allowed(0)
for btn in range(8):
self.assertFalse(self._tx(self._button_msg(btn)))
self.safety.set_controls_allowed(1)
for btn in range(8):
self.assertFalse(self._tx(self._button_msg(btn)))
for enabled in (True, False):
self._rx(self._pcm_status_msg(enabled))
self.assertEqual(enabled, self._tx(self._button_msg(Buttons.CANCEL)))
class TestGmCameraEVSafety(TestGmCameraSafety, TestGmEVSafetyBase):
pass
class TestGmCameraLongitudinalSafety(GmLongitudinalBase, TestGmCameraSafetyBase):
TX_MSGS = [[0x180, 0], [0x315, 0], [0x2CB, 0], [0x370, 0], # pt bus
[0x184, 2]] # camera bus
FWD_BLACKLISTED_ADDRS = {2: [0x180, 0x2CB, 0x370, 0x315], 0: [0x184]} # block LKAS, ACC messages and PSCMStatus
RELAY_MALFUNCTION_ADDRS = {0: (0x180, 0x2CB, 0x370, 0x315), 2: (0x184,)}
BUTTONS_BUS = 0 # rx only
MAX_GAS = 1346
MIN_GAS = -540 # maximum regen
INACTIVE_GAS = -500
def setUp(self):
self.packer = CANPackerSafety("gm_global_a_powertrain_generated")
self.packer_chassis = CANPackerSafety("gm_global_a_chassis")
self.safety = libsafety_py.libsafety
self.safety.set_safety_hooks(CarParams.SafetyModel.gm, GMSafetyFlags.HW_CAM | GMSafetyFlags.HW_CAM_LONG | self.EXTRA_SAFETY_PARAM)
self.safety.init_tests()
class TestGmCameraLongitudinalEVSafety(TestGmCameraLongitudinalSafety, TestGmEVSafetyBase):
pass
class TestGmCameraNonACCSafety(TestGmCameraSafety):
def setUp(self):
self.packer = CANPackerSafety("gm_global_a_powertrain_generated")
self.packer_chassis = CANPackerSafety("gm_global_a_chassis")
self.safety = libsafety_py.libsafety
self.safety.set_current_safety_param_sp(GMSafetyFlagsSP.NON_ACC)
self.safety.set_safety_hooks(CarParams.SafetyModel.gm, GMSafetyFlags.HW_CAM | self.EXTRA_SAFETY_PARAM)
self.safety.init_tests()
def _pcm_status_msg(self, enable):
values = {"CruiseActive": enable}
return self.packer.make_can_msg_safety("ECMCruiseControl", 0, values)
class TestGmCameraEVNonACCSafety(TestGmCameraNonACCSafety, TestGmEVSafetyBase):
pass
class TestGmIgnition(unittest.TestCase):
TX_MSGS: list = []
def setUp(self):
self.safety = libsafety_py.libsafety
self.safety.init_tests()
self.packer = CANPackerSafety("gm_global_a_powertrain_generated")
def _msg(self, mode):
return self.packer.make_can_msg_safety("BCMGeneralPlatformStatus", 0, {"SystemPowerMode": mode})
# SystemPowerMode 2=Run, 3=Crank Request
def test_ignition_on(self):
self.safety.ignition_can_hook(self._msg(2))
self.assertTrue(self.safety.get_ignition_can())
def test_ignition_off(self):
self.safety.ignition_can_hook(self._msg(2))
self.assertTrue(self.safety.get_ignition_can())
self.safety.ignition_can_hook(self._msg(0))
self.assertFalse(self.safety.get_ignition_can())
if __name__ == "__main__":
unittest.main()
+705
View File
@@ -0,0 +1,705 @@
#!/usr/bin/env python3
import unittest
import numpy as np
from opendbc.car.honda.values import HondaSafetyFlags
from opendbc.safety.tests.libsafety import libsafety_py
import opendbc.safety.tests.common as common
from opendbc.car.structs import CarParams
from opendbc.safety.tests.common import CANPackerSafety, MAX_WRONG_COUNTERS
from opendbc.safety.tests.gas_interceptor_common import GasInterceptorSafetyTest
from opendbc.sunnypilot.car.honda.values_ext import HondaSafetyFlagsSP
HONDA_N_COMMON_TX_MSGS = [[0xE4, 0], [0x194, 0], [0x1FA, 0], [0x30C, 0], [0x33D, 0]]
class Btn:
NONE = 0
MAIN = 1
CANCEL = 2
SET = 3
RESUME = 4
# Honda safety has several different configurations tested here:
# * Nidec
# * normal (PCM-enable)
# * alt SCM messages (PCM-enable)
# * gas interceptor (button-enable)
# * gas interceptor with alt SCM messages (button-enable)
# * Bosch
# * Bosch with Longitudinal Support
# * Bosch Radarless
# * Bosch Radarless with Longitudinal Support
class HondaButtonEnableBase(common.CarSafetyTest):
# override these inherited tests since we're using button enable
def test_disable_control_allowed_from_cruise(self):
pass
def test_enable_control_allowed_from_cruise(self):
pass
def test_cruise_engaged_prev(self):
pass
def test_buttons_with_main_off(self):
for btn in (Btn.SET, Btn.RESUME, Btn.CANCEL):
self.safety.set_controls_allowed(1)
self._rx(self._acc_state_msg(False))
self._rx(self._button_msg(btn, main_on=False))
self.assertFalse(self.safety.get_controls_allowed())
def test_set_resume_buttons(self):
"""
Both SET and RES should enter controls allowed on their falling edge.
"""
for main_on in (True, False):
self._rx(self._acc_state_msg(main_on))
for btn_prev in range(8):
for btn_cur in range(8):
self._rx(self._button_msg(Btn.NONE))
self.safety.set_controls_allowed(0)
for _ in range(10):
self._rx(self._button_msg(btn_prev))
self.assertFalse(self.safety.get_controls_allowed())
# should enter controls allowed on falling edge and not transitioning to cancel or main
should_enable = (main_on and
btn_cur != btn_prev and
btn_prev in (Btn.RESUME, Btn.SET) and
btn_cur not in (Btn.CANCEL, Btn.MAIN))
self._rx(self._button_msg(btn_cur, main_on=main_on))
self.assertEqual(should_enable, self.safety.get_controls_allowed(), msg=f"{main_on=} {btn_prev=} {btn_cur=}")
def test_main_cancel_buttons(self):
"""
Both MAIN and CANCEL should exit controls immediately.
"""
for btn in (Btn.MAIN, Btn.CANCEL):
self.safety.set_controls_allowed(1)
self._rx(self._button_msg(btn, main_on=True))
self.assertFalse(self.safety.get_controls_allowed())
def test_disengage_on_main(self):
self.safety.set_controls_allowed(1)
self._rx(self._acc_state_msg(True))
self.assertTrue(self.safety.get_controls_allowed())
self._rx(self._acc_state_msg(False))
self.assertFalse(self.safety.get_controls_allowed())
def test_rx_hook(self):
# TODO: move this test to common
# checksum checks
for msg_type in ["btn", "gas", "speed"]:
self.safety.set_controls_allowed(1)
if msg_type == "btn":
msg = self._button_msg(Btn.SET)
if msg_type == "gas":
msg = self._user_gas_msg(0)
if msg_type == "speed":
msg = self._speed_msg(0)
self.assertTrue(self._rx(msg))
if msg_type != "btn":
msg[0].data[4] = 0 # invalidate checksum
msg[0].data[5] = 0
msg[0].data[6] = 0
msg[0].data[7] = 0
self.assertFalse(self._rx(msg))
self.assertFalse(self.safety.get_controls_allowed())
# counter
# reset wrong_counters to zero by sending valid messages
for i in range(MAX_WRONG_COUNTERS + 1):
self.__class__.cnt_speed += 1
self.__class__.cnt_button += 1
self.__class__.cnt_powertrain_data += 1
if i < MAX_WRONG_COUNTERS:
self.safety.set_controls_allowed(1)
self._rx(self._button_msg(Btn.SET))
self._rx(self._speed_msg(0))
self._rx(self._user_gas_msg(0))
else:
self.assertFalse(self._rx(self._button_msg(Btn.SET)))
self.assertFalse(self._rx(self._speed_msg(0)))
self.assertFalse(self._rx(self._user_gas_msg(0)))
self.assertFalse(self.safety.get_controls_allowed())
# restore counters for future tests with a couple of good messages
for _ in range(2):
self.safety.set_controls_allowed(1)
self._rx(self._button_msg(Btn.SET, main_on=True))
self._rx(self._speed_msg(0))
self._rx(self._user_gas_msg(0))
self._rx(self._button_msg(Btn.SET, main_on=True))
self.assertTrue(self.safety.get_controls_allowed())
class HondaPcmEnableBase(common.CarSafetyTest):
def test_buttons(self):
"""
Buttons should only cancel in this configuration,
since our state is tied to the PCM's cruise state.
"""
for controls_allowed in (True, False):
for main_on in (True, False):
# not a valid state
if controls_allowed and not main_on:
continue
for btn in (Btn.SET, Btn.RESUME, Btn.CANCEL):
self.safety.set_controls_allowed(controls_allowed)
self._rx(self._acc_state_msg(main_on))
# btn + none for falling edge
self._rx(self._button_msg(btn, main_on=main_on))
self._rx(self._button_msg(Btn.NONE, main_on=main_on))
if btn == Btn.CANCEL:
self.assertFalse(self.safety.get_controls_allowed())
else:
self.assertEqual(controls_allowed, self.safety.get_controls_allowed())
class HondaBase(common.CarSafetyTest):
MAX_BRAKE = 255
PT_BUS: int | None = None # must be set when inherited
STEER_BUS: int | None = None # must be set when inherited
BUTTONS_BUS: int | None = None # must be set when inherited, tx on this bus, rx on PT_BUS
RELAY_MALFUNCTION_ADDRS = {0: (0xE4, 0x194)} # STEERING_CONTROL
cnt_speed = 0
cnt_button = 0
cnt_brake = 0
cnt_powertrain_data = 0
cnt_acc_state = 0
def _powertrain_data_msg(self, cruise_on=None, brake_pressed=None, gas_pressed=None):
# preserve the state
if cruise_on is None:
# or'd with controls allowed since the tests use it to "enable" cruise
cruise_on = self.safety.get_cruise_engaged_prev() or self.safety.get_controls_allowed()
if brake_pressed is None:
brake_pressed = self.safety.get_brake_pressed_prev()
if gas_pressed is None:
gas_pressed = self.safety.get_gas_pressed_prev()
values = {
"ACC_STATUS": cruise_on,
"BRAKE_PRESSED": brake_pressed,
"PEDAL_GAS": gas_pressed,
"COUNTER": self.cnt_powertrain_data % 4
}
self.__class__.cnt_powertrain_data += 1
return self.packer.make_can_msg_safety("POWERTRAIN_DATA", self.PT_BUS, values)
def _pcm_status_msg(self, enable):
return self._powertrain_data_msg(cruise_on=enable)
def _speed_msg(self, speed):
values = {"XMISSION_SPEED": speed, "COUNTER": self.cnt_speed % 4}
self.__class__.cnt_speed += 1
return self.packer.make_can_msg_safety("ENGINE_DATA", self.PT_BUS, values)
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_safety("SCM_FEEDBACK", 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, "COUNTER": self.cnt_button % 4}
self.__class__.cnt_button += 1
return self.packer.make_can_msg_safety("SCM_BUTTONS", bus, values)
def _user_brake_msg(self, brake):
return self._powertrain_data_msg(brake_pressed=brake)
def _user_gas_msg(self, gas):
return self._powertrain_data_msg(gas_pressed=gas)
def _send_steer_msg(self, steer):
values = {"STEER_TORQUE": steer}
return self.packer.make_can_msg_safety("STEERING_CONTROL", self.STEER_BUS, values)
def _send_brake_msg(self, brake):
# must be implemented when inherited
raise NotImplementedError
def test_disengage_on_brake(self):
self.safety.set_controls_allowed(1)
self._rx(self._user_brake_msg(1))
self.assertFalse(self.safety.get_controls_allowed())
def test_steer_safety_check(self):
self.safety.set_controls_allowed(0)
self.assertTrue(self._tx(self._send_steer_msg(0x0000)))
self.assertFalse(self._tx(self._send_steer_msg(0x1000)))
def _lkas_button_msg(self, lkas_button=False, setting_btn=0):
values = {"CRUISE_SETTING": 1 if lkas_button else setting_btn, "COUNTER": self.cnt_button % 4}
self.__class__.cnt_button += 1
return self.packer.make_can_msg_safety("SCM_BUTTONS", self.PT_BUS, values)
def test_enable_control_allowed_with_mads_button(self):
"""Tests MADS button state transitions and internal button press state."""
for enable_mads in (True, False):
with self.subTest("enable_mads", mads_enabled=enable_mads):
self.safety.set_mads_params(enable_mads, False, False)
# Verify initial state
self._rx(self._lkas_button_msg(False, 0))
self.assertEqual(0, self.safety.get_mads_button_press()) # NOT_PRESSED
self.assertFalse(self.safety.get_controls_allowed_lateral())
# Verify press sets correct internal state
self._rx(self._lkas_button_msg(False, 1))
self.assertEqual(1, self.safety.get_mads_button_press()) # PRESSED
self.assertEqual(enable_mads, self.safety.get_controls_allowed_lateral())
# Verify release sets correct internal state
self._rx(self._lkas_button_msg(False, 0))
self.assertEqual(0, self.safety.get_mads_button_press()) # NOT_PRESSED
self.assertEqual(enable_mads, self.safety.get_controls_allowed_lateral())
# Test invalid values - should not change button press state
for invalid_setting in (2, 3):
self._rx(self._lkas_button_msg(False, invalid_setting))
self.assertEqual(0, self.safety.get_mads_button_press()) # Should remain NOT_PRESSED
self.assertEqual(enable_mads, self.safety.get_controls_allowed_lateral())
# Verify we can still transition after invalid values
self._rx(self._lkas_button_msg(False, 1))
self.assertEqual(1, self.safety.get_mads_button_press())
self._rx(self._lkas_button_msg(False, 0))
self.assertEqual(0, self.safety.get_mads_button_press())
# ********************* Honda Nidec **********************
class TestHondaNidecSafetyBase(HondaBase):
TX_MSGS = HONDA_N_COMMON_TX_MSGS
FWD_BLACKLISTED_ADDRS = {2: [0xE4, 0x194, 0x33D, 0x30C]}
RELAY_MALFUNCTION_ADDRS = {0: (0xE4, 0x194, 0x33D, 0x30C)}
PT_BUS = 0
STEER_BUS = 0
BUTTONS_BUS = 0
MAX_GAS = 198
BRAKE_SIG = "COMPUTER_BRAKE"
def setUp(self):
self.packer = CANPackerSafety("honda_civic_touring_2016_can_generated")
self.safety = libsafety_py.libsafety
self.safety.set_safety_hooks(CarParams.SafetyModel.hondaNidec, 0)
self.safety.init_tests()
def _send_brake_msg(self, brake, aeb_req=0, bus=0):
values = {self.BRAKE_SIG: brake, "AEB_REQ_1": aeb_req}
return self.packer.make_can_msg_safety("BRAKE_COMMAND", bus, values)
def _rx_brake_msg(self, brake, aeb_req=0):
return self._send_brake_msg(brake, aeb_req, bus=2)
def _send_acc_hud_msg(self, pcm_gas, pcm_speed):
# Used to control ACC on Nidec without pedal
values = {"PCM_GAS": pcm_gas, "PCM_SPEED": pcm_speed}
return self.packer.make_can_msg_safety("ACC_HUD", 0, values)
def test_acc_hud_safety_check(self):
for controls_allowed in [True, False]:
self.safety.set_controls_allowed(controls_allowed)
for pcm_gas in range(255):
for pcm_speed in range(100):
send = (controls_allowed and pcm_gas <= self.MAX_GAS) or (pcm_gas == 0 and pcm_speed == 0)
self.assertEqual(send, self._tx(self._send_acc_hud_msg(pcm_gas, pcm_speed)))
def test_fwd_hook(self):
# normal operation, not forwarding AEB
self.FWD_BLACKLISTED_ADDRS[2].append(0x1FA)
self.safety.set_honda_fwd_brake(False)
super().test_fwd_hook()
# forwarding AEB brake signal
self.FWD_BLACKLISTED_ADDRS = {2: [0xE4, 0x194, 0x33D, 0x30C]}
self.safety.set_honda_fwd_brake(True)
super().test_fwd_hook()
def test_honda_fwd_brake_latching(self):
# Shouldn't fwd stock Honda requesting brake without AEB
self.assertTrue(self._rx(self._rx_brake_msg(self.MAX_BRAKE, aeb_req=0)))
self.assertFalse(self.safety.get_honda_fwd_brake())
# Now allow controls and request some brake
openpilot_brake = round(self.MAX_BRAKE / 2.0)
self.safety.set_controls_allowed(True)
self.assertTrue(self._tx(self._send_brake_msg(openpilot_brake)))
# Still shouldn't fwd stock Honda brake until it's more than openpilot's
for stock_honda_brake in range(self.MAX_BRAKE + 1):
self.assertTrue(self._rx(self._rx_brake_msg(stock_honda_brake, aeb_req=1)))
should_fwd_brake = stock_honda_brake >= openpilot_brake
self.assertEqual(should_fwd_brake, self.safety.get_honda_fwd_brake())
# Shouldn't stop fwding until AEB event is over
for stock_honda_brake in range(self.MAX_BRAKE + 1)[::-1]:
self.assertTrue(self._rx(self._rx_brake_msg(stock_honda_brake, aeb_req=1)))
self.assertTrue(self.safety.get_honda_fwd_brake())
self.assertTrue(self._rx(self._rx_brake_msg(0, aeb_req=0)))
self.assertFalse(self.safety.get_honda_fwd_brake())
def test_brake_safety_check(self):
for fwd_brake in [False, True]:
self.safety.set_honda_fwd_brake(fwd_brake)
for brake in np.arange(0, self.MAX_BRAKE + 10, 1):
for controls_allowed in [True, False]:
self.safety.set_controls_allowed(controls_allowed)
if fwd_brake:
send = False # block openpilot brake msg when fwd'ing stock msg
elif controls_allowed:
send = self.MAX_BRAKE >= brake >= 0
else:
send = brake == 0
self.assertEqual(send, self._tx(self._send_brake_msg(brake)))
class TestHondaNidecPcmSafety(HondaPcmEnableBase, TestHondaNidecSafetyBase):
"""
Covers the Honda Nidec safety mode
"""
# Nidec doesn't disengage on falling edge of cruise. See comment in safety_honda.h
def test_disable_control_allowed_from_cruise(self):
pass
class TestHondaNidecGasInterceptorSafety(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 = CANPackerSafety("honda_civic_touring_2016_can_generated")
self.safety = libsafety_py.libsafety
self.safety.set_current_safety_param_sp(HondaSafetyFlagsSP.GAS_INTERCEPTOR)
self.safety.set_safety_hooks(CarParams.SafetyModel.hondaNidec, 0)
self.safety.init_tests()
class TestHondaNidecPcmAltSafety(TestHondaNidecPcmSafety):
"""
Covers the Honda Nidec safety mode with alt SCM messages
"""
def setUp(self):
self.packer = CANPackerSafety("acura_ilx_2016_can_generated")
self.safety = libsafety_py.libsafety
self.safety.set_safety_hooks(CarParams.SafetyModel.hondaNidec, HondaSafetyFlags.NIDEC_ALT)
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_safety("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_safety("SCM_BUTTONS", bus, values)
class TestHondaNidecAltGasInterceptorSafety(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 = CANPackerSafety("acura_ilx_2016_can_generated")
self.safety = libsafety_py.libsafety
self.safety.set_current_safety_param_sp(HondaSafetyFlagsSP.GAS_INTERCEPTOR)
self.safety.set_safety_hooks(CarParams.SafetyModel.hondaNidec, HondaSafetyFlags.NIDEC_ALT)
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_safety("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_safety("SCM_BUTTONS", bus, values)
# ********************* Honda Bosch **********************
class TestHondaBoschSafetyBase(HondaBase):
PT_BUS = 1
STEER_BUS = 0
BUTTONS_BUS = 1
TX_MSGS = [[0xE4, 0], [0xE5, 0], [0x296, 1], [0x33D, 0], [0x33DA, 0], [0x33DB, 0]]
FWD_BLACKLISTED_ADDRS = {2: [0xE4, 0xE5, 0x33D, 0x33DA, 0x33DB]}
RELAY_MALFUNCTION_ADDRS = {0: (0xE4, 0xE5, 0x33D, 0x33DA, 0x33DB)} # STEERING_CONTROL, BOSCH_SUPPLEMENTAL_1
def setUp(self):
self.packer = CANPackerSafety("honda_civic_hatchback_ex_2017_can_generated")
self.safety = libsafety_py.libsafety
def _alt_brake_msg(self, brake):
values = {"BRAKE_PRESSED": brake, "COUNTER": self.cnt_brake % 4}
self.__class__.cnt_brake += 1
return self.packer.make_can_msg_safety("BRAKE_MODULE", self.PT_BUS, values)
def _send_brake_msg(self, brake):
pass
def test_spam_cancel_safety_check(self):
self.safety.set_controls_allowed(0)
self.assertTrue(self._tx(self._button_msg(Btn.CANCEL, bus=self.BUTTONS_BUS)))
self.assertFalse(self._tx(self._button_msg(Btn.RESUME, bus=self.BUTTONS_BUS)))
self.assertFalse(self._tx(self._button_msg(Btn.SET, bus=self.BUTTONS_BUS)))
# do not block resume if we are engaged already
self.safety.set_controls_allowed(1)
self.assertTrue(self._tx(self._button_msg(Btn.RESUME, bus=self.BUTTONS_BUS)))
class TestHondaBoschAltBrakeSafetyBase(TestHondaBoschSafetyBase):
"""
Base Bosch safety test class with an alternate brake message
"""
def setUp(self):
super().setUp()
self.safety.set_safety_hooks(CarParams.SafetyModel.hondaBosch, HondaSafetyFlags.ALT_BRAKE)
self.safety.init_tests()
def _user_brake_msg(self, brake):
return self._alt_brake_msg(brake)
def test_alt_brake_rx_hook(self):
self.safety.set_honda_alt_brake_msg(1)
self.safety.set_controls_allowed(1)
msg = self._alt_brake_msg(0)
self.assertTrue(self._rx(msg))
msg[0].data[2] = msg[0].data[2] & 0xF0 # invalidate checksum
self.assertFalse(self._rx(msg))
self.assertFalse(self.safety.get_controls_allowed())
def test_alt_disengage_on_brake(self):
self.safety.set_honda_alt_brake_msg(1)
self.safety.set_controls_allowed(1)
self._rx(self._alt_brake_msg(1))
self.assertFalse(self.safety.get_controls_allowed())
self.safety.set_honda_alt_brake_msg(0)
self.safety.set_controls_allowed(1)
self._rx(self._alt_brake_msg(1))
self.assertTrue(self.safety.get_controls_allowed())
class TestHondaBoschSafety(HondaPcmEnableBase, TestHondaBoschSafetyBase):
"""
Covers the Honda Bosch safety mode with stock longitudinal
"""
def setUp(self):
super().setUp()
self.safety.set_safety_hooks(CarParams.SafetyModel.hondaBosch, 0)
self.safety.init_tests()
class TestHondaBoschAltBrakeSafety(HondaPcmEnableBase, TestHondaBoschAltBrakeSafetyBase):
"""
Covers the Honda Bosch safety mode with stock longitudinal and an alternate brake message
"""
class TestHondaBoschLongSafety(HondaButtonEnableBase, TestHondaBoschSafetyBase):
"""
Covers the Honda Bosch safety mode with longitudinal control
"""
NO_GAS = -30000
MAX_GAS = 2000
MAX_ACCEL = 2.0 # accel is used for brakes, but openpilot can set positive values
MIN_ACCEL = -3.5
STEER_BUS = 1
TX_MSGS = [[0xE4, 1], [0x1DF, 1], [0x1EF, 1], [0x1FA, 1], [0x30C, 1], [0x33D, 1], [0x33DA, 1], [0x33DB, 1], [0x39F, 1], [0x18DAB0F1, 1]]
FWD_BLACKLISTED_ADDRS = {}
# 0x1DF is to test that radar is disabled
RELAY_MALFUNCTION_ADDRS = {1: (0xE4, 0x1DF, 0x33D, 0x33DA, 0x33DB)} # STEERING_CONTROL, ACC_CONTROL
def setUp(self):
super().setUp()
self.safety.set_safety_hooks(CarParams.SafetyModel.hondaBosch, HondaSafetyFlags.BOSCH_LONG)
self.safety.init_tests()
def _send_gas_brake_msg(self, gas, accel):
values = {
"GAS_COMMAND": gas,
"ACCEL_COMMAND": accel,
"BRAKE_REQUEST": accel < 0,
}
return self.packer.make_can_msg_safety("ACC_CONTROL", self.PT_BUS, values)
# Longitudinal doesn't need to send buttons
def test_spam_cancel_safety_check(self):
pass
def test_diagnostics(self):
tester_present = libsafety_py.make_CANPacket(0x18DAB0F1, self.PT_BUS, b"\x02\x3E\x80\x00\x00\x00\x00\x00")
self.assertTrue(self._tx(tester_present))
not_tester_present = libsafety_py.make_CANPacket(0x18DAB0F1, self.PT_BUS, b"\x03\xAA\xAA\x00\x00\x00\x00\x00")
self.assertFalse(self._tx(not_tester_present))
def test_gas_safety_check(self):
for controls_allowed in [True, False]:
for gas in np.arange(self.NO_GAS, self.MAX_GAS + 2000, 100):
accel = 0 if gas < 0 else gas / 1000
self.safety.set_controls_allowed(controls_allowed)
send = (controls_allowed and 0 <= gas <= self.MAX_GAS) or gas == self.NO_GAS
self.assertEqual(send, self._tx(self._send_gas_brake_msg(gas, accel)), (controls_allowed, gas, accel))
def test_brake_safety_check(self):
for controls_allowed in [True, False]:
for accel in np.arange(self.MIN_ACCEL - 1, self.MAX_ACCEL + 1, 0.01):
accel = round(accel, 2) # floats might not hit exact boundary conditions without rounding
self.safety.set_controls_allowed(controls_allowed)
send = self.MIN_ACCEL <= accel <= self.MAX_ACCEL if controls_allowed else accel == 0
self.assertEqual(send, self._tx(self._send_gas_brake_msg(self.NO_GAS, accel)), (controls_allowed, accel))
class TestHondaBoschRadarlessSafetyBase(TestHondaBoschSafetyBase):
"""Base class for radarless Honda Bosch"""
PT_BUS = 0
STEER_BUS = 0
BUTTONS_BUS = 2 # camera controls ACC, need to send buttons on bus 2
TX_MSGS = [[0xE4, 0], [0x296, 2], [0x33D, 0]]
FWD_BLACKLISTED_ADDRS = {2: [0xE4, 0x33D]}
RELAY_MALFUNCTION_ADDRS = {0: (0xE4, 0x33D)} # STEERING_CONTROL
def setUp(self):
self.packer = CANPackerSafety("honda_bosch_radarless_generated")
self.safety = libsafety_py.libsafety
class TestHondaBoschRadarlessSafety(HondaPcmEnableBase, TestHondaBoschRadarlessSafetyBase):
"""
Covers the Honda Bosch Radarless safety mode with stock longitudinal
"""
def setUp(self):
super().setUp()
self.safety.set_safety_hooks(CarParams.SafetyModel.hondaBosch, HondaSafetyFlags.RADARLESS)
self.safety.init_tests()
class TestHondaBoschRadarlessAltBrakeSafety(HondaPcmEnableBase, TestHondaBoschRadarlessSafetyBase, TestHondaBoschAltBrakeSafetyBase):
"""
Covers the Honda Bosch Radarless safety mode with stock longitudinal and an alternate brake message
"""
def setUp(self):
super().setUp()
self.safety.set_safety_hooks(CarParams.SafetyModel.hondaBosch, HondaSafetyFlags.RADARLESS | HondaSafetyFlags.ALT_BRAKE)
self.safety.init_tests()
class TestHondaBoschRadarlessLongSafety(common.LongitudinalAccelSafetyTest, HondaButtonEnableBase,
TestHondaBoschRadarlessSafetyBase):
"""
Covers the Honda Bosch Radarless safety mode with longitudinal control
"""
TX_MSGS = [[0xE4, 0], [0x33D, 0], [0x1C8, 0], [0x30C, 0]]
FWD_BLACKLISTED_ADDRS = {2: [0xE4, 0x33D, 0x1C8, 0x30C]}
RELAY_MALFUNCTION_ADDRS = {0: (0xE4, 0x1C8, 0x30C, 0x33D)}
def setUp(self):
super().setUp()
self.safety.set_safety_hooks(CarParams.SafetyModel.hondaBosch, HondaSafetyFlags.RADARLESS | HondaSafetyFlags.BOSCH_LONG)
self.safety.init_tests()
def _accel_msg(self, accel):
values = {
"ACCEL_COMMAND": accel,
}
return self.packer.make_can_msg_safety("ACC_CONTROL", self.PT_BUS, values)
# Longitudinal doesn't need to send buttons
def test_spam_cancel_safety_check(self):
pass
class TestHondaBoschCANFDSafetyBase(TestHondaBoschSafetyBase):
"""Base class for CANFD Honda Bosch"""
PT_BUS = 0
STEER_BUS = 0
BUTTONS_BUS = 0
TX_MSGS = [[0xE4, 0], [0x296, 0], [0x33D, 0]]
FWD_BLACKLISTED_ADDRS = {2: [0xE4, 0x33D]}
RELAY_MALFUNCTION_ADDRS = {0: (0xE4, 0x33D)}
def setUp(self):
self.packer = CANPackerSafety("honda_common_canfd_generated")
self.safety = libsafety_py.libsafety
class TestHondaBoschCANFDSafety(HondaPcmEnableBase, TestHondaBoschCANFDSafetyBase):
"""
Covers the Honda Bosch CANFD safety mode with stock longitudinal
"""
def setUp(self):
super().setUp()
self.safety.set_safety_hooks(CarParams.SafetyModel.hondaBosch, HondaSafetyFlags.BOSCH_CANFD)
self.safety.init_tests()
class TestHondaBoschCANFDAltBrakeSafety(HondaPcmEnableBase, TestHondaBoschCANFDSafetyBase, TestHondaBoschAltBrakeSafetyBase):
"""
Covers the Honda Bosch CANFD safety mode with stock longitudinal and an alternate brake message
"""
def setUp(self):
super().setUp()
self.safety.set_safety_hooks(CarParams.SafetyModel.hondaBosch, HondaSafetyFlags.BOSCH_CANFD | HondaSafetyFlags.ALT_BRAKE)
self.safety.init_tests()
class TestHondaNidecHybridSafety(TestHondaNidecPcmSafety):
"""
Covers the Honda Nidec safety mode with hybrid brake
"""
BRAKE_SIG = "COMPUTER_BRAKE_HYBRID"
def setUp(self):
self.packer = CANPackerSafety("honda_clarity_hybrid_2018_can_generated")
self.safety = libsafety_py.libsafety
self.safety.set_current_safety_param_sp(HondaSafetyFlagsSP.NIDEC_HYBRID)
self.safety.set_safety_hooks(CarParams.SafetyModel.hondaNidec, 0)
self.safety.init_tests()
if __name__ == "__main__":
unittest.main()
+588
View File
@@ -0,0 +1,588 @@
#!/usr/bin/env python3
from opendbc.testing import parameterized_class
import random
import unittest
from opendbc.car.hyundai.values import HyundaiSafetyFlags
from opendbc.car.structs import CarParams
from opendbc.safety.tests.libsafety import libsafety_py
import opendbc.safety.tests.common as common
from opendbc.safety.tests.common import CANPackerSafety
from opendbc.safety.tests.hyundai_common import HyundaiButtonBase, HyundaiLongitudinalBase
from opendbc.sunnypilot.car.hyundai.values import HyundaiSafetyFlagsSP
# LDA button availability
LDA_BUTTON = [
{"SAFETY_PARAM_SP": HyundaiSafetyFlagsSP.DEFAULT},
{"SAFETY_PARAM_SP": HyundaiSafetyFlagsSP.HAS_LDA_BUTTON},
]
# All combinations of non-SCC HEV/PHEV/EV cars
_ALL_NON_SCC_HEV_EV_COMBOS = [
# Hybrid
{"PCM_STATUS_MSG": ("E_CRUISE_CONTROL", "CRUISE_LAMP_S"),
"ACC_STATE_MSG": ("E_CRUISE_CONTROL", "CRUISE_LAMP_M"),
"GAS_MSG": ("E_EMS11", "CR_Vcu_AccPedDep_Pos"),
"SAFETY_PARAM": HyundaiSafetyFlags.HYBRID_GAS},
# EV
{"PCM_STATUS_MSG": ("LABEL11", "CC_ACT"),
"ACC_STATE_MSG": ("LABEL11", "CC_React"),
"GAS_MSG": ("E_EMS11", "Accel_Pedal_Pos"),
"SAFETY_PARAM": HyundaiSafetyFlags.EV_GAS},
]
ALL_NON_SCC_HEV_EV_COMBOS = [{**p, **lda} for lda in LDA_BUTTON for p in _ALL_NON_SCC_HEV_EV_COMBOS]
# 4 bit checkusm used in some hyundai messages
# lives outside the can packer because we never send this msg
def checksum(msg):
addr, dat, bus = msg
chksum = 0
if addr == 0x386:
for i, b in enumerate(dat):
for j in range(8):
# exclude checksum and counter bits
if (i != 1 or j < 6) and (i != 3 or j < 6) and (i != 5 or j < 6) and (i != 7 or j < 6):
bit = (b >> j) & 1
else:
bit = 0
chksum += bit
chksum = (chksum ^ 9) & 0xF
ret = bytearray(dat)
ret[5] |= (chksum & 0x3) << 6
ret[7] |= (chksum & 0xc) << 4
else:
for i, b in enumerate(dat):
if addr in [0x260, 0x421] and i == 7:
b &= 0x0F if addr == 0x421 else 0xF0
elif addr == 0x394 and i == 6:
b &= 0xF0
elif addr == 0x394 and i == 7:
continue
chksum += sum(divmod(b, 16))
chksum = (16 - chksum) % 16
ret = bytearray(dat)
ret[6 if addr == 0x394 else 7] |= chksum << (4 if addr == 0x421 else 0)
return addr, ret, bus
@parameterized_class(LDA_BUTTON)
class TestHyundaiSafety(HyundaiButtonBase, common.CarSafetyTest, common.DriverTorqueSteeringSafetyTest, common.SteerRequestCutSafetyTest):
TX_MSGS = [[0x340, 0], [0x4F1, 0], [0x485, 0]]
STANDSTILL_THRESHOLD = 12 # 0.375 kph
RELAY_MALFUNCTION_ADDRS = {0: (0x340, 0x485)} # LKAS11
FWD_BLACKLISTED_ADDRS = {2: [0x340, 0x485]}
MAX_RATE_UP = 3
MAX_RATE_DOWN = 7
MAX_TORQUE_LOOKUP = [0], [384]
MAX_RT_DELTA = 112
DRIVER_TORQUE_ALLOWANCE = 50
DRIVER_TORQUE_FACTOR = 2
# Safety around steering req bit
MIN_VALID_STEERING_FRAMES = 89
MAX_INVALID_STEERING_FRAMES = 2
cnt_gas = 0
cnt_speed = 0
cnt_brake = 0
cnt_cruise = 0
cnt_button = 0
SAFETY_PARAM_SP: int = 0
@classmethod
def setUpClass(cls):
if cls.__name__ == "TestHyundaiSafety":
cls.safety = None
raise unittest.SkipTest
def setUp(self):
self.packer = CANPackerSafety("hyundai_can_generated")
self.safety = libsafety_py.libsafety
self.safety.set_current_safety_param_sp(self.SAFETY_PARAM_SP)
self.safety.set_safety_hooks(CarParams.SafetyModel.hyundai, 0)
self.safety.init_tests()
def _button_msg(self, buttons, main_button=0, bus=0):
values = {"CF_Clu_CruiseSwState": buttons, "CF_Clu_CruiseSwMain": main_button, "CF_Clu_AliveCnt1": self.cnt_button}
self.__class__.cnt_button += 1
return self.packer.make_can_msg_safety("CLU11", bus, values)
def _user_gas_msg(self, gas):
values = {"CF_Ems_AclAct": gas, "AliveCounter": self.cnt_gas % 4}
self.__class__.cnt_gas += 1
return self.packer.make_can_msg_safety("EMS16", 0, values, fix_checksum=checksum)
def _user_brake_msg(self, brake):
values = {"DriverOverride": 2 if brake else random.choice((0, 1, 3)),
"AliveCounterTCS": self.cnt_brake % 8}
self.__class__.cnt_brake += 1
return self.packer.make_can_msg_safety("TCS13", 0, values, fix_checksum=checksum)
def _speed_msg(self, speed):
# safety doesn't scale, so undo the scaling
values = {"WHL_SPD_%s" % s: speed * 0.03125 for s in ["FL", "FR", "RL", "RR"]}
values["WHL_SPD_AliveCounter_LSB"] = (self.cnt_speed % 16) & 0x3
values["WHL_SPD_AliveCounter_MSB"] = (self.cnt_speed % 16) >> 2
self.__class__.cnt_speed += 1
return self.packer.make_can_msg_safety("WHL_SPD11", 0, values, fix_checksum=checksum)
def _pcm_status_msg(self, enable):
values = {"ACCMode": enable, "CR_VSM_Alive": self.cnt_cruise % 16}
self.__class__.cnt_cruise += 1
return self.packer.make_can_msg_safety("SCC12", self.SCC_BUS, values, fix_checksum=checksum)
def _torque_driver_msg(self, torque):
values = {"CR_Mdps_StrColTq": torque}
return self.packer.make_can_msg_safety("MDPS12", 0, values)
def _torque_cmd_msg(self, torque, steer_req=1):
values = {"CR_Lkas_StrToqReq": torque, "CF_Lkas_ActToi": steer_req}
return self.packer.make_can_msg_safety("LKAS11", 0, values)
def _acc_state_msg(self, enable):
values = {"MainMode_ACC": enable}
return self.packer.make_can_msg_safety("SCC11", self.SCC_BUS, values)
def _lkas_button_msg(self, enabled):
if self.SAFETY_PARAM_SP & HyundaiSafetyFlagsSP.HAS_LDA_BUTTON:
values = {"LDA_BTN": enabled}
return self.packer.make_can_msg_safety("BCM_PO_11", 0, values)
else:
raise NotImplementedError
def _main_cruise_button_msg(self, enabled):
return self._button_msg(0, enabled)
def test_pcm_main_cruise_state_availability(self):
"""Test that ACC main state is correctly set when receiving SCC11 (0x420), toggling HYUNDAI_LONG flag.
Only applicable to SCC-based cars. Non-SCC cars use different messages for ACC state
and their rx_checks don't include SCC11 after mode reconfiguration.
"""
if any('NonSCC' in cls.__name__ for cls in type(self).__mro__):
raise unittest.SkipTest("Non-SCC cars use different ACC state messages, not SCC11")
prior_safety_mode = self.safety.get_current_safety_mode()
prior_safety_param = self.safety.get_current_safety_param()
safety_param_sp = self.SAFETY_PARAM_SP
for hyundai_longitudinal in (True, False):
with self.subTest("hyundai_longitudinal", hyundai_longitudinal=hyundai_longitudinal):
self.safety.set_current_safety_param_sp(self.SAFETY_PARAM_SP)
self.safety.set_safety_hooks(CarParams.SafetyModel.hyundai, 0 if hyundai_longitudinal else HyundaiSafetyFlags.LONG)
for should_turn_acc_main_on in (True, False):
with self.subTest("acc_main_on", should_turn_acc_main_on=should_turn_acc_main_on):
self.safety.set_acc_main_on(False)
self._rx(self._acc_state_msg(should_turn_acc_main_on))
expected_acc_main = should_turn_acc_main_on and hyundai_longitudinal
self.assertEqual(expected_acc_main, self.safety.get_acc_main_on())
self.safety.set_current_safety_param_sp(safety_param_sp)
self.safety.set_safety_hooks(prior_safety_mode, prior_safety_param)
self.safety.init_tests()
def test_enable_control_allowed_with_mads_button(self):
"""Toggle MADS with MADS button, testing HAS_LDA_BUTTON param gating."""
default_safety_mode = self.safety.get_current_safety_mode()
default_safety_param = self.safety.get_current_safety_param()
default_safety_param_sp = self.SAFETY_PARAM_SP
try:
self._lkas_button_msg(False)
except NotImplementedError as err:
raise unittest.SkipTest("Skipping test because LDA button is not supported") from err
# CameraSCC rx_checks always include BCM_PO_11 regardless of HAS_LDA_BUTTON param,
# so we can only test the has_lda_button=True case for CameraSCC.
camera_scc = bool(default_safety_param & HyundaiSafetyFlags.CAMERA_SCC)
lda_button_variants = [True] if camera_scc else [True, False]
try:
for enable_mads in (True, False):
with self.subTest("enable_mads", mads_enabled=enable_mads):
for has_lda_button_param in lda_button_variants:
with self.subTest("has_lda_button", has_lda_button_param=has_lda_button_param):
has_lda_button = HyundaiSafetyFlagsSP.HAS_LDA_BUTTON if has_lda_button_param else 0
sp = (default_safety_param_sp & ~HyundaiSafetyFlagsSP.HAS_LDA_BUTTON) | has_lda_button
self.safety.set_current_safety_param_sp(sp)
self.safety.set_safety_hooks(default_safety_mode, default_safety_param)
self.safety.init_tests()
self.safety.set_controls_allowed(False)
self.safety.set_acc_main_on(False)
self.safety.set_controls_allowed_lateral(False)
self.safety.set_mads_params(enable_mads, False, False)
self.assertEqual(enable_mads, self.safety.get_enable_mads())
self._rx(self._lkas_button_msg(True))
self._rx(self._lkas_button_msg(False))
self.assertEqual(enable_mads and has_lda_button_param, self.safety.get_controls_allowed_lateral())
finally:
self.safety.set_current_safety_param_sp(default_safety_param_sp)
@parameterized_class(LDA_BUTTON)
class TestHyundaiSafetyAltLimits(TestHyundaiSafety):
MAX_RATE_UP = 2
MAX_RATE_DOWN = 3
MAX_TORQUE_LOOKUP = [0], [270]
@classmethod
def setUpClass(cls):
if cls.__name__ == "TestHyundaiSafetyAltLimits":
cls.safety = None
raise unittest.SkipTest
def setUp(self):
self.packer = CANPackerSafety("hyundai_can_generated")
self.safety = libsafety_py.libsafety
self.safety.set_current_safety_param_sp(self.SAFETY_PARAM_SP)
self.safety.set_safety_hooks(CarParams.SafetyModel.hyundai, HyundaiSafetyFlags.ALT_LIMITS)
self.safety.init_tests()
@parameterized_class(LDA_BUTTON)
class TestHyundaiSafetyAltLimits2(TestHyundaiSafety):
MAX_RATE_UP = 2
MAX_RATE_DOWN = 3
MAX_TORQUE_LOOKUP = [0], [170]
@classmethod
def setUpClass(cls):
if cls.__name__ == "TestHyundaiSafetyAltLimits2":
cls.safety = None
raise unittest.SkipTest
def setUp(self):
self.packer = CANPackerSafety("hyundai_can_generated")
self.safety = libsafety_py.libsafety
self.safety.set_current_safety_param_sp(self.SAFETY_PARAM_SP)
self.safety.set_safety_hooks(CarParams.SafetyModel.hyundai, HyundaiSafetyFlags.ALT_LIMITS_2)
self.safety.init_tests()
@parameterized_class(LDA_BUTTON)
class TestHyundaiSafetyCameraSCC(TestHyundaiSafety):
BUTTONS_TX_BUS = 2 # tx on 2, rx on 0
SCC_BUS = 2 # rx on 2
@classmethod
def setUpClass(cls):
if cls.__name__ == "TestHyundaiSafetyCameraSCC":
cls.safety = None
raise unittest.SkipTest
def setUp(self):
self.packer = CANPackerSafety("hyundai_can_generated")
self.safety = libsafety_py.libsafety
self.safety.set_current_safety_param_sp(self.SAFETY_PARAM_SP)
self.safety.set_safety_hooks(CarParams.SafetyModel.hyundai, HyundaiSafetyFlags.CAMERA_SCC)
self.safety.init_tests()
def test_pcm_main_cruise_state_availability(self):
"""
Test that ACC main state is correctly set when receiving 0x420 message.
For camera SCC, ACC main should always be on when receiving 0x420 message
"""
for should_turn_acc_main_on in (True, False):
with self.subTest("acc_main_on", should_turn_acc_main_on=should_turn_acc_main_on):
self._rx(self._acc_state_msg(should_turn_acc_main_on))
self.assertEqual(should_turn_acc_main_on, self.safety.get_acc_main_on())
@parameterized_class(LDA_BUTTON)
class TestHyundaiSafetyFCEV(TestHyundaiSafety):
@classmethod
def setUpClass(cls):
if cls.__name__ == "TestHyundaiSafetyFCEV":
cls.safety = None
raise unittest.SkipTest
def setUp(self):
self.packer = CANPackerSafety("hyundai_can_generated")
self.safety = libsafety_py.libsafety
self.safety.set_current_safety_param_sp(self.SAFETY_PARAM_SP)
self.safety.set_safety_hooks(CarParams.SafetyModel.hyundai, HyundaiSafetyFlags.FCEV_GAS)
self.safety.init_tests()
def _user_gas_msg(self, gas):
values = {"ACCELERATOR_PEDAL": gas}
return self.packer.make_can_msg_safety("FCEV_ACCELERATOR", 0, values)
class TestHyundaiLegacySafety(TestHyundaiSafety):
def setUp(self):
self.packer = CANPackerSafety("hyundai_can_generated")
self.safety = libsafety_py.libsafety
self.safety.set_safety_hooks(CarParams.SafetyModel.hyundaiLegacy, 0)
self.safety.init_tests()
class TestHyundaiLegacySafetyEV(TestHyundaiSafety):
def setUp(self):
self.packer = CANPackerSafety("hyundai_can_generated")
self.safety = libsafety_py.libsafety
self.safety.set_safety_hooks(CarParams.SafetyModel.hyundaiLegacy, HyundaiSafetyFlags.EV_GAS)
self.safety.init_tests()
def _user_gas_msg(self, gas):
values = {"Accel_Pedal_Pos": gas}
return self.packer.make_can_msg_safety("E_EMS11", 0, values, fix_checksum=checksum)
class TestHyundaiLegacySafetyHEV(TestHyundaiSafety):
def setUp(self):
self.packer = CANPackerSafety("hyundai_can_generated")
self.safety = libsafety_py.libsafety
self.safety.set_safety_hooks(CarParams.SafetyModel.hyundaiLegacy, HyundaiSafetyFlags.HYBRID_GAS)
self.safety.init_tests()
def _user_gas_msg(self, gas):
values = {"CR_Vcu_AccPedDep_Pos": gas}
return self.packer.make_can_msg_safety("E_EMS11", 0, values, fix_checksum=checksum)
@parameterized_class(LDA_BUTTON)
class TestHyundaiLongitudinalSafety(HyundaiLongitudinalBase, TestHyundaiSafety):
TX_MSGS = [[0x340, 0], [0x4F1, 0], [0x485, 0], [0x420, 0], [0x421, 0], [0x50A, 0], [0x389, 0], [0x4A2, 0], [0x38D, 0], [0x483, 0], [0x7D0, 0]]
FWD_BLACKLISTED_ADDRS = {2: [0x340, 0x485, 0x421, 0x420, 0x50A, 0x389]}
RELAY_MALFUNCTION_ADDRS = {0: (0x340, 0x485, 0x421, 0x420, 0x50A, 0x389)} # LKAS11, LFAHDA_MFC, SCC12, SCC11, SCC13, SCC14
DISABLED_ECU_UDS_MSG = (0x7D0, 0)
DISABLED_ECU_ACTUATION_MSG = (0x421, 0)
@classmethod
def setUpClass(cls):
if cls.__name__ == "TestHyundaiLongitudinalSafety":
cls.safety = None
raise unittest.SkipTest
def setUp(self):
self.packer = CANPackerSafety("hyundai_can_generated")
self.safety = libsafety_py.libsafety
self.safety.set_current_safety_param_sp(self.SAFETY_PARAM_SP)
self.safety.set_safety_hooks(CarParams.SafetyModel.hyundai, HyundaiSafetyFlags.LONG)
self.safety.init_tests()
def _accel_msg(self, accel, aeb_req=False, aeb_decel=0):
values = {
"aReqRaw": accel,
"aReqValue": accel,
"AEB_CmdAct": int(aeb_req),
"CR_VSM_DecCmd": aeb_decel,
}
return self.packer.make_can_msg_safety("SCC12", self.SCC_BUS, values)
def _fca11_msg(self, idx=0, vsm_aeb_req=False, fca_aeb_req=False, aeb_decel=0):
values = {
"CR_FCA_Alive": idx % 0xF,
"FCA_Status": 2,
"CR_VSM_DecCmd": aeb_decel,
"CF_VSM_DecCmdAct": int(vsm_aeb_req),
"FCA_CmdAct": int(fca_aeb_req),
}
return self.packer.make_can_msg_safety("FCA11", 0, values)
def _tx_acc_state_msg(self, enable):
values = {"MainMode_ACC": enable}
return self.packer.make_can_msg_safety("SCC11", 0, values)
def test_no_aeb_fca11(self):
self.assertTrue(self._tx(self._fca11_msg()))
self.assertFalse(self._tx(self._fca11_msg(vsm_aeb_req=True)))
self.assertFalse(self._tx(self._fca11_msg(fca_aeb_req=True)))
self.assertFalse(self._tx(self._fca11_msg(aeb_decel=1.0)))
def test_no_aeb_scc12(self):
self.assertTrue(self._tx(self._accel_msg(0)))
self.assertFalse(self._tx(self._accel_msg(0, aeb_req=True)))
self.assertFalse(self._tx(self._accel_msg(0, aeb_decel=1.0)))
class TestHyundaiLongitudinalSafetyCameraSCC(HyundaiLongitudinalBase, TestHyundaiSafety):
TX_MSGS = [[0x340, 0], [0x4F1, 2], [0x485, 0], [0x420, 0], [0x421, 0], [0x50A, 0], [0x389, 0], [0x4A2, 0]]
FWD_BLACKLISTED_ADDRS = {2: [0x340, 0x485, 0x420, 0x421, 0x50A, 0x389]}
RELAY_MALFUNCTION_ADDRS = {0: (0x340, 0x485, 0x421, 0x420, 0x50A, 0x389)} # LKAS11, LFAHDA_MFC, SCC12, SCC11, SCC13, SCC14
def setUp(self):
self.packer = CANPackerSafety("hyundai_can_generated")
self.safety = libsafety_py.libsafety
self.safety.set_current_safety_param_sp(HyundaiSafetyFlagsSP.HAS_LDA_BUTTON)
self.safety.set_safety_hooks(CarParams.SafetyModel.hyundai, HyundaiSafetyFlags.LONG | HyundaiSafetyFlags.CAMERA_SCC)
self.safety.init_tests()
def _accel_msg(self, accel, aeb_req=False, aeb_decel=0):
values = {
"aReqRaw": accel,
"aReqValue": accel,
"AEB_CmdAct": int(aeb_req),
"CR_VSM_DecCmd": aeb_decel,
}
return self.packer.make_can_msg_safety("SCC12", self.SCC_BUS, values)
def _tx_acc_state_msg(self, enable):
values = {"MainMode_ACC": enable}
return self.packer.make_can_msg_safety("SCC11", self.SCC_BUS, values)
def test_no_aeb_scc12(self):
self.assertTrue(self._tx(self._accel_msg(0)))
self.assertFalse(self._tx(self._accel_msg(0, aeb_req=True)))
self.assertFalse(self._tx(self._accel_msg(0, aeb_decel=1.0)))
def test_tester_present_allowed(self):
pass
def test_disabled_ecu_alive(self):
pass
@parameterized_class(LDA_BUTTON)
class TestHyundaiSafetyFCEVLong(TestHyundaiLongitudinalSafety, TestHyundaiSafetyFCEV):
@classmethod
def setUpClass(cls):
if cls.__name__ == "TestHyundaiSafetyFCEVLong":
cls.safety = None
raise unittest.SkipTest
def setUp(self):
self.packer = CANPackerSafety("hyundai_can_generated")
self.safety = libsafety_py.libsafety
self.safety.set_current_safety_param_sp(self.SAFETY_PARAM_SP)
self.safety.set_safety_hooks(CarParams.SafetyModel.hyundai, HyundaiSafetyFlags.FCEV_GAS | HyundaiSafetyFlags.LONG)
self.safety.init_tests()
@parameterized_class(LDA_BUTTON)
class TestHyundaiLongitudinalESCCSafety(HyundaiLongitudinalBase, TestHyundaiSafety):
TX_MSGS = [[0x340, 0], [0x4F1, 0], [0x485, 0], [0x420, 0], [0x421, 0], [0x50A, 0], [0x389, 0]]
FWD_BLACKLISTED_ADDRS = {2: [0x340, 0x485, 0x420, 0x421, 0x50A, 0x389]}
RELAY_MALFUNCTION_ADDRS = {0: (0x340, 0x485, 0x420, 0x421, 0x50A, 0x389)} # LKAS11, LFAHDA_MFC, SCC12, SCC11, SCC13, SCC14
@classmethod
def setUpClass(cls):
if cls.__name__ == "TestHyundaiLongitudinalESCCSafety":
cls.safety = None
raise unittest.SkipTest
def setUp(self):
self.packer = CANPackerSafety("hyundai_can_generated")
self.safety = libsafety_py.libsafety
self.safety.set_current_safety_param_sp(HyundaiSafetyFlagsSP.ESCC | self.SAFETY_PARAM_SP)
self.safety.set_safety_hooks(CarParams.SafetyModel.hyundai, HyundaiSafetyFlags.LONG)
self.safety.init_tests()
def _accel_msg(self, accel, aeb_req=False, aeb_decel=0):
values = {
"aReqRaw": accel,
"aReqValue": accel,
}
return self.packer.make_can_msg_safety("SCC12", self.SCC_BUS, values)
def _tx_acc_state_msg(self, enable):
values = {"MainMode_ACC": enable}
return self.packer.make_can_msg_safety("SCC11", 0, values)
def test_tester_present_allowed(self):
pass
def test_disabled_ecu_alive(self):
pass
@parameterized_class(LDA_BUTTON)
class TestHyundaiNonSCCSafety(TestHyundaiSafety):
@classmethod
def setUpClass(cls):
if cls.__name__ == "TestHyundaiNonSCCSafety":
cls.safety = None
raise unittest.SkipTest
def setUp(self):
self.packer = CANPackerSafety("hyundai_can_generated")
self.safety = libsafety_py.libsafety
self.safety.set_current_safety_param_sp(HyundaiSafetyFlagsSP.NON_SCC | self.SAFETY_PARAM_SP)
self.safety.set_safety_hooks(CarParams.SafetyModel.hyundai, 0)
self.safety.init_tests()
def _pcm_status_msg(self, enable):
values = {"CRUISE_LAMP_S": enable, "AliveCounter": self.cnt_gas % 4}
self.__class__.cnt_gas += 1
return self.packer.make_can_msg_safety("EMS16", 0, values, fix_checksum=checksum)
def _acc_state_msg(self, enable):
values = {"CRUISE_LAMP_M": enable, "AliveCounter": self.cnt_gas % 4}
self.__class__.cnt_gas += 1
return self.packer.make_can_msg_safety("EMS16", 0, values, fix_checksum=checksum)
def _user_gas_msg(self, gas: float, controls_allowed: bool = True):
values = {"CF_Ems_AclAct": gas, "CRUISE_LAMP_M": 1, "CRUISE_LAMP_S": controls_allowed, "AliveCounter": self.cnt_gas % 4}
self.__class__.cnt_gas += 1
return self.packer.make_can_msg_safety("EMS16", 0, values, fix_checksum=checksum)
def test_allow_engage_with_gas_pressed(self):
self._rx(self._user_gas_msg(1, self.safety.get_controls_allowed()))
self.safety.set_controls_allowed(True)
self._rx(self._user_gas_msg(1, self.safety.get_controls_allowed()))
self.assertTrue(self.safety.get_controls_allowed())
self._rx(self._user_gas_msg(1, self.safety.get_controls_allowed()))
self.assertTrue(self.safety.get_controls_allowed())
def test_no_disengage_on_gas(self):
self._rx(self._user_gas_msg(0, self.safety.get_controls_allowed()))
self.safety.set_controls_allowed(True)
self._rx(self._user_gas_msg(self.GAS_PRESSED_THRESHOLD + 1, self.safety.get_controls_allowed()))
# Test we allow lateral, but not longitudinal
self.assertTrue(self.safety.get_controls_allowed())
self.assertFalse(self.safety.get_longitudinal_allowed())
# Make sure we can re-gain longitudinal actuation
self._rx(self._user_gas_msg(0, self.safety.get_controls_allowed()))
self.assertTrue(self.safety.get_longitudinal_allowed())
@parameterized_class(ALL_NON_SCC_HEV_EV_COMBOS)
class TestHyundaiNonSCCSafety_HEV_EV(TestHyundaiSafety):
PCM_STATUS_MSG = ("", "")
ACC_STATE_MSG = ("", "")
GAS_MSG = ("", "")
SAFETY_PARAM = 0
@classmethod
def setUpClass(cls):
if cls.__name__ == "TestHyundaiNonSCCSafety_HEV_EV":
cls.safety = None
raise unittest.SkipTest
def setUp(self):
self.packer = CANPackerSafety("hyundai_can_generated")
self.safety = libsafety_py.libsafety
self.safety.set_current_safety_param_sp(HyundaiSafetyFlagsSP.NON_SCC | self.SAFETY_PARAM_SP)
self.safety.set_safety_hooks(CarParams.SafetyModel.hyundai, self.SAFETY_PARAM)
self.safety.init_tests()
def _pcm_status_msg(self, enable):
values = {self.PCM_STATUS_MSG[1]: enable}
return self.packer.make_can_msg_safety(self.PCM_STATUS_MSG[0], 0, values)
def _acc_state_msg(self, enable):
values = {self.ACC_STATE_MSG[1]: enable}
return self.packer.make_can_msg_safety(self.ACC_STATE_MSG[0], 0, values)
def _user_gas_msg(self, gas):
values = {self.GAS_MSG[1]: gas}
return self.packer.make_can_msg_safety(self.GAS_MSG[0], 0, values, fix_checksum=checksum)
if __name__ == "__main__":
unittest.main()
+312
View File
@@ -0,0 +1,312 @@
#!/usr/bin/env python3
from opendbc.testing import parameterized_class
import unittest
from opendbc.car.hyundai.values import HyundaiSafetyFlags
from opendbc.car.structs import CarParams
from opendbc.safety.tests.libsafety import libsafety_py
import opendbc.safety.tests.common as common
from opendbc.safety.tests.common import CANPackerSafety
from opendbc.safety.tests.hyundai_common import HyundaiButtonBase, HyundaiLongitudinalBase
# All combinations of radar/camera-SCC and gas/hybrid/EV cars
ALL_GAS_EV_HYBRID_COMBOS = [
# Radar SCC
{"GAS_MSG": ("ACCELERATOR_BRAKE_ALT", "ACCELERATOR_PEDAL_PRESSED"), "SCC_BUS": 0, "SAFETY_PARAM": 0},
{"GAS_MSG": ("ACCELERATOR", "ACCELERATOR_PEDAL"), "SCC_BUS": 0, "SAFETY_PARAM": HyundaiSafetyFlags.EV_GAS},
{"GAS_MSG": ("ACCELERATOR_ALT", "ACCELERATOR_PEDAL"), "SCC_BUS": 0, "SAFETY_PARAM": HyundaiSafetyFlags.HYBRID_GAS},
# Camera SCC
{"GAS_MSG": ("ACCELERATOR_BRAKE_ALT", "ACCELERATOR_PEDAL_PRESSED"), "SCC_BUS": 2, "SAFETY_PARAM": HyundaiSafetyFlags.CAMERA_SCC},
{"GAS_MSG": ("ACCELERATOR", "ACCELERATOR_PEDAL"), "SCC_BUS": 2, "SAFETY_PARAM": HyundaiSafetyFlags.EV_GAS | HyundaiSafetyFlags.CAMERA_SCC},
{"GAS_MSG": ("ACCELERATOR_ALT", "ACCELERATOR_PEDAL"), "SCC_BUS": 2, "SAFETY_PARAM": HyundaiSafetyFlags.HYBRID_GAS | HyundaiSafetyFlags.CAMERA_SCC},
]
class TestHyundaiCanfdBase(HyundaiButtonBase, common.CarSafetyTest, common.DriverTorqueSteeringSafetyTest, common.SteerRequestCutSafetyTest):
TX_MSGS = [[0x50, 0], [0x1CF, 1], [0x2A4, 0]]
STANDSTILL_THRESHOLD = 12 # 0.375 kph
FWD_BLACKLISTED_ADDRS = {2: [0x50, 0x2a4]}
MAX_RATE_UP = 2
MAX_RATE_DOWN = 3
MAX_TORQUE_LOOKUP = [0], [270]
MAX_RT_DELTA = 112
DRIVER_TORQUE_ALLOWANCE = 250
DRIVER_TORQUE_FACTOR = 2
# Safety around steering req bit
MIN_VALID_STEERING_FRAMES = 89
MAX_INVALID_STEERING_FRAMES = 2
PT_BUS = 0
SCC_BUS = 2
STEER_BUS = 0
STEER_MSG = ""
GAS_MSG = ("", "")
BUTTONS_TX_BUS = 1
def _torque_driver_msg(self, torque):
values = {"MDPS_StrTqSnsrVal": torque}
return self.packer.make_can_msg_safety("MDPS", self.PT_BUS, values)
def _torque_cmd_msg(self, torque, steer_req=1):
values = {"StrTqReqVal": torque, "ActToiSta": steer_req}
return self.packer.make_can_msg_safety(self.STEER_MSG, self.STEER_BUS, values)
def _speed_msg(self, speed):
values = {f"WHL_Spd{pos}Val": speed * 0.03125 for pos in ["FL", "FR", "RL", "RR"]}
return self.packer.make_can_msg_safety("WHEEL_SPEEDS", self.PT_BUS, values)
def _user_brake_msg(self, brake):
values = {"DriverBraking": brake}
return self.packer.make_can_msg_safety("TCS", self.PT_BUS, values)
def _user_gas_msg(self, gas):
values = {self.GAS_MSG[1]: gas}
return self.packer.make_can_msg_safety(self.GAS_MSG[0], self.PT_BUS, values)
def _pcm_status_msg(self, enable):
values = {"ACCMode": 1 if enable else 0}
return self.packer.make_can_msg_safety("SCC_CONTROL", self.SCC_BUS, values)
def _button_msg(self, buttons, main_button=0, bus=None):
if bus is None:
bus = self.PT_BUS
values = {
"CRUISE_BUTTONS": buttons,
"ADAPTIVE_CRUISE_MAIN_BTN": main_button,
}
return self.packer.make_can_msg_safety("CRUISE_BUTTONS", bus, values)
def _acc_state_msg(self, enable):
values = {"MainMode_ACC": enable}
return self.packer.make_can_msg_safety("SCC_CONTROL", self.SCC_BUS, values)
def _lkas_button_msg(self, enabled):
values = {"LDA_BTN": enabled}
return self.packer.make_can_msg_safety("CRUISE_BUTTONS", self.PT_BUS, values)
def _main_cruise_button_msg(self, enabled):
return self._button_msg(0, enabled)
class TestHyundaiCanfdLFASteeringBase(TestHyundaiCanfdBase):
TX_MSGS = [[0x12A, 0], [0x1A0, 1], [0x1CF, 0], [0x1E0, 0]]
RELAY_MALFUNCTION_ADDRS = {0: (0x12A, 0x1E0)} # LFA, LFAHDA_CLUSTER
FWD_BLACKLISTED_ADDRS = {2: [0x12A, 0x1E0]}
STEER_MSG = "LFA"
BUTTONS_TX_BUS = 2
SAFETY_PARAM: int
@classmethod
def setUpClass(cls):
super().setUpClass()
if cls.__name__ in ("TestHyundaiCanfdLFASteering", "TestHyundaiCanfdLFASteeringAltButtons"):
cls.packer = None
cls.safety = None
raise unittest.SkipTest
def setUp(self):
self.packer = CANPackerSafety("hyundai_canfd_generated")
self.safety = libsafety_py.libsafety
self.safety.set_safety_hooks(CarParams.SafetyModel.hyundaiCanfd, self.SAFETY_PARAM)
self.safety.init_tests()
@parameterized_class(ALL_GAS_EV_HYBRID_COMBOS)
class TestHyundaiCanfdLFASteering(TestHyundaiCanfdLFASteeringBase):
pass
class TestHyundaiCanfdLFASteeringAltButtonsBase(TestHyundaiCanfdLFASteeringBase):
SAFETY_PARAM: int
def setUp(self):
self.packer = CANPackerSafety("hyundai_canfd_generated")
self.safety = libsafety_py.libsafety
self.safety.set_safety_hooks(CarParams.SafetyModel.hyundaiCanfd, HyundaiSafetyFlags.CANFD_ALT_BUTTONS | self.SAFETY_PARAM)
self.safety.init_tests()
def _button_msg(self, buttons, main_button=0, bus=1):
values = {
"CRUISE_BUTTONS": buttons,
"ADAPTIVE_CRUISE_MAIN_BTN": main_button,
}
return self.packer.make_can_msg_safety("CRUISE_BUTTONS_ALT", self.PT_BUS, values)
def _lkas_button_msg(self, enabled):
values = {"LDA_BTN": enabled}
return self.packer.make_can_msg_safety("CRUISE_BUTTONS_ALT", self.PT_BUS, values)
def _acc_cancel_msg(self, cancel, accel=0):
values = {"ACCMode": 4 if cancel else 0, "aReqRaw": accel, "aReqValue": accel}
return self.packer.make_can_msg_safety("SCC_CONTROL", self.PT_BUS, values)
def test_button_sends(self):
"""
No button send allowed with alt buttons.
"""
for enabled in (True, False):
for btn in range(8):
self.safety.set_controls_allowed(enabled)
self.assertFalse(self._tx(self._button_msg(btn)))
def test_acc_cancel(self):
# FIXME: the CANFD_ALT_BUTTONS cars are the only ones that use SCC_CONTROL to cancel, why can't we use buttons?
for enabled in (True, False):
self.safety.set_controls_allowed(enabled)
self.assertTrue(self._tx(self._acc_cancel_msg(True)))
self.assertFalse(self._tx(self._acc_cancel_msg(True, accel=1)))
self.assertFalse(self._tx(self._acc_cancel_msg(False)))
@parameterized_class(ALL_GAS_EV_HYBRID_COMBOS)
class TestHyundaiCanfdLFASteeringAltButtons(TestHyundaiCanfdLFASteeringAltButtonsBase):
pass
class TestHyundaiCanfdLKASteeringEV(TestHyundaiCanfdBase):
TX_MSGS = [[0x50, 0], [0x1CF, 1], [0x2A4, 0]]
RELAY_MALFUNCTION_ADDRS = {0: (0x50, 0x2a4)} # LKAS, CAM_0x2A4
FWD_BLACKLISTED_ADDRS = {2: [0x50, 0x2a4]}
PT_BUS = 1
SCC_BUS = 1
STEER_MSG = "LKAS"
GAS_MSG = ("ACCELERATOR", "ACCELERATOR_PEDAL")
def setUp(self):
self.packer = CANPackerSafety("hyundai_canfd_generated")
self.safety = libsafety_py.libsafety
self.safety.set_safety_hooks(CarParams.SafetyModel.hyundaiCanfd, HyundaiSafetyFlags.CANFD_LKA_STEER_MSG | HyundaiSafetyFlags.EV_GAS)
self.safety.init_tests()
# TODO: Handle ICE and HEV configurations once we see cars that use the new messages
class TestHyundaiCanfdLKASteeringAltEV(TestHyundaiCanfdBase):
TX_MSGS = [[0x110, 0], [0x1CF, 1], [0x362, 0]]
RELAY_MALFUNCTION_ADDRS = {0: (0x110, 0x362)} # LKAS_ALT, CAM_0x362
FWD_BLACKLISTED_ADDRS = {2: [0x110, 0x362]}
PT_BUS = 1
SCC_BUS = 1
STEER_MSG = "LKAS_ALT"
GAS_MSG = ("ACCELERATOR", "ACCELERATOR_PEDAL")
def setUp(self):
self.packer = CANPackerSafety("hyundai_canfd_generated")
self.safety = libsafety_py.libsafety
self.safety.set_safety_hooks(CarParams.SafetyModel.hyundaiCanfd, HyundaiSafetyFlags.CANFD_LKA_STEER_MSG | HyundaiSafetyFlags.EV_GAS |
HyundaiSafetyFlags.CANFD_LKA_STEER_MSG_ALT)
self.safety.init_tests()
class TestHyundaiCanfdLKASteeringLongEV(HyundaiLongitudinalBase, TestHyundaiCanfdLKASteeringEV):
TX_MSGS = [[0x50, 0], [0x1CF, 1], [0x2A4, 0], [0x51, 0], [0x730, 1], [0x12a, 1], [0x160, 1],
[0x1e0, 1], [0x1a0, 1], [0x1ea, 1], [0x200, 1], [0x345, 1], [0x1da, 1]]
RELAY_MALFUNCTION_ADDRS = {0: (0x50, 0x2a4), 1: (0x1a0,)} # LKAS, CAM_0x2A4, SCC_CONTROL
DISABLED_ECU_UDS_MSG = (0x730, 1)
DISABLED_ECU_ACTUATION_MSG = (0x1a0, 1)
STEER_MSG = "LFA"
GAS_MSG = ("ACCELERATOR", "ACCELERATOR_PEDAL")
STEER_BUS = 1
def setUp(self):
self.packer = CANPackerSafety("hyundai_canfd_generated")
self.safety = libsafety_py.libsafety
self.safety.set_safety_hooks(CarParams.SafetyModel.hyundaiCanfd, HyundaiSafetyFlags.CANFD_LKA_STEER_MSG |
HyundaiSafetyFlags.LONG | HyundaiSafetyFlags.EV_GAS)
self.safety.init_tests()
def _accel_msg(self, accel, aeb_req=False, aeb_decel=0):
values = {
"aReqRaw": accel,
"aReqValue": accel,
}
return self.packer.make_can_msg_safety("SCC_CONTROL", self.PT_BUS, values)
def _tx_acc_state_msg(self, enable):
values = {"MainMode_ACC": enable}
return self.packer.make_can_msg_safety("SCC_CONTROL", self.PT_BUS, values)
# Tests longitudinal for ICE, hybrid, EV cars with LFA steering
class TestHyundaiCanfdLFASteeringLongBase(HyundaiLongitudinalBase, TestHyundaiCanfdLFASteeringBase):
FWD_BLACKLISTED_ADDRS = {2: [0x12a, 0x1e0, 0x1a0, 0x160]}
RELAY_MALFUNCTION_ADDRS = {0: (0x12A, 0x1E0, 0x1a0, 0x160)} # LFA, LFAHDA_CLUSTER, SCC_CONTROL, ADRV_0x160
DISABLED_ECU_UDS_MSG = (0x7D0, 0)
DISABLED_ECU_ACTUATION_MSG = (0x1a0, 0)
@classmethod
def setUpClass(cls):
if cls.__name__ == "TestHyundaiCanfdLFASteeringLongBase":
cls.safety = None
raise unittest.SkipTest
def setUp(self):
self.packer = CANPackerSafety("hyundai_canfd_generated")
self.safety = libsafety_py.libsafety
self.safety.set_safety_hooks(CarParams.SafetyModel.hyundaiCanfd, HyundaiSafetyFlags.LONG | self.SAFETY_PARAM)
self.safety.init_tests()
def _accel_msg(self, accel, aeb_req=False, aeb_decel=0):
values = {
"aReqRaw": accel,
"aReqValue": accel,
}
return self.packer.make_can_msg_safety("SCC_CONTROL", self.PT_BUS, values)
def _tx_acc_state_msg(self, enable):
values = {"MainMode_ACC": enable}
return self.packer.make_can_msg_safety("SCC_CONTROL", self.PT_BUS, values)
# no knockout
def test_tester_present_allowed(self):
pass
@parameterized_class(ALL_GAS_EV_HYBRID_COMBOS)
class TestHyundaiCanfdLFASteeringLong(TestHyundaiCanfdLFASteeringLongBase):
@classmethod
def setUpClass(cls):
if cls.__name__ == "TestHyundaiCanfdLFASteeringLong":
cls.safety = None
raise unittest.SkipTest
@parameterized_class(ALL_GAS_EV_HYBRID_COMBOS)
class TestHyundaiCanfdLFASteeringLongAltButtons(TestHyundaiCanfdLFASteeringLongBase, TestHyundaiCanfdLFASteeringAltButtonsBase):
@classmethod
def setUpClass(cls):
if cls.__name__ == "TestHyundaiCanfdLFASteeringLongAltButtons":
cls.safety = None
raise unittest.SkipTest
def setUp(self):
self.packer = CANPackerSafety("hyundai_canfd_generated")
self.safety = libsafety_py.libsafety
self.safety.set_safety_hooks(CarParams.SafetyModel.hyundaiCanfd, HyundaiSafetyFlags.LONG | HyundaiSafetyFlags.CANFD_ALT_BUTTONS | self.SAFETY_PARAM)
self.safety.init_tests()
def test_acc_cancel(self):
# Alt buttons does not use SCC_CONTROL to cancel if longitudinal
pass
if __name__ == "__main__":
unittest.main()
+107
View File
@@ -0,0 +1,107 @@
#!/usr/bin/env python3
import unittest
from opendbc.car.structs import CarParams
from opendbc.safety.tests.libsafety import libsafety_py
import opendbc.safety.tests.common as common
from opendbc.safety.tests.common import CANPackerSafety, make_msg
class TestMazdaSafety(common.CarSafetyTest, common.DriverTorqueSteeringSafetyTest):
TX_MSGS = [[0x243, 0], [0x09d, 0], [0x440, 0]]
STANDSTILL_THRESHOLD = .1
RELAY_MALFUNCTION_ADDRS = {0: (0x243, 0x440)}
FWD_BLACKLISTED_ADDRS = {2: [0x243, 0x440]}
MAX_RATE_UP = 10
MAX_RATE_DOWN = 25
MAX_TORQUE_LOOKUP = [0], [800]
MAX_RT_DELTA = 300
DRIVER_TORQUE_ALLOWANCE = 15
DRIVER_TORQUE_FACTOR = 1
# Mazda actually does not set any bit when requesting torque
NO_STEER_REQ_BIT = True
def setUp(self):
self.packer = CANPackerSafety("mazda_2017")
self.safety = libsafety_py.libsafety
self.safety.set_safety_hooks(CarParams.SafetyModel.mazda, 0)
self.safety.init_tests()
def _torque_meas_msg(self, torque):
values = {"STEER_TORQUE_MOTOR": torque}
return self.packer.make_can_msg_safety("STEER_TORQUE", 0, values)
def _torque_driver_msg(self, torque):
values = {"STEER_TORQUE_SENSOR": torque}
return self.packer.make_can_msg_safety("STEER_TORQUE", 0, values)
def _torque_cmd_msg(self, torque, steer_req=1):
values = {"LKAS_REQUEST": torque}
return self.packer.make_can_msg_safety("CAM_LKAS", 0, values)
def _speed_msg(self, speed):
values = {"SPEED": speed}
return self.packer.make_can_msg_safety("ENGINE_DATA", 0, values)
def _user_brake_msg(self, brake):
values = {"BRAKE_ON": brake}
return self.packer.make_can_msg_safety("PEDALS", 0, values)
def _user_gas_msg(self, gas):
values = {"PEDAL_GAS": gas}
return self.packer.make_can_msg_safety("ENGINE_DATA", 0, values)
def _pcm_status_msg(self, enable):
values = {"CRZ_ACTIVE": enable}
return self.packer.make_can_msg_safety("CRZ_CTRL", 0, values)
def _button_msg(self, resume=False, cancel=False):
values = {
"CAN_OFF": cancel,
"CAN_OFF_INV": (cancel + 1) % 2,
"RES": resume,
"RES_INV": (resume + 1) % 2,
}
return self.packer.make_can_msg_safety("CRZ_BTNS", 0, values)
def test_buttons(self):
# only cancel allows while controls not allowed
self.safety.set_controls_allowed(0)
self.assertTrue(self._tx(self._button_msg(cancel=True)))
self.assertFalse(self._tx(self._button_msg(resume=True)))
# do not block resume if we are engaged already
self.safety.set_controls_allowed(1)
self.assertTrue(self._tx(self._button_msg(cancel=True)))
self.assertTrue(self._tx(self._button_msg(resume=True)))
class TestMazdaIgnition(unittest.TestCase):
TX_MSGS: list = []
def setUp(self):
self.safety = libsafety_py.libsafety
self.safety.init_tests()
def _msg(self, byte0):
return make_msg(0, 0x9E, dat=bytes([byte0]) + b"\x00" * 7)
# 0x9E byte 0 high 3 bits == 6 (0xC0)
def test_ignition_on(self):
self.safety.ignition_can_hook(self._msg(0xC0))
self.assertTrue(self.safety.get_ignition_can())
def test_ignition_off(self):
self.safety.ignition_can_hook(self._msg(0xC0))
self.assertTrue(self.safety.get_ignition_can())
self.safety.ignition_can_hook(self._msg(0x20))
self.assertFalse(self.safety.get_ignition_can())
if __name__ == "__main__":
unittest.main()
+152
View File
@@ -0,0 +1,152 @@
#!/usr/bin/env python3
import unittest
from opendbc.car.nissan.values import NissanSafetyFlags
from opendbc.sunnypilot.car.nissan.values import NissanSafetyFlagsSP
from opendbc.car.structs import CarParams
from opendbc.safety.tests.libsafety import libsafety_py
import opendbc.safety.tests.common as common
from opendbc.safety.tests.common import CANPackerSafety
class TestNissanSafety(common.CarSafetyTest, common.AngleSteeringSafetyTest):
TX_MSGS = [[0x169, 0], [0x2b1, 0], [0x4cc, 0], [0x20b, 2]]
GAS_PRESSED_THRESHOLD = 3
RELAY_MALFUNCTION_ADDRS = {0: (0x169, 0x2b1, 0x4cc)}
FWD_BLACKLISTED_ADDRS = {2: [0x169, 0x2b1, 0x4cc]}
EPS_BUS = 0
CRUISE_BUS = 2
ACC_MAIN_BUS = 1
# Angle control limits
STEER_ANGLE_MAX = 600 # deg, reasonable limit
DEG_TO_CAN = 100
ANGLE_RATE_BP = [0., 5., 15.]
ANGLE_RATE_UP = [5., .8, .15] # windup limit
ANGLE_RATE_DOWN = [5., 3.5, .4] # unwind limit
def setUp(self):
self.packer = CANPackerSafety("nissan_x_trail_2017_generated")
self.safety = libsafety_py.libsafety
self.safety.set_safety_hooks(CarParams.SafetyModel.nissan, 0)
self.safety.init_tests()
def _angle_cmd_msg(self, angle: float, enabled: bool):
values = {"DESIRED_ANGLE": angle, "LKA_ACTIVE": 1 if enabled else 0}
return self.packer.make_can_msg_safety("LKAS", 0, values)
def _angle_meas_msg(self, angle: float):
values = {"STEER_ANGLE": angle}
return self.packer.make_can_msg_safety("STEER_TORQUE_SENSOR", 0, values)
def _pcm_status_msg(self, enable):
values = {"CRUISE_ENABLED": enable}
return self.packer.make_can_msg_safety("CRUISE_STATE", self.CRUISE_BUS, values)
def _speed_msg(self, speed):
values = {"WHEEL_SPEED_%s" % s: speed * 3.6 for s in ["RR", "RL"]}
return self.packer.make_can_msg_safety("WHEEL_SPEEDS_REAR", self.EPS_BUS, values)
def _user_brake_msg(self, brake):
values = {"USER_BRAKE_PRESSED": brake}
return self.packer.make_can_msg_safety("DOORS_LIGHTS", self.EPS_BUS, values)
def _user_gas_msg(self, gas):
values = {"GAS_PEDAL": gas}
return self.packer.make_can_msg_safety("GAS_PEDAL", self.EPS_BUS, values)
def _acc_state_msg(self, main_on):
values = {"CRUISE_ON": main_on}
return self.packer.make_can_msg_safety("PRO_PILOT", self.ACC_MAIN_BUS, values)
def _acc_button_cmd(self, cancel=0, propilot=0, flw_dist=0, _set=0, res=0):
no_button = not any([cancel, propilot, flw_dist, _set, res])
values = {"CANCEL_BUTTON": cancel, "PROPILOT_BUTTON": propilot,
"FOLLOW_DISTANCE_BUTTON": flw_dist, "SET_BUTTON": _set,
"RES_BUTTON": res, "NO_BUTTON_PRESSED": no_button}
return self.packer.make_can_msg_safety("CRUISE_THROTTLE", self.CRUISE_BUS, values)
def test_acc_buttons(self):
btns = [
("cancel", True),
("propilot", False),
("flw_dist", False),
("_set", False),
("res", False),
(None, False),
]
for controls_allowed in (True, False):
for btn, should_tx in btns:
self.safety.set_controls_allowed(controls_allowed)
args = {} if btn is None else {btn: 1}
tx = self._tx(self._acc_button_cmd(**args))
self.assertEqual(tx, should_tx)
class TestNissanSafetyAltEpsBus(TestNissanSafety):
"""Altima uses different buses"""
TX_MSGS = [[0x169, 0], [0x2b1, 0], [0x4cc, 0], [0x20b, 1]]
RELAY_MALFUNCTION_ADDRS = {0: (0x169, 0x2b1, 0x4cc)}
FWD_BLACKLISTED_ADDRS = {2: [0x169, 0x2b1, 0x4cc]}
EPS_BUS = 1
CRUISE_BUS = 1
ACC_MAIN_BUS = 2
def setUp(self):
self.packer = CANPackerSafety("nissan_x_trail_2017_generated")
self.safety = libsafety_py.libsafety
self.safety.set_safety_hooks(CarParams.SafetyModel.nissan, NissanSafetyFlags.ALT_EPS_BUS)
self.safety.init_tests()
class TestNissanLeafSafety(TestNissanSafety):
TX_MSGS = [[0x169, 0], [0x2b1, 0], [0x4cc, 0], [0x239, 2]]
RELAY_MALFUNCTION_ADDRS = {0: (0x169, 0x2b1, 0x4cc), 2: (0x239,)}
FWD_BLACKLISTED_ADDRS = {0: [0x239], 2: [0x169, 0x2b1, 0x4cc]}
def setUp(self):
self.packer = CANPackerSafety("nissan_leaf_2018_generated")
self.safety = libsafety_py.libsafety
self.safety.set_current_safety_param_sp(NissanSafetyFlagsSP.LEAF)
self.safety.set_safety_hooks(CarParams.SafetyModel.nissan, 0)
self.safety.init_tests()
def _user_brake_msg(self, brake):
values = {"USER_BRAKE_PRESSED": brake}
return self.packer.make_can_msg_safety("CRUISE_THROTTLE", 0, values)
def _user_gas_msg(self, gas):
values = {"GAS_PEDAL": gas}
return self.packer.make_can_msg_safety("CRUISE_THROTTLE", 0, values)
def _acc_state_msg(self, main_on):
values = {"CRUISE_AVAILABLE": main_on}
return self.packer.make_can_msg_safety("CRUISE_THROTTLE", 0, values)
def test_acc_buttons(self):
# Leaf uses continuous CRUISE_THROTTLE forwarding (relay replacement),
# no button check in safety — all buttons pass through
btns = [
("cancel", True),
("propilot", True),
("flw_dist", True),
("_set", True),
("res", True),
(None, True),
]
for controls_allowed in (True, False):
for btn, should_tx in btns:
self.safety.set_controls_allowed(controls_allowed)
args = {} if btn is None else {btn: 1}
tx = self._tx(self._acc_button_cmd(**args))
self.assertEqual(tx, should_tx)
if __name__ == "__main__":
unittest.main()
@@ -0,0 +1,90 @@
#!/usr/bin/env python3
import unittest
from opendbc.car.structs import CarParams
from opendbc.safety.tests.libsafety import libsafety_py
import opendbc.safety.tests.common as common
from opendbc.safety.tests.common import CANPackerSafety
LANE_KEEP_ASSIST = 0x3F2
class TestPsaSafetyBase(common.CarSafetyTest, common.AngleSteeringSafetyTest):
RELAY_MALFUNCTION_ADDRS = {0: (LANE_KEEP_ASSIST,)}
FWD_BLACKLISTED_ADDRS = {2: [LANE_KEEP_ASSIST]}
TX_MSGS = [[1010, 0]]
MAIN_BUS = 0
ADAS_BUS = 1
CAM_BUS = 2
STEER_ANGLE_MAX = 390
DEG_TO_CAN = 10
ANGLE_RATE_BP = [0., 5., 25.]
ANGLE_RATE_UP = [2.5, 1.5, .2]
ANGLE_RATE_DOWN = [5., 2., .3]
def setUp(self):
self.packer = CANPackerSafety("psa_aee2010_r3")
self.safety = libsafety_py.libsafety
self.safety.set_safety_hooks(CarParams.SafetyModel.psa, 0)
self.safety.init_tests()
def _angle_cmd_msg(self, angle: float, enabled: bool):
values = {"SET_ANGLE": angle, "TORQUE_FACTOR": 100 if enabled else 0}
return self.packer.make_can_msg_safety("LANE_KEEP_ASSIST", self.MAIN_BUS, values)
def _angle_meas_msg(self, angle: float):
values = {"ANGLE": angle}
return self.packer.make_can_msg_safety("STEERING_ALT", self.MAIN_BUS, values)
def _pcm_status_msg(self, enable):
values = {"RVV_ACC_ACTIVATION_REQ": enable}
return self.packer.make_can_msg_safety("HS2_DAT_MDD_CMD_452", self.ADAS_BUS, values)
def _speed_msg(self, speed):
values = {"VITESSE_VEHICULE_ROUES": speed * 3.6}
return self.packer.make_can_msg_safety("HS2_DYN_ABR_38D", self.MAIN_BUS, values)
def _user_brake_msg(self, brake):
values = {"P013_MainBrake": brake}
return self.packer.make_can_msg_safety("Dat_BSI", self.CAM_BUS, values)
def _user_gas_msg(self, gas):
values = {"P002_Com_rAPP": int(gas * 100)}
return self.packer.make_can_msg_safety("Dyn_CMM", self.MAIN_BUS, values)
def test_rx_hook(self):
# speed
for _ in range(10):
self.assertTrue(self._rx(self._speed_msg(0)))
msg = self._speed_msg(0)
# invalidate checksum
msg[0].data[5] = 0x00
self.assertFalse(self._rx(msg))
# cruise
for _ in range(10):
self.assertTrue(self._rx(self._pcm_status_msg(0)))
msg = self._pcm_status_msg(0)
# invalidate checksum
msg[0].data[5] = 0x00
self.assertFalse(self._rx(msg))
msg = self._pcm_status_msg(0)
# write to unused payload byte
msg[0].data[6] = 0xAB
self.assertTrue(self._rx(msg))
class TestPsaStockSafety(TestPsaSafetyBase):
def setUp(self):
self.packer = CANPackerSafety("psa_aee2010_r3")
self.safety = libsafety_py.libsafety
self.safety.set_safety_hooks(CarParams.SafetyModel.psa, 0)
self.safety.init_tests()
if __name__ == "__main__":
unittest.main()
@@ -0,0 +1,16 @@
#!/usr/bin/env python3
import unittest
from opendbc.safety.tests.libsafety.libsafety_py import _build_libsafety
class TestBuild(unittest.TestCase):
def test_development_build(self):
_build_libsafety(release=False)
def test_release_build(self):
_build_libsafety(release=True)
if __name__ == "__main__":
unittest.main()
+177
View File
@@ -0,0 +1,177 @@
#!/usr/bin/env python3
import unittest
from opendbc.car.structs import CarParams
from opendbc.safety.tests.libsafety import libsafety_py
import opendbc.safety.tests.common as common
from opendbc.safety.tests.common import CANPackerSafety
from opendbc.car.rivian.values import RivianSafetyFlags
from opendbc.car.rivian.riviancan import checksum as _checksum
def checksum(msg):
addr, dat, bus = msg
ret = bytearray(dat)
# ESP_Status
if addr == 0x208:
ret[0] = _checksum(ret[1:], 0x1D, 0xB1)
elif addr == 0x150:
ret[0] = _checksum(ret[1:], 0x1D, 0x9A)
return addr, ret, bus
class TestRivianSafetyBase(common.CarSafetyTest, common.DriverTorqueSteeringSafetyTest, common.LongitudinalAccelSafetyTest,
common.VehicleSpeedSafetyTest):
TX_MSGS = [[0x120, 0], [0x321, 2], [0x162, 2]]
RELAY_MALFUNCTION_ADDRS = {0: (0x120,), 2: (0x321, 0x162)}
FWD_BLACKLISTED_ADDRS = {0: [0x321, 0x162], 2: [0x120]}
MAX_TORQUE_LOOKUP = [9, 17], [350, 250]
DYNAMIC_MAX_TORQUE = True
MAX_RATE_UP = 3
MAX_RATE_DOWN = 5
MAX_RT_DELTA = 125
DRIVER_TORQUE_ALLOWANCE = 100
DRIVER_TORQUE_FACTOR = 2
cnt_speed = 0
cnt_speed_2 = 0
def _torque_driver_msg(self, torque):
values = {"EPAS_TorsionBarTorque": torque / 100.0}
return self.packer.make_can_msg_safety("EPAS_SystemStatus", 0, values)
def _torque_cmd_msg(self, torque, steer_req=1):
values = {"ACM_lkaStrToqReq": torque, "ACM_lkaActToi": steer_req}
return self.packer.make_can_msg_safety("ACM_lkaHbaCmd", 0, values)
def _speed_msg(self, speed, quality_flag=True):
values = {"ESP_Vehicle_Speed": speed * 3.6, "ESP_Status_Counter": self.cnt_speed % 15,
"ESP_Vehicle_Speed_Q": 1 if quality_flag else 0}
self.__class__.cnt_speed += 1
return self.packer.make_can_msg_safety("ESP_Status", 0, values, fix_checksum=checksum)
def _speed_msg_2(self, speed, quality_flag=True):
# Rivian has a dynamic max torque limit based on speed, so it checks two sources
return self._user_gas_msg(0, speed, quality_flag)
def _user_brake_msg(self, brake):
values = {"iBESP2_BrakePedalApplied": brake}
return self.packer.make_can_msg_safety("iBESP2", 0, values)
def _user_gas_msg(self, gas, speed=0, quality_flag=True):
values = {"VDM_AcceleratorPedalPosition": gas, "VDM_VehicleSpeed": speed * 3.6,
"VDM_PropStatus_Counter": self.cnt_speed_2 % 15, "VDM_VehicleSpeedQ": 1 if quality_flag else 0}
self.__class__.cnt_speed_2 += 1
return self.packer.make_can_msg_safety("VDM_PropStatus", 0, values, fix_checksum=checksum)
def _pcm_status_msg(self, enable):
values = {"ACM_FeatureStatus": enable, "ACM_Unkown1": 1}
return self.packer.make_can_msg_safety("ACM_Status", 2, values)
def _accel_msg(self, accel: float):
values = {"ACM_AccelerationRequest": accel}
return self.packer.make_can_msg_safety("ACM_longitudinalRequest", 0, values)
def test_wheel_touch(self):
# For hiding hold wheel alert on engage
for controls_allowed in (True, False):
self.safety.set_controls_allowed(controls_allowed)
values = {
"SCCM_WheelTouch_HandsOn": 1 if controls_allowed else 0,
"SCCM_WheelTouch_CapacitiveValue": 100 if controls_allowed else 0,
"SETME_X52": 100,
}
self.assertTrue(self._tx(self.packer.make_can_msg_safety("SCCM_WheelTouch", 2, values)))
def test_rx_hook(self):
# checksum, counter, and quality flag checks
for quality_flag in (True, False):
for msg_type in ("speed", "speed_2"):
self.safety.set_controls_allowed(True)
# send multiple times to verify counter checks
for _ in range(10):
if msg_type == "speed":
msg = self._speed_msg(0, quality_flag=quality_flag)
elif msg_type == "speed_2":
msg = self._speed_msg_2(0, quality_flag=quality_flag)
self.assertEqual(quality_flag, self._rx(msg))
self.assertEqual(quality_flag, self.safety.get_controls_allowed())
# Mess with checksum to make it fail
msg[0].data[0] = 0xff
self.assertFalse(self._rx(msg))
self.assertFalse(self.safety.get_controls_allowed())
class TestRivianStockSafety(TestRivianSafetyBase):
LONGITUDINAL = False
def setUp(self):
self.packer = CANPackerSafety("rivian_primary_actuator")
self.safety = libsafety_py.libsafety
self.safety.set_safety_hooks(CarParams.SafetyModel.rivian, 0)
self.safety.init_tests()
def test_adas_status(self):
# For canceling stock ACC
for controls_allowed in (True, False):
self.safety.set_controls_allowed(controls_allowed)
for interface_status in range(4):
values = {"VDM_AdasInterfaceStatus": interface_status}
self.assertTrue(self._tx(self.packer.make_can_msg_safety("VDM_AdasSts", 2, values)))
class TestRivianLongitudinalSafety(TestRivianSafetyBase):
TX_MSGS = [[0x120, 0], [0x321, 2], [0x160, 0]]
RELAY_MALFUNCTION_ADDRS = {0: (0x120, 0x160), 2: (0x321,)}
FWD_BLACKLISTED_ADDRS = {0: [0x321], 2: [0x120, 0x160]}
def setUp(self):
self.packer = CANPackerSafety("rivian_primary_actuator")
self.safety = libsafety_py.libsafety
self.safety.set_safety_hooks(CarParams.SafetyModel.rivian, RivianSafetyFlags.LONG_CONTROL)
self.safety.init_tests()
class TestRivianIgnition(unittest.TestCase):
TX_MSGS: list = []
def setUp(self):
self.safety = libsafety_py.libsafety
self.safety.init_tests()
self.packer = CANPackerSafety("rivian_primary_actuator")
def _msg(self, counter, mode):
return self.packer.make_can_msg_safety("VDM_OutputSignals", 0,
{"VDM_OutputSigs_Counter": counter,
"VDM_EpasPowerMode": mode})
# VDM_EpasPowerMode_Drive_On=1
def test_ignition_on(self):
for i in range(15):
self.safety.init_tests()
self.safety.ignition_can_hook(self._msg(i, 1))
self.assertFalse(self.safety.get_ignition_can())
self.safety.ignition_can_hook(self._msg((i + 1) % 15, 1))
self.assertTrue(self.safety.get_ignition_can())
def test_ignition_off(self):
self.safety.ignition_can_hook(self._msg(0, 1))
self.safety.ignition_can_hook(self._msg(1, 1))
self.assertTrue(self.safety.get_ignition_can())
self.safety.ignition_can_hook(self._msg(2, 0))
self.safety.ignition_can_hook(self._msg(3, 0))
self.assertFalse(self.safety.get_ignition_can())
if __name__ == "__main__":
unittest.main()
+252
View File
@@ -0,0 +1,252 @@
#!/usr/bin/env python3
import enum
import unittest
from opendbc.car.subaru.values import SubaruSafetyFlags
from opendbc.car.structs import CarParams
from opendbc.safety.tests.libsafety import libsafety_py
import opendbc.safety.tests.common as common
from opendbc.safety.tests.common import CANPackerSafety
from functools import partial
class SubaruMsg(enum.IntEnum):
Brake_Status = 0x13c
CruiseControl = 0x240
Throttle = 0x40
Steering_Torque = 0x119
Wheel_Speeds = 0x13a
ES_LKAS = 0x122
ES_LKAS_ANGLE = 0x124
ES_Brake = 0x220
ES_Distance = 0x221
ES_Status = 0x222
ES_DashStatus = 0x321
ES_LKAS_State = 0x322
ES_Infotainment = 0x323
ES_UDS_Request = 0x787
ES_HighBeamAssist = 0x22A
ES_STATIC_1 = 0x325
ES_STATIC_2 = 0x121
SUBARU_MAIN_BUS = 0
SUBARU_ALT_BUS = 1
SUBARU_CAM_BUS = 2
def lkas_tx_msgs(alt_bus, lkas_msg=SubaruMsg.ES_LKAS):
return [[lkas_msg, SUBARU_MAIN_BUS],
[SubaruMsg.ES_Distance, alt_bus],
[SubaruMsg.ES_DashStatus, SUBARU_MAIN_BUS],
[SubaruMsg.ES_LKAS_State, SUBARU_MAIN_BUS],
[SubaruMsg.ES_Infotainment, SUBARU_MAIN_BUS]]
def long_tx_msgs(alt_bus):
return [[SubaruMsg.ES_Brake, alt_bus],
[SubaruMsg.ES_Status, alt_bus]]
def gen2_long_additional_tx_msgs():
return [[SubaruMsg.ES_UDS_Request, SUBARU_CAM_BUS],
[SubaruMsg.ES_HighBeamAssist, SUBARU_MAIN_BUS],
[SubaruMsg.ES_STATIC_1, SUBARU_MAIN_BUS],
[SubaruMsg.ES_STATIC_2, SUBARU_MAIN_BUS]]
def fwd_blacklisted_addr(lkas_msg=SubaruMsg.ES_LKAS):
return {SUBARU_CAM_BUS: [lkas_msg, SubaruMsg.ES_DashStatus, SubaruMsg.ES_LKAS_State, SubaruMsg.ES_Infotainment]}
class TestSubaruSafetyBase(common.CarSafetyTest):
FLAGS = 0
RELAY_MALFUNCTION_ADDRS = {SUBARU_MAIN_BUS: (SubaruMsg.ES_LKAS, SubaruMsg.ES_DashStatus, SubaruMsg.ES_LKAS_State,
SubaruMsg.ES_Infotainment)}
FWD_BLACKLISTED_ADDRS = fwd_blacklisted_addr()
MAX_RT_DELTA = 940
DRIVER_TORQUE_ALLOWANCE = 60
DRIVER_TORQUE_FACTOR = 50
ALT_MAIN_BUS = SUBARU_MAIN_BUS
ALT_CAM_BUS = SUBARU_CAM_BUS
DEG_TO_CAN = 100
INACTIVE_GAS = 1818
def setUp(self):
self.packer = CANPackerSafety("subaru_global_2017_generated")
self.safety = libsafety_py.libsafety
self.safety.set_safety_hooks(CarParams.SafetyModel.subaru, self.FLAGS)
self.safety.init_tests()
def _set_prev_torque(self, t):
self.safety.set_desired_torque_last(t)
self.safety.set_rt_torque_last(t)
def _torque_driver_msg(self, torque):
values = {"Steer_Torque_Sensor": torque}
return self.packer.make_can_msg_safety("Steering_Torque", 0, values)
def _speed_msg(self, speed):
values = {s: speed for s in ["FR", "FL", "RR", "RL"]}
return self.packer.make_can_msg_safety("Wheel_Speeds", self.ALT_MAIN_BUS, values)
def _user_brake_msg(self, brake):
values = {"Brake": brake}
return self.packer.make_can_msg_safety("Brake_Status", self.ALT_MAIN_BUS, values)
def _user_gas_msg(self, gas):
values = {"Throttle_Pedal": gas}
return self.packer.make_can_msg_safety("Throttle", 0, values)
def _pcm_status_msg(self, enable):
values = {"Cruise_Activated": enable}
return self.packer.make_can_msg_safety("CruiseControl", self.ALT_MAIN_BUS, values)
def _lkas_button_msg(self, lkas_pressed=False, lkas_hud=0):
values = {"LKAS_Dash_State": 2 if lkas_pressed else lkas_hud}
return self.packer.make_can_msg_safety("ES_LKAS_State", SUBARU_CAM_BUS, values)
def test_enable_control_allowed_with_mads_button(self):
for enable_mads in (True, False):
with self.subTest("enable_mads", mads_enabled=enable_mads):
for mads_button_press in range(4):
with self.subTest("mads_button_press", button_state=mads_button_press):
self.safety.set_mads_params(enable_mads, False, False)
self._rx(self._lkas_button_msg(False, mads_button_press))
self.assertEqual(enable_mads and mads_button_press in range(1, 4),
self.safety.get_controls_allowed_lateral())
class TestSubaruStockLongitudinalSafetyBase(TestSubaruSafetyBase):
def _cancel_msg(self, cancel, cruise_throttle=0):
values = {"Cruise_Cancel": cancel, "Cruise_Throttle": cruise_throttle}
return self.packer.make_can_msg_safety("ES_Distance", self.ALT_MAIN_BUS, values)
def test_cancel_message(self):
# test that we can only send the cancel message (ES_Distance) with inactive throttle (1818) and Cruise_Cancel=1
for cancel in [True, False]:
self._generic_limit_safety_check(partial(self._cancel_msg, cancel), self.INACTIVE_GAS, self.INACTIVE_GAS, 0, 2**12, 1, self.INACTIVE_GAS, cancel)
class TestSubaruLongitudinalSafetyBase(TestSubaruSafetyBase, common.LongitudinalGasBrakeSafetyTest):
MIN_GAS = 808
MAX_GAS = 3400
INACTIVE_GAS = 1818
MAX_POSSIBLE_GAS = 2**13
MIN_BRAKE = 0
MAX_BRAKE = 600
MAX_POSSIBLE_BRAKE = 2**16
MIN_RPM = 0
MAX_RPM = 3600
MAX_POSSIBLE_RPM = 2**13
FWD_BLACKLISTED_ADDRS = {2: [SubaruMsg.ES_LKAS, SubaruMsg.ES_Brake, SubaruMsg.ES_Distance,
SubaruMsg.ES_Status, SubaruMsg.ES_DashStatus,
SubaruMsg.ES_LKAS_State, SubaruMsg.ES_Infotainment]}
def test_rpm_safety_check(self):
self._generic_limit_safety_check(self._send_rpm_msg, self.MIN_RPM, self.MAX_RPM, 0, self.MAX_POSSIBLE_RPM, 1)
def _send_brake_msg(self, brake):
values = {"Brake_Pressure": brake}
return self.packer.make_can_msg_safety("ES_Brake", self.ALT_MAIN_BUS, values)
def _send_gas_msg(self, gas):
values = {"Cruise_Throttle": gas}
return self.packer.make_can_msg_safety("ES_Distance", self.ALT_MAIN_BUS, values)
def _send_rpm_msg(self, rpm):
values = {"Cruise_RPM": rpm}
return self.packer.make_can_msg_safety("ES_Status", self.ALT_MAIN_BUS, values)
class TestSubaruTorqueSafetyBase(TestSubaruSafetyBase, common.DriverTorqueSteeringSafetyTest, common.SteerRequestCutSafetyTest):
MAX_RATE_UP = 50
MAX_RATE_DOWN = 70
MAX_TORQUE_LOOKUP = [0], [2047]
# Safety around steering req bit
MIN_VALID_STEERING_FRAMES = 7
MAX_INVALID_STEERING_FRAMES = 1
STEER_STEP = 2
def _torque_cmd_msg(self, torque, steer_req=1):
values = {"LKAS_Output": torque, "LKAS_Request": steer_req}
return self.packer.make_can_msg_safety("ES_LKAS", SUBARU_MAIN_BUS, values)
class TestSubaruGen1TorqueStockLongitudinalSafety(TestSubaruStockLongitudinalSafetyBase, TestSubaruTorqueSafetyBase):
FLAGS = 0
TX_MSGS = lkas_tx_msgs(SUBARU_MAIN_BUS)
class TestSubaruGen2TorqueSafetyBase(TestSubaruTorqueSafetyBase):
ALT_MAIN_BUS = SUBARU_ALT_BUS
ALT_CAM_BUS = SUBARU_ALT_BUS
MAX_RATE_UP = 35
MAX_RATE_DOWN = 50
MAX_TORQUE_LOOKUP = [0], [1500]
class TestSubaruGen2TorqueStockLongitudinalSafety(TestSubaruStockLongitudinalSafetyBase, TestSubaruGen2TorqueSafetyBase):
FLAGS = SubaruSafetyFlags.GEN2
TX_MSGS = lkas_tx_msgs(SUBARU_ALT_BUS)
class TestSubaruGen1LongitudinalSafety(TestSubaruLongitudinalSafetyBase, TestSubaruTorqueSafetyBase):
FLAGS = SubaruSafetyFlags.LONG
TX_MSGS = lkas_tx_msgs(SUBARU_MAIN_BUS) + long_tx_msgs(SUBARU_MAIN_BUS)
RELAY_MALFUNCTION_ADDRS = {SUBARU_MAIN_BUS: (SubaruMsg.ES_LKAS, SubaruMsg.ES_DashStatus, SubaruMsg.ES_LKAS_State,
SubaruMsg.ES_Infotainment, SubaruMsg.ES_Brake, SubaruMsg.ES_Status,
SubaruMsg.ES_Distance)}
class TestSubaruGen2LongitudinalSafety(TestSubaruLongitudinalSafetyBase, TestSubaruGen2TorqueSafetyBase):
FLAGS = SubaruSafetyFlags.LONG | SubaruSafetyFlags.GEN2
TX_MSGS = lkas_tx_msgs(SUBARU_ALT_BUS) + long_tx_msgs(SUBARU_ALT_BUS) + gen2_long_additional_tx_msgs()
FWD_BLACKLISTED_ADDRS = {2: [SubaruMsg.ES_LKAS, SubaruMsg.ES_DashStatus, SubaruMsg.ES_LKAS_State,
SubaruMsg.ES_Infotainment]}
RELAY_MALFUNCTION_ADDRS = {SUBARU_MAIN_BUS: (SubaruMsg.ES_LKAS, SubaruMsg.ES_DashStatus, SubaruMsg.ES_LKAS_State,
SubaruMsg.ES_Infotainment),
SUBARU_ALT_BUS: (SubaruMsg.ES_Brake, SubaruMsg.ES_Status, SubaruMsg.ES_Distance)}
def _rdbi_msg(self, did: int):
return b'\x03\x22' + did.to_bytes(2) + b'\x00\x00\x00\x00'
def _es_uds_msg(self, msg: bytes):
return libsafety_py.make_CANPacket(SubaruMsg.ES_UDS_Request, 2, msg)
def test_es_uds_message(self):
tester_present = b'\x02\x3E\x80\x00\x00\x00\x00\x00'
not_tester_present = b"\x03\xAA\xAA\x00\x00\x00\x00\x00"
button_did = 0x1130
# Tester present is allowed for gen2 long to keep eyesight disabled
self.assertTrue(self._tx(self._es_uds_msg(tester_present)))
# Non-Tester present is not allowed
self.assertFalse(self._tx(self._es_uds_msg(not_tester_present)))
# Only button_did is allowed to be read via UDS
for did in range(0xFFFF):
should_tx = (did == button_did)
self.assertEqual(self._tx(self._es_uds_msg(self._rdbi_msg(did))), should_tx)
# any other msg is not allowed
for sid in range(0xFF):
msg = b'\x03' + sid.to_bytes(1) + b'\x00' * 6
self.assertFalse(self._tx(self._es_uds_msg(msg)))
if __name__ == "__main__":
unittest.main()
+69
View File
@@ -0,0 +1,69 @@
#!/usr/bin/env python3
import unittest
from opendbc.car.structs import CarParams
from opendbc.car.subaru.values import SubaruSafetyFlags
from opendbc.safety.tests.libsafety import libsafety_py
import opendbc.safety.tests.common as common
from opendbc.safety.tests.common import CANPackerSafety
class TestSubaruPreglobalSafety(common.CarSafetyTest, common.DriverTorqueSteeringSafetyTest):
FLAGS = 0
DBC = "subaru_outback_2015_generated"
TX_MSGS = [[0x161, 0], [0x164, 0]]
RELAY_MALFUNCTION_ADDRS = {0: (0x164, 0x161)}
FWD_BLACKLISTED_ADDRS = {2: [0x161, 0x164]}
MAX_RATE_UP = 50
MAX_RATE_DOWN = 70
MAX_TORQUE_LOOKUP = [0], [2047]
MAX_RT_DELTA = 940
DRIVER_TORQUE_ALLOWANCE = 75
DRIVER_TORQUE_FACTOR = 10
def setUp(self):
self.packer = CANPackerSafety(self.DBC)
self.safety = libsafety_py.libsafety
self.safety.set_safety_hooks(CarParams.SafetyModel.subaruPreglobal, self.FLAGS)
self.safety.init_tests()
def _set_prev_torque(self, t):
self.safety.set_desired_torque_last(t)
self.safety.set_rt_torque_last(t)
def _torque_driver_msg(self, torque):
values = {"Steer_Torque_Sensor": torque}
return self.packer.make_can_msg_safety("Steering_Torque", 0, values)
def _speed_msg(self, speed):
# subaru safety doesn't use the scaled value, so undo the scaling
values = {s: speed*0.0592 for s in ["FR", "FL", "RR", "RL"]}
return self.packer.make_can_msg_safety("Wheel_Speeds", 0, values)
def _user_brake_msg(self, brake):
values = {"Brake_Pedal": brake}
return self.packer.make_can_msg_safety("Brake_Pedal", 0, values)
def _torque_cmd_msg(self, torque, steer_req=1):
values = {"LKAS_Command": torque, "LKAS_Active": steer_req}
return self.packer.make_can_msg_safety("ES_LKAS", 0, values)
def _user_gas_msg(self, gas):
values = {"Throttle_Pedal": gas}
return self.packer.make_can_msg_safety("Throttle", 0, values)
def _pcm_status_msg(self, enable):
values = {"Cruise_Activated": enable}
return self.packer.make_can_msg_safety("CruiseControl", 0, values)
class TestSubaruPreglobalReversedDriverTorqueSafety(TestSubaruPreglobalSafety):
FLAGS = SubaruSafetyFlags.PREGLOBAL_REVERSED_DRIVER_TORQUE
DBC = "subaru_outback_2019_generated"
if __name__ == "__main__":
unittest.main()
+511
View File
@@ -0,0 +1,511 @@
#!/usr/bin/env python3
import random
import unittest
import numpy as np
from opendbc.car.lateral import get_max_angle_delta_vm, get_max_angle_vm
from opendbc.car.tesla.teslacan import get_steer_ctrl_type
from opendbc.car.tesla.values import CarControllerParams, TeslaSafetyFlags, TeslaFlags, CANBUS
from opendbc.car.tesla.carcontroller import get_safety_CP
from opendbc.car.structs import CarParams
from opendbc.car.vehicle_model import VehicleModel
from opendbc.can import CANDefine
from opendbc.safety.tests.libsafety import libsafety_py
import opendbc.safety.tests.common as common
from opendbc.safety.tests.common import CANPackerSafety, MAX_SPEED_DELTA, MAX_WRONG_COUNTERS, away_round, round_speed
from opendbc.sunnypilot.car.tesla.values import TeslaSafetyFlagsSP
MSG_DAS_steeringControl = 0x488
MSG_APS_eacMonitor = 0x27d
MSG_DAS_Control = 0x2b9
def round_angle(apply_angle, can_offset=0):
apply_angle_can = (apply_angle + 1638.35) / 0.1 + can_offset
# 0.49999_ == 0.5
rnd_offset = 1e-5 if apply_angle >= 0 else -1e-5
return away_round(apply_angle_can + rnd_offset) * 0.1 - 1638.35
class TestTeslaSafetyBase(common.CarSafetyTest, common.AngleSteeringSafetyTest, common.LongitudinalAccelSafetyTest):
SAFETY_PARAM = 0
RELAY_MALFUNCTION_ADDRS = {0: (MSG_DAS_steeringControl, MSG_APS_eacMonitor)}
FWD_BLACKLISTED_ADDRS = {2: [MSG_DAS_steeringControl, MSG_APS_eacMonitor]}
TX_MSGS = [[MSG_DAS_steeringControl, 0], [MSG_APS_eacMonitor, 0], [MSG_DAS_Control, 0]]
STANDSTILL_THRESHOLD = 0.1
GAS_PRESSED_THRESHOLD = 3
# Angle control limits
STEER_ANGLE_MAX = 360 # deg
DEG_TO_CAN = 10
# Tesla uses get_max_angle_delta_vm and get_max_angle_vm for real lateral accel and jerk limits
# TODO: integrate this into AngleSteeringSafetyTest
ANGLE_RATE_BP = None
ANGLE_RATE_UP = None
ANGLE_RATE_DOWN = None
# Real time limits
LATERAL_FREQUENCY = 50 # Hz
# Long control limits
MAX_ACCEL = 2.0
MIN_ACCEL = -3.48
INACTIVE_ACCEL = 0.0
cnt_epas = 0
cnt_angle_cmd = 0
packer: CANPackerSafety
def _get_steer_cmd_angle_max(self, speed):
return get_max_angle_vm(max(speed, 1), self.VM, CarControllerParams)
def setUp(self):
self.VM = VehicleModel(get_safety_CP())
self.packer = CANPackerSafety("tesla_model3_party")
self.define = CANDefine("tesla_model3_party")
self.acc_states = {d: v for v, d in self.define.dv["DAS_control"]["DAS_accState"].items()}
self.autopark_states = {d: v for v, d in self.define.dv["DI_state"]["DI_autoparkState"].items()}
self.active_autopark_states = [self.autopark_states[s] for s in ('ACTIVE', 'COMPLETE', 'SELFPARK_STARTED')]
self.steer_control_types = {d: v for v, d in self.define.dv["DAS_steeringControl"]["DAS_steeringControlType"].items()}
self.safety = libsafety_py.libsafety
self.safety.set_safety_hooks(CarParams.SafetyModel.tesla, self.SAFETY_PARAM)
self.safety.init_tests()
def _angle_cmd_msg(self, angle: float, state: bool | int, increment_timer: bool = True, bus: int = 0):
# If FSD 14, translate steer control type to new flipped definition
if self.safety.get_current_safety_param() & TeslaSafetyFlags.FSD_14:
state = get_steer_ctrl_type(TeslaFlags.FSD_14, int(state))
values = {"DAS_steeringAngleRequest": angle, "DAS_steeringControlType": state}
if increment_timer:
self.safety.set_timer(self.cnt_angle_cmd * int(1e6 / self.LATERAL_FREQUENCY))
self.__class__.cnt_angle_cmd += 1
return self.packer.make_can_msg_safety("DAS_steeringControl", bus, values)
def _angle_meas_msg(self, angle: float, hands_on_level: int = 0, eac_status: int = 1, eac_error_code: int = 0):
values = {"EPAS3S_internalSAS": angle, "EPAS3S_handsOnLevel": hands_on_level,
"EPAS3S_eacStatus": eac_status, "EPAS3S_eacErrorCode": eac_error_code,
"EPAS3S_sysStatusCounter": self.cnt_epas % 16}
self.__class__.cnt_epas += 1
return self.packer.make_can_msg_safety("EPAS3S_sysStatus", 0, values)
def _user_brake_msg(self, brake, quality_flag: bool = True):
values = {"ESP_driverBrakeApply": 2 if brake else 1}
if not quality_flag:
values["ESP_driverBrakeApply"] = random.choice((0, 3)) # NotInit_orOff, Faulty_SNA
return self.packer.make_can_msg_safety("ESP_status", 0, values)
def _speed_msg(self, speed):
values = {"DI_vehicleSpeed": speed * 3.6}
return self.packer.make_can_msg_safety("DI_speed", 0, values)
def _speed_msg_2(self, speed, quality_flag=True):
values = {"ESP_vehicleSpeed": speed * 3.6, "ESP_wheelSpeedsQF": quality_flag}
return self.packer.make_can_msg_safety("ESP_B", 0, values)
def _vehicle_moving_msg(self, speed: float, quality_flag=True):
values = {"ESP_vehicleStandstillSts": 1 if speed <= self.STANDSTILL_THRESHOLD else 0,
"ESP_wheelSpeedsQF": quality_flag}
return self.packer.make_can_msg_safety("ESP_B", 0, values)
def _user_gas_msg(self, gas):
values = {"DI_accelPedalPos": gas}
return self.packer.make_can_msg_safety("DI_systemStatus", 0, values)
def _pcm_status_msg(self, enable, autopark_state=0):
values = {
"DI_cruiseState": 2 if enable else 0,
"DI_autoparkState": autopark_state,
}
return self.packer.make_can_msg_safety("DI_state", 0, values)
def _long_control_msg(self, set_speed, acc_state=0, jerk_limits=(0, 0), accel_limits=(0, 0), aeb_event=0, bus=0):
values = {
"DAS_setSpeed": set_speed,
"DAS_accState": acc_state,
"DAS_aebEvent": aeb_event,
"DAS_jerkMin": jerk_limits[0],
"DAS_jerkMax": jerk_limits[1],
"DAS_accelMin": accel_limits[0],
"DAS_accelMax": accel_limits[1],
}
return self.packer.make_can_msg_safety("DAS_control", bus, values)
def _accel_msg(self, accel: float):
# For common.LongitudinalAccelSafetyTest
return self._long_control_msg(10, accel_limits=(accel, max(accel, 0)))
def test_rx_hook(self):
# counter check
for msg_type in ("angle", "long", "speed", "speed_2"):
# send multiple times to verify counter checks
for i in range(10):
if msg_type == "angle":
msg = self._angle_cmd_msg(0, True, bus=2)
elif msg_type == "long":
msg = self._long_control_msg(0, bus=2)
elif msg_type == "speed":
msg = self._speed_msg(0)
elif msg_type == "speed_2":
msg = self._speed_msg_2(0)
should_rx = i >= 5
if not should_rx:
# mess with checksums
if msg_type == "angle":
msg[0].data[3] = 0
elif msg_type == "long":
msg[0].data[7] = 0
elif msg_type == "speed":
msg[0].data[0] = 0
elif msg_type == "speed_2":
msg[0].data[7] = 0
self.safety.set_controls_allowed(True)
self.assertEqual(should_rx, self._rx(msg))
self.assertEqual(should_rx, self.safety.get_controls_allowed())
# Send static counters
for i in range(MAX_WRONG_COUNTERS + 1):
should_rx = i + 1 < MAX_WRONG_COUNTERS
self.assertEqual(should_rx, self._rx(msg))
self.assertEqual(should_rx, self.safety.get_controls_allowed())
def test_vehicle_speed_measurements(self):
# OVERRIDDEN: 79.1667 is the max speed in m/s
self._common_measurement_test(self._speed_msg, 0, 285 / 3.6, 1,
self.safety.get_vehicle_speed_min, self.safety.get_vehicle_speed_max)
def test_rx_hook_speed_mismatch(self):
# TODO: overridden because of custom rounding
# Tesla relies on speed for lateral limits close to ISO 11270, so it checks two sources
for speed in np.arange(0, 40, 0.5):
# match signal rounding on CAN
speed = away_round(speed / 0.08 * 3.6) * 0.08 / 3.6
for speed_delta in np.arange(-5, 5, 0.1):
speed_2 = max(speed + speed_delta, 0)
speed_2 = away_round(speed_2 * 2 * 3.6) / 2 / 3.6
# Set controls allowed in between rx since first message can reset it
self.assertTrue(self._rx(self._speed_msg(speed)))
self.safety.set_controls_allowed(True)
self.assertTrue(self._rx(self._speed_msg_2(speed_2)))
within_delta = abs(speed - speed_2) <= MAX_SPEED_DELTA
self.assertEqual(self.safety.get_controls_allowed(), within_delta)
# Test ESP_B quality flag
for quality_flag in (True, False):
self.safety.set_controls_allowed(True)
self.assertTrue(self._rx(self._speed_msg(0)))
self.assertEqual(quality_flag, self._rx(self._speed_msg_2(0, quality_flag=quality_flag)))
self.assertEqual(quality_flag, self.safety.get_controls_allowed())
def test_user_brake_quality_flag(self):
for quality_flag in (True, False):
msg = self._user_brake_msg(True, quality_flag=quality_flag)
self.assertEqual(quality_flag, self._rx(msg))
def test_steering_wheel_disengage(self):
# Tesla disengages when the user forcibly overrides the locked-in angle steering control
# Either when the hands on level is high, or if there is a high angle rate fault
for hands_on_level in range(4):
for eac_status in range(8):
for eac_error_code in range(16):
self.safety.set_controls_allowed(True)
should_disengage = hands_on_level >= 3 or (eac_status == 0 and eac_error_code == 9)
self.assertTrue(self._rx(self._angle_meas_msg(0, hands_on_level=hands_on_level, eac_status=eac_status,
eac_error_code=eac_error_code)))
self.assertNotEqual(should_disengage, self.safety.get_controls_allowed())
self.assertEqual(should_disengage, self.safety.get_steering_disengage_prev())
# Should not recover
self.assertTrue(self._rx(self._angle_meas_msg(0, hands_on_level=0, eac_status=1, eac_error_code=0)))
self.assertNotEqual(should_disengage, self.safety.get_controls_allowed())
self.assertFalse(self.safety.get_steering_disengage_prev())
def test_autopark_summon_while_enabled(self):
# We should not respect Autopark that activates while controls are allowed
self._rx(self._pcm_status_msg(True, 0))
self._rx(self._pcm_status_msg(True, self.autopark_states["SELFPARK_STARTED"]))
self.assertTrue(self.safety.get_controls_allowed())
self.assertTrue(self._tx(self._angle_cmd_msg(0, True)))
self.assertTrue(self._tx(self._long_control_msg(0, acc_state=self.acc_states["ACC_CANCEL_GENERIC_SILENT"])))
# We should still not respect Autopark if we disengage cruise
self._rx(self._pcm_status_msg(False, self.autopark_states["SELFPARK_STARTED"]))
self.assertFalse(self.safety.get_controls_allowed())
self.assertTrue(self._tx(self._angle_cmd_msg(0, False)))
self.assertTrue(self._tx(self._long_control_msg(0, acc_state=self.acc_states["ACC_CANCEL_GENERIC_SILENT"])))
def test_autopark_summon_behavior(self):
for autopark_state in range(16):
self._rx(self._pcm_status_msg(False, 0))
# We shouldn't allow controls if Autopark is an active state
autopark_active = autopark_state in self.active_autopark_states
self._rx(self._pcm_status_msg(False, autopark_state))
self._rx(self._pcm_status_msg(True, autopark_state))
self.assertNotEqual(autopark_active, self.safety.get_controls_allowed())
# We should also start blocking all inactive/active openpilot msgs
self.assertNotEqual(autopark_active, self._tx(self._angle_cmd_msg(0, False)))
self.assertNotEqual(autopark_active, self._tx(self._angle_cmd_msg(0, True)))
self.assertNotEqual(autopark_active, self._tx(self._long_control_msg(0, acc_state=self.acc_states["ACC_CANCEL_GENERIC_SILENT"])))
self.assertNotEqual(autopark_active or not self.LONGITUDINAL, self._tx(self._long_control_msg(0, acc_state=self.acc_states["ACC_ON"])))
# Regain controls when Autopark disables
self._rx(self._pcm_status_msg(True, 0))
self.assertTrue(self.safety.get_controls_allowed())
self.assertTrue(self._tx(self._angle_cmd_msg(0, False)))
self.assertTrue(self._tx(self._angle_cmd_msg(0, True)))
self.assertTrue(self._tx(self._long_control_msg(0, acc_state=self.acc_states["ACC_CANCEL_GENERIC_SILENT"])))
self.assertEqual(self.LONGITUDINAL, self._tx(self._long_control_msg(0, acc_state=self.acc_states["ACC_ON"])))
def test_steering_control_type(self):
# Only angle control is allowed (no LANE_KEEP_ASSIST or EMERGENCY_LANE_KEEP)
self.safety.set_controls_allowed(True)
for steer_control_type in range(4):
should_tx = steer_control_type in (self.steer_control_types["NONE"],
self.steer_control_types["ANGLE_CONTROL"],
self.steer_control_types["LANE_KEEP_ASSIST"])
self.assertEqual(should_tx, self._tx(self._angle_cmd_msg(0, state=steer_control_type)))
def test_stock_lkas_passthrough(self):
# TODO: make these generic passthrough tests
no_lkas_msg = self._angle_cmd_msg(0, state=False)
no_lkas_msg_cam = self._angle_cmd_msg(0, state=True, bus=2)
lkas_msg_cam = self._angle_cmd_msg(0, state=self.steer_control_types['LANE_KEEP_ASSIST'], bus=2)
# stock system sends no LKAS -> no forwarding, and OP is allowed to TX
self.assertEqual(1, self._rx(no_lkas_msg_cam))
self.assertEqual(-1, self.safety.safety_fwd_hook(2, no_lkas_msg_cam.addr))
self.assertTrue(self._tx(no_lkas_msg))
# stock system sends LKAS -> forwarding, and OP is not allowed to TX
self.assertEqual(1, self._rx(lkas_msg_cam))
self.assertEqual(0, self.safety.safety_fwd_hook(2, lkas_msg_cam.addr))
self.assertFalse(self._tx(no_lkas_msg))
def test_angle_cmd_when_enabled(self):
# We properly test lateral acceleration and jerk below
pass
def test_lateral_accel_limit(self):
for speed in np.linspace(0, 40, 100):
speed = max(speed, 1)
# match DI_vehicleSpeed rounding on CAN
speed = round_speed(away_round(speed / 0.08 * 3.6) * 0.08 / 3.6)
for sign in (-1, 1):
self.safety.set_controls_allowed(True)
self._reset_speed_measurement(speed + 1) # safety fudges the speed
# angle signal can't represent 0, so it biases one unit down
angle_unit_offset = -1 if sign == -1 else 0
# at limit (safety tolerance adds 1)
max_angle = round_angle(get_max_angle_vm(speed, self.VM, CarControllerParams), angle_unit_offset + 1) * sign
max_angle = np.clip(max_angle, -self.STEER_ANGLE_MAX, self.STEER_ANGLE_MAX)
self.safety.set_desired_angle_last(round(max_angle * self.DEG_TO_CAN))
self.assertTrue(self._tx(self._angle_cmd_msg(max_angle, True)))
# 1 unit above limit
max_angle_raw = round_angle(get_max_angle_vm(speed, self.VM, CarControllerParams), angle_unit_offset + 2) * sign
max_angle = np.clip(max_angle_raw, -self.STEER_ANGLE_MAX, self.STEER_ANGLE_MAX)
self._tx(self._angle_cmd_msg(max_angle, True))
# at low speeds max angle is above 360, so adding 1 has no effect
should_tx = abs(max_angle_raw) >= self.STEER_ANGLE_MAX
self.assertEqual(should_tx, self._tx(self._angle_cmd_msg(max_angle, True)))
def test_lateral_jerk_limit(self):
for speed in np.linspace(0, 40, 100):
speed = max(speed, 1)
# match DI_vehicleSpeed rounding on CAN
speed = round_speed(away_round(speed / 0.08 * 3.6) * 0.08 / 3.6)
for sign in (-1, 1): # (-1, 1):
self.safety.set_controls_allowed(True)
self._reset_speed_measurement(speed + 1) # safety fudges the speed
self._tx(self._angle_cmd_msg(0, True))
# angle signal can't represent 0, so it biases one unit down
angle_unit_offset = 1 if sign == -1 else 0
# Stay within limits
# Up
max_angle_delta = round_angle(get_max_angle_delta_vm(speed, self.VM, CarControllerParams), angle_unit_offset) * sign
self.assertTrue(self._tx(self._angle_cmd_msg(max_angle_delta, True)))
# Don't change
self.assertTrue(self._tx(self._angle_cmd_msg(max_angle_delta, True)))
# Down
self.assertTrue(self._tx(self._angle_cmd_msg(0, True)))
# Inject too high rates
# Up
max_angle_delta = round_angle(get_max_angle_delta_vm(speed, self.VM, CarControllerParams), angle_unit_offset + 1) * sign
self.assertFalse(self._tx(self._angle_cmd_msg(max_angle_delta, True)))
# Don't change
self.safety.set_desired_angle_last(round(max_angle_delta * self.DEG_TO_CAN))
self.assertTrue(self._tx(self._angle_cmd_msg(max_angle_delta, True)))
# Down
self.assertFalse(self._tx(self._angle_cmd_msg(0, True)))
# Recover
self.assertTrue(self._tx(self._angle_cmd_msg(0, True)))
class TestTeslaStockSafety(TestTeslaSafetyBase):
LONGITUDINAL = False
def test_cancel(self):
for acc_state in range(16):
self.safety.set_controls_allowed(True)
should_tx = acc_state == self.acc_states["ACC_CANCEL_GENERIC_SILENT"]
self.assertFalse(self._tx(self._long_control_msg(0, acc_state=acc_state, accel_limits=(self.MIN_ACCEL, self.MAX_ACCEL))))
self.assertEqual(should_tx, self._tx(self._long_control_msg(0, acc_state=acc_state)))
def test_no_aeb(self):
for aeb_event in range(4):
should_tx = aeb_event == 0
ret = self._tx(self._long_control_msg(10, acc_state=self.acc_states["ACC_CANCEL_GENERIC_SILENT"], aeb_event=aeb_event))
self.assertEqual(ret, should_tx)
def test_stock_aeb_no_cancel(self):
# No passthrough logic since we always forward DAS_control,
# but ensure we can't send cancel cmd while stock AEB is active
no_aeb_msg = self._long_control_msg(10, acc_state=self.acc_states["ACC_CANCEL_GENERIC_SILENT"], aeb_event=0)
no_aeb_msg_cam = self._long_control_msg(10, aeb_event=0, bus=2)
aeb_msg_cam = self._long_control_msg(10, aeb_event=1, bus=2)
# stock system sends no AEB -> no forwarding, and OP is allowed to TX
self.assertEqual(1, self._rx(no_aeb_msg_cam))
self.assertEqual(0, self.safety.safety_fwd_hook(2, no_aeb_msg_cam.addr))
self.assertTrue(self._tx(no_aeb_msg))
# stock system sends AEB -> forwarding, and OP is not allowed to TX
self.assertEqual(1, self._rx(aeb_msg_cam))
self.assertEqual(0, self.safety.safety_fwd_hook(2, aeb_msg_cam.addr))
self.assertFalse(self._tx(no_aeb_msg))
class TestTeslaFSD14StockSafety(TestTeslaStockSafety):
SAFETY_PARAM = TeslaSafetyFlags.FSD_14
class TestTeslaLongitudinalSafety(TestTeslaSafetyBase):
SAFETY_PARAM = TeslaSafetyFlags.LONG_CONTROL
RELAY_MALFUNCTION_ADDRS = {0: (MSG_DAS_steeringControl, MSG_APS_eacMonitor, MSG_DAS_Control)}
FWD_BLACKLISTED_ADDRS = {2: [MSG_DAS_steeringControl, MSG_APS_eacMonitor, MSG_DAS_Control]}
def test_no_aeb(self):
for aeb_event in range(4):
self.assertEqual(self._tx(self._long_control_msg(10, aeb_event=aeb_event)), aeb_event == 0)
def test_stock_aeb_passthrough(self):
no_aeb_msg = self._long_control_msg(10, aeb_event=0)
no_aeb_msg_cam = self._long_control_msg(10, aeb_event=0, bus=2)
aeb_msg_cam = self._long_control_msg(10, aeb_event=1, bus=2)
# stock system sends no AEB -> no forwarding, and OP is allowed to TX
self.assertEqual(1, self._rx(no_aeb_msg_cam))
self.assertEqual(-1, self.safety.safety_fwd_hook(2, no_aeb_msg_cam.addr))
self.assertTrue(self._tx(no_aeb_msg))
# stock system sends AEB -> forwarding, and OP is not allowed to TX
self.assertEqual(1, self._rx(aeb_msg_cam))
self.assertEqual(0, self.safety.safety_fwd_hook(2, aeb_msg_cam.addr))
self.assertFalse(self._tx(no_aeb_msg))
def test_prevent_reverse(self):
# Note: Tesla can reverse while at a standstill if both accel_min and accel_max are negative.
self.safety.set_controls_allowed(True)
# accel_min and accel_max are positive
self.assertTrue(self._tx(self._long_control_msg(set_speed=10, accel_limits=(1.1, 0.8))))
self.assertTrue(self._tx(self._long_control_msg(set_speed=0, accel_limits=(1.1, 0.8))))
# accel_min and accel_max are both zero
self.assertTrue(self._tx(self._long_control_msg(set_speed=10, accel_limits=(0, 0))))
self.assertTrue(self._tx(self._long_control_msg(set_speed=0, accel_limits=(0, 0))))
# accel_min and accel_max have opposing signs
self.assertTrue(self._tx(self._long_control_msg(set_speed=10, accel_limits=(-0.8, 1.3))))
self.assertTrue(self._tx(self._long_control_msg(set_speed=0, accel_limits=(0.8, -1.3))))
self.assertTrue(self._tx(self._long_control_msg(set_speed=0, accel_limits=(0, -1.3))))
# accel_min and accel_max are negative
self.assertFalse(self._tx(self._long_control_msg(set_speed=10, accel_limits=(-1.1, -0.6))))
self.assertFalse(self._tx(self._long_control_msg(set_speed=0, accel_limits=(-0.6, -1.1))))
self.assertFalse(self._tx(self._long_control_msg(set_speed=0, accel_limits=(-0.1, -0.1))))
class TestTeslaFSD14LongitudinalSafety(TestTeslaLongitudinalSafety):
SAFETY_PARAM = TeslaSafetyFlags.LONG_CONTROL | TeslaSafetyFlags.FSD_14
class TestTeslaIgnition(unittest.TestCase):
TX_MSGS: list = []
def setUp(self):
self.safety = libsafety_py.libsafety
self.safety.init_tests()
self.packer = CANPackerSafety("tesla_model3_party")
def _msg(self, counter, state):
return self.packer.make_can_msg_safety("VCFRONT_LVPowerState", 0,
{"VCFRONT_LVPowerStateCounter": counter,
"VCFRONT_vehiclePowerState": state})
# VEHICLE_POWER_STATE_DRIVE=3 (counter-gated)
def test_ignition_on(self):
for i in range(16):
self.safety.init_tests()
self.safety.ignition_can_hook(self._msg(i, 3))
self.assertFalse(self.safety.get_ignition_can())
self.safety.ignition_can_hook(self._msg((i + 1) % 16, 3))
self.assertTrue(self.safety.get_ignition_can())
def test_ignition_off(self):
self.safety.ignition_can_hook(self._msg(0, 3))
self.safety.ignition_can_hook(self._msg(1, 3))
self.assertTrue(self.safety.get_ignition_can())
self.safety.ignition_can_hook(self._msg(2, 2))
self.safety.ignition_can_hook(self._msg(3, 2))
self.assertFalse(self.safety.get_ignition_can())
class TestTeslaVehicleBusSafety(TestTeslaSafetyBase):
LONGITUDINAL = False
def setUp(self):
super().setUp()
self.safety = libsafety_py.libsafety
self.packer_adas = CANPackerSafety("tesla_model3_vehicle")
self.safety.set_current_safety_param_sp(TeslaSafetyFlagsSP.HAS_VEHICLE_BUS)
self.safety.set_safety_hooks(CarParams.SafetyModel.tesla, 0)
self.safety.init_tests()
def _lkas_button_msg(self, enabled):
values = {"UI_activeTouchPoints": 3 if enabled else 0}
return self.packer_adas.make_can_msg_safety("UI_status2", CANBUS.vehicle, values)
if __name__ == "__main__":
unittest.main()
+513
View File
@@ -0,0 +1,513 @@
#!/usr/bin/env python3
from opendbc.testing import parameterized_class
import numpy as np
import random
import unittest
import itertools
from opendbc.car.toyota.values import ToyotaSafetyFlags
from opendbc.sunnypilot.car.toyota.values import ToyotaSafetyFlagsSP
from opendbc.car.structs import CarParams
from opendbc.safety.tests.libsafety import libsafety_py
import opendbc.safety.tests.common as common
from opendbc.safety.tests.common import CANPackerSafety
from opendbc.safety.tests.gas_interceptor_common import GasInterceptorSafetyTest
TOYOTA_COMMON_TX_MSGS = [[0x2E4, 0], [0x191, 0], [0x412, 0], [0x343, 0], [0x1D2, 0]] # LKAS + LTA + ACC & PCM cancel cmds
TOYOTA_SECOC_TX_MSGS = [[0x131, 0], [0x183, 0]] + TOYOTA_COMMON_TX_MSGS
TOYOTA_COMMON_LONG_TX_MSGS = [[0x283, 0], [0x2E6, 0], [0x2E7, 0], [0x33E, 0], [0x344, 0], [0x365, 0], [0x366, 0], [0x4CB, 0], # DSU bus 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]]
UNSUPPORTED_DSU = [
{"SAFETY_PARAM_SP": ToyotaSafetyFlagsSP.DEFAULT},
{"SAFETY_PARAM_SP": ToyotaSafetyFlagsSP.UNSUPPORTED_DSU},
]
class TestToyotaSafetyBase(common.CarSafetyTest, common.LongitudinalAccelSafetyTest):
TX_MSGS = TOYOTA_COMMON_TX_MSGS + TOYOTA_COMMON_LONG_TX_MSGS
RELAY_MALFUNCTION_ADDRS = {0: (0x2E4, 0x191, 0x412, 0x343)}
FWD_BLACKLISTED_ADDRS = {2: [0x2E4, 0x412, 0x191, 0x343]}
EPS_SCALE = 73
SAFETY_PARAM_SP: int = 0
packer: CANPackerSafety
safety: libsafety_py.LibSafety
def _torque_meas_msg(self, torque: int, driver_torque: int | None = None):
values = {"STEER_TORQUE_EPS": (torque / self.EPS_SCALE) * 100.}
if driver_torque is not None:
values["STEER_TORQUE_DRIVER"] = driver_torque
return self.packer.make_can_msg_safety("STEER_TORQUE_SENSOR", 0, values)
# Both torque and angle safety modes test with each other's steering commands
def _torque_cmd_msg(self, torque, steer_req=1):
values = {"STEER_TORQUE_CMD": torque, "STEER_REQUEST": steer_req}
return self.packer.make_can_msg_safety("STEERING_LKA", 0, values)
def _angle_meas_msg(self, angle: float, steer_angle_initializing: bool = False):
# This creates a steering torque angle message. Not set on all platforms,
# relative to init angle on some older TSS2 platforms. Only to be used with LTA
values = {"STEER_ANGLE": angle, "STEER_ANGLE_INITIALIZING": int(steer_angle_initializing)}
return self.packer.make_can_msg_safety("STEER_TORQUE_SENSOR", 0, values)
def _angle_cmd_msg(self, angle: float, enabled: bool):
return self._lta_msg(int(enabled), int(enabled), angle, torque_wind_down=100 if enabled else 0)
def _lta_msg(self, req, req2, angle_cmd, torque_wind_down=100):
values = {"STEER_REQUEST": req, "STEER_REQUEST_2": req2, "STEER_ANGLE_CMD": angle_cmd, "TORQUE_WIND_DOWN": torque_wind_down}
return self.packer.make_can_msg_safety("STEERING_LTA", 0, values)
def _accel_msg_343(self, accel, cancel_req=0):
values = {"ACCEL_CMD": accel, "CANCEL_REQ": cancel_req}
return self.packer.make_can_msg_safety("ACC_CONTROL", 0, values)
def _accel_msg(self, accel, cancel_req=0):
return self._accel_msg_343(accel, cancel_req)
def _speed_msg(self, speed, quality_flag=True):
values = {("WHEEL_SPEED_%s" % n): speed * 3.6 for n in ["FR", "FL", "RR", "RL"]}
if not quality_flag:
values |= {"WHEEL_SPEED_%s_FAULT" % n: 1.0 for n in ["FR", "FL", "RR", "RL"]}
return self.packer.make_can_msg_safety("WHEEL_SPEEDS", 0, values)
def _user_brake_msg(self, brake):
values = {"BRAKE_PRESSED": brake}
return self.packer.make_can_msg_safety("BRAKE_MODULE", 0, values)
def _user_gas_msg(self, gas):
cruise_active = self.safety.get_controls_allowed()
values = {"GAS_RELEASED": not gas, "CRUISE_ACTIVE": cruise_active}
return self.packer.make_can_msg_safety("PCM_CRUISE", 0, values)
def _pcm_status_msg(self, enable):
values = {"CRUISE_ACTIVE": enable}
return self.packer.make_can_msg_safety("PCM_CRUISE", 0, values)
def _acc_state_msg(self, enabled):
msg = "DSU_CRUISE" if self.SAFETY_PARAM_SP & ToyotaSafetyFlagsSP.UNSUPPORTED_DSU else "PCM_CRUISE_2"
values = {"MAIN_ON": enabled}
return self.packer.make_can_msg_safety(msg, 0, values)
def test_diagnostics(self, stock_longitudinal: bool = False, ecu_disabled: bool = True):
for should_tx, msg in ((False, b"\x6D\x02\x3E\x00\x00\x00\x00\x00"), # fwdCamera tester present
(False, b"\x0F\x03\xAA\xAA\x00\x00\x00\x00"), # non-tester present
(True, b"\x0F\x02\x3E\x00\x00\x00\x00\x00")):
tester_present = libsafety_py.make_CANPacket(0x750, 0, msg)
self.assertEqual(should_tx and ecu_disabled and not stock_longitudinal, self._tx(tester_present))
def test_block_aeb(self, stock_longitudinal: bool = False):
for controls_allowed in (True, False):
for bad in (True, False):
for _ in range(10):
self.safety.set_controls_allowed(controls_allowed)
dat = [random.randint(1, 255) for _ in range(7)]
if not bad:
dat = [0]*6 + dat[-1:]
msg = libsafety_py.make_CANPacket(0x283, 0, bytes(dat))
self.assertEqual(not bad and not stock_longitudinal, self._tx(msg))
# Only allow LTA msgs with no actuation
def test_lta_steer_cmd(self):
for engaged, req, req2, torque_wind_down, angle in itertools.product([True, False],
[0, 1], [0, 1],
[0, 50, 100],
np.linspace(-20, 20, 5)):
self.safety.set_controls_allowed(engaged)
should_tx = not req and not req2 and angle == 0 and torque_wind_down == 0
self.assertEqual(should_tx, self._tx(self._lta_msg(req, req2, angle, torque_wind_down)),
f"{req=} {req2=} {angle=} {torque_wind_down=}")
def test_rx_hook(self):
# checksum checks
for msg in ["trq", "pcm"]:
self.safety.set_controls_allowed(1)
if msg == "trq":
msg = self._torque_meas_msg(0)
if msg == "pcm":
msg = self._pcm_status_msg(True)
self.assertTrue(self._rx(msg))
msg[0].data[4] = 0
msg[0].data[5] = 0
msg[0].data[6] = 0
msg[0].data[7] = 0
self.assertFalse(self._rx(msg))
self.assertFalse(self.safety.get_controls_allowed())
# quality flag tests
msg = self._speed_msg(0)
self.assertTrue(self._rx(msg))
msg = self._speed_msg(0, quality_flag=False)
self.assertFalse(self._rx(msg))
def test_vehicle_speed_measurements(self):
# OVERRIDDEN: 72.22_ is the max speed in m/s
self._common_measurement_test(self._speed_msg, 0, 259 / 3.6, 1,
self.safety.get_vehicle_speed_min, self.safety.get_vehicle_speed_max)
class TestToyotaSafetyGasInterceptorBase(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 = libsafety_py.libsafety
self.safety.set_current_safety_param_sp(self.SAFETY_PARAM_SP | ToyotaSafetyFlagsSP.GAS_INTERCEPTOR)
self.safety.set_safety_hooks(CarParams.SafetyModel.toyota, self.safety.get_current_safety_param())
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 = libsafety_py.libsafety
self.safety.set_current_safety_param_sp(self.SAFETY_PARAM_SP | ToyotaSafetyFlagsSP.GAS_INTERCEPTOR)
self.safety.set_safety_hooks(CarParams.SafetyModel.toyota, self.safety.get_current_safety_param() | ToyotaSafetyFlags.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_no_disengage_on_gas_interceptor,
self.test_gas_interceptor_safety_check):
with self.subTest(test=test.__name__):
with self.assertRaises(AssertionError):
test()
@parameterized_class(UNSUPPORTED_DSU)
class TestToyotaSafetyTorque(TestToyotaSafetyBase, common.MotorTorqueSteeringSafetyTest, common.SteerRequestCutSafetyTest):
MAX_RATE_UP = 15
MAX_RATE_DOWN = 25
MAX_TORQUE_LOOKUP = [0], [1500]
MAX_RT_DELTA = 450
MAX_TORQUE_ERROR = 350
TORQUE_MEAS_TOLERANCE = 1 # toyota safety adds one to be conservative for rounding
# Safety around steering req bit
MIN_VALID_STEERING_FRAMES = 17
MAX_INVALID_STEERING_FRAMES = 1
@classmethod
def setUpClass(cls):
if cls.__name__ == "TestToyotaSafetyTorque":
cls.safety = None
raise unittest.SkipTest
def setUp(self):
self.packer = CANPackerSafety("toyota_nodsu_pt_generated")
self.safety = libsafety_py.libsafety
self.safety.set_current_safety_param_sp(self.SAFETY_PARAM_SP)
self.safety.set_safety_hooks(CarParams.SafetyModel.toyota, self.EPS_SCALE)
self.safety.init_tests()
@parameterized_class(UNSUPPORTED_DSU)
class TestToyotaSafetyTorqueGasInterceptor(TestToyotaSafetyGasInterceptorBase, TestToyotaSafetyTorque):
@classmethod
def setUpClass(cls):
if cls.__name__ == "TestToyotaSafetyTorqueGasInterceptor":
cls.safety = None
raise unittest.SkipTest
class TestToyotaSafetyAngle(TestToyotaSafetyBase, common.AngleSteeringSafetyTest):
# Angle control limits
STEER_ANGLE_MAX = 94.9461 # deg
DEG_TO_CAN = 17.452007 # 1 / 0.0573 deg to can
ANGLE_RATE_BP = [5., 25., 25.]
ANGLE_RATE_UP = [0.3, 0.15, 0.15] # windup limit
ANGLE_RATE_DOWN = [0.36, 0.26, 0.26] # unwind limit
MAX_LTA_ANGLE = 94.9461 # PCS faults if commanding above this, deg
MAX_MEAS_TORQUE = 1500 # max allowed measured EPS torque before wind down
MAX_LTA_DRIVER_TORQUE = 150 # max allowed driver torque before wind down
def setUp(self):
self.packer = CANPackerSafety("toyota_nodsu_pt_generated")
self.safety = libsafety_py.libsafety
self.safety.set_safety_hooks(CarParams.SafetyModel.toyota, self.EPS_SCALE | ToyotaSafetyFlags.LTA)
self.safety.init_tests()
# Only allow LKA msgs with no actuation
def test_lka_steer_cmd(self):
for engaged, steer_req, torque in itertools.product([True, False],
[0, 1],
np.linspace(-1500, 1500, 7)):
self.safety.set_controls_allowed(engaged)
torque = int(torque)
self.safety.set_rt_torque_last(torque)
self.safety.set_torque_meas(torque, torque)
self.safety.set_desired_torque_last(torque)
should_tx = not steer_req and torque == 0
self.assertEqual(should_tx, self._tx(self._torque_cmd_msg(torque, steer_req)))
def test_lta_steer_cmd(self):
"""
Tests the LTA steering command message
controls_allowed:
* STEER_REQUEST and STEER_REQUEST_2 do not mismatch
* TORQUE_WIND_DOWN is only set to 0 or 100 when STEER_REQUEST and STEER_REQUEST_2 are both 1
* Full torque messages are blocked if either EPS torque or driver torque is above the threshold
not controls_allowed:
* STEER_REQUEST, STEER_REQUEST_2, and TORQUE_WIND_DOWN are all 0
"""
for controls_allowed in (True, False):
for angle in np.arange(-90, 90, 1):
self.safety.set_controls_allowed(controls_allowed)
self._reset_angle_measurement(angle)
self._set_prev_desired_angle(angle)
self.assertTrue(self._tx(self._lta_msg(0, 0, angle, 0)))
if controls_allowed:
# Test the two steer request bits and TORQUE_WIND_DOWN torque wind down signal
for req, req2, torque_wind_down in itertools.product([0, 1], [0, 1], [0, 50, 100]):
mismatch = not (req or req2) and torque_wind_down != 0
should_tx = req == req2 and (torque_wind_down in (0, 100)) and not mismatch
self.assertEqual(should_tx, self._tx(self._lta_msg(req, req2, angle, torque_wind_down)))
else:
# Controls not allowed
for req, req2, torque_wind_down in itertools.product([0, 1], [0, 1], [0, 50, 100]):
should_tx = not (req or req2) and torque_wind_down == 0
self.assertEqual(should_tx, self._tx(self._lta_msg(req, req2, angle, torque_wind_down)))
# Test max EPS torque and driver override thresholds (independent of angle, test a few representative angles)
for angle in (-89, 0, 89):
self.safety.set_controls_allowed(True)
self._reset_angle_measurement(angle)
self._set_prev_desired_angle(angle)
cases = itertools.product(
(0, self.MAX_MEAS_TORQUE - 1, self.MAX_MEAS_TORQUE, self.MAX_MEAS_TORQUE + 1, self.MAX_MEAS_TORQUE * 2),
(0, self.MAX_LTA_DRIVER_TORQUE - 1, self.MAX_LTA_DRIVER_TORQUE, self.MAX_LTA_DRIVER_TORQUE + 1, self.MAX_LTA_DRIVER_TORQUE * 2)
)
for eps_torque, driver_torque in cases:
for sign in (-1, 1):
for _ in range(6):
self._rx(self._torque_meas_msg(sign * eps_torque, sign * driver_torque))
# Toyota adds 1 to EPS torque since it is rounded after EPS factor
should_tx = (eps_torque - 1) <= self.MAX_MEAS_TORQUE and driver_torque <= self.MAX_LTA_DRIVER_TORQUE
self.assertEqual(should_tx, self._tx(self._lta_msg(1, 1, angle, 100)))
self.assertTrue(self._tx(self._lta_msg(1, 1, angle, 0))) # should tx if we wind down torque
def test_angle_measurements(self):
"""
* Tests angle meas quality flag dictates whether angle measurement is parsed, and if rx is valid
* Tests rx hook correctly clips the angle measurement, since it is to be compared to LTA cmd when inactive
"""
for steer_angle_initializing in (True, False):
for angle in np.arange(0, self.STEER_ANGLE_MAX * 2, 1):
# If init flag is set, do not rx or parse any angle measurements
for a in (angle, -angle, 0, 0, 0, 0):
self.assertEqual(not steer_angle_initializing,
self._rx(self._angle_meas_msg(a, steer_angle_initializing)))
final_angle = 0 if steer_angle_initializing else round(angle * self.DEG_TO_CAN)
self.assertEqual(self.safety.get_angle_meas_min(), -final_angle)
self.assertEqual(self.safety.get_angle_meas_max(), final_angle)
self._rx(self._angle_meas_msg(0))
self.assertEqual(self.safety.get_angle_meas_min(), -final_angle)
self.assertEqual(self.safety.get_angle_meas_max(), 0)
self._rx(self._angle_meas_msg(0))
self.assertEqual(self.safety.get_angle_meas_min(), 0)
self.assertEqual(self.safety.get_angle_meas_max(), 0)
class TestToyotaSafetyAngleGasInterceptor(TestToyotaSafetyGasInterceptorBase, TestToyotaSafetyAngle):
pass
@parameterized_class(UNSUPPORTED_DSU)
class TestToyotaAltBrakeSafety(TestToyotaSafetyTorque):
@classmethod
def setUpClass(cls):
if cls.__name__ == "TestToyotaAltBrakeSafety":
cls.safety = None
raise unittest.SkipTest
def setUp(self):
self.packer = CANPackerSafety("toyota_new_mc_pt_generated")
self.safety = libsafety_py.libsafety
self.safety.set_current_safety_param_sp(self.SAFETY_PARAM_SP)
self.safety.set_safety_hooks(CarParams.SafetyModel.toyota, self.EPS_SCALE | ToyotaSafetyFlags.ALT_BRAKE)
self.safety.init_tests()
def _user_brake_msg(self, brake):
values = {"BRAKE_PRESSED": brake}
return self.packer.make_can_msg_safety("BRAKE_MODULE", 0, values)
# No LTA message in the DBC
def test_lta_steer_cmd(self):
pass
@parameterized_class(UNSUPPORTED_DSU)
class TestToyotaAltBrakeSafetyGasInterceptor(TestToyotaSafetyGasInterceptorBase, TestToyotaAltBrakeSafety):
@classmethod
def setUpClass(cls):
if cls.__name__ == "TestToyotaAltBrakeSafetyGasInterceptor":
cls.safety = None
raise unittest.SkipTest
# No LTA message in the DBC
def test_lta_steer_cmd(self):
pass
class TestToyotaStockLongitudinalBase(TestToyotaSafetyBase):
TX_MSGS = TOYOTA_COMMON_TX_MSGS
# Base addresses minus ACC_CONTROL (0x343)
RELAY_MALFUNCTION_ADDRS = {0: (0x2E4, 0x191, 0x412)}
FWD_BLACKLISTED_ADDRS = {2: [0x2E4, 0x412, 0x191]}
LONGITUDINAL = False
def test_diagnostics(self, stock_longitudinal: bool = True, ecu_disabled: bool = True):
super().test_diagnostics(stock_longitudinal=stock_longitudinal, ecu_disabled=ecu_disabled)
def test_block_aeb(self, stock_longitudinal: bool = True):
super().test_block_aeb(stock_longitudinal=stock_longitudinal)
def test_acc_cancel(self):
"""
Regardless of controls allowed, never allow ACC_CONTROL if cancel bit isn't set
"""
for controls_allowed in [True, False]:
self.safety.set_controls_allowed(controls_allowed)
for accel in np.arange(self.MIN_ACCEL - 1, self.MAX_ACCEL + 1, 0.1):
self.assertFalse(self._tx(self._accel_msg_343(accel)))
should_tx = np.isclose(accel, self.INACTIVE_ACCEL, atol=0.0001)
self.assertEqual(should_tx, self._tx(self._accel_msg_343(accel, cancel_req=1)))
@parameterized_class(UNSUPPORTED_DSU)
class TestToyotaStockLongitudinalTorque(TestToyotaStockLongitudinalBase, TestToyotaSafetyTorque):
@classmethod
def setUpClass(cls):
if cls.__name__ == "TestToyotaStockLongitudinalTorque":
cls.safety = None
raise unittest.SkipTest
def setUp(self):
self.packer = CANPackerSafety("toyota_nodsu_pt_generated")
self.safety = libsafety_py.libsafety
self.safety.set_current_safety_param_sp(self.SAFETY_PARAM_SP)
self.safety.set_safety_hooks(CarParams.SafetyModel.toyota, self.EPS_SCALE | ToyotaSafetyFlags.STOCK_LONGITUDINAL)
self.safety.init_tests()
class TestToyotaStockLongitudinalAngle(TestToyotaStockLongitudinalBase, TestToyotaSafetyAngle):
def setUp(self):
self.packer = CANPackerSafety("toyota_nodsu_pt_generated")
self.safety = libsafety_py.libsafety
self.safety.set_safety_hooks(CarParams.SafetyModel.toyota,
self.EPS_SCALE | ToyotaSafetyFlags.STOCK_LONGITUDINAL | ToyotaSafetyFlags.LTA)
self.safety.init_tests()
class TestToyotaSecOcSafetyBase(TestToyotaSafetyBase):
TX_MSGS = TOYOTA_SECOC_TX_MSGS
RELAY_MALFUNCTION_ADDRS = {0: (0x2E4, 0x191, 0x412, 0x131)}
FWD_BLACKLISTED_ADDRS = {2: [0x2E4, 0x191, 0x412, 0x131]}
def setUp(self):
self.packer = CANPackerSafety("toyota_secoc_pt_generated")
self.safety = libsafety_py.libsafety
self.safety.set_safety_hooks(CarParams.SafetyModel.toyota,
self.EPS_SCALE | ToyotaSafetyFlags.SECOC)
self.safety.init_tests()
def test_diagnostics(self, ecu_disabled: bool = False):
super().test_diagnostics(ecu_disabled=ecu_disabled)
# This platform also has alternate brake and PCM messages, but same naming in the DBC, so same packers work
def _user_gas_msg(self, gas):
values = {"GAS_PEDAL_USER": gas}
return self.packer.make_can_msg_safety("GAS_PEDAL", 0, values)
# This platform sends both STEERING_LTA (same as other Toyota) and STEERING_LTA_2 (SecOC signed)
# STEERING_LTA is checked for no-actuation by the base class, STEERING_LTA_2 is checked for no-actuation below
def _lta_2_msg(self, req, req2, angle_cmd, torque_wind_down=100):
values = {"STEER_REQUEST": req, "STEER_REQUEST_2": req2, "STEER_ANGLE_CMD": angle_cmd}
return self.packer.make_can_msg_safety("STEERING_LTA_2", 0, values)
def test_lta_2_steer_cmd(self):
for engaged, req, req2, angle in itertools.product([True, False], [0, 1], [0, 1], np.linspace(-20, 20, 5)):
self.safety.set_controls_allowed(engaged)
should_tx = not req and not req2 and angle == 0
self.assertEqual(should_tx, self._tx(self._lta_2_msg(req, req2, angle)), f"{req=} {req2=} {angle=}")
def _accel_msg_183(self, accel):
values = {"ACCEL_CMD": accel}
return self.packer.make_can_msg_safety("ACC_CONTROL_2", 0, values)
def _accel_msg(self, accel, cancel_req=0):
return self._accel_msg_183(accel)
class TestToyotaSecOcSafetyStockLongitudinal(TestToyotaSecOcSafetyBase, TestToyotaStockLongitudinalBase):
def setUp(self):
self.packer = CANPackerSafety("toyota_secoc_pt_generated")
self.safety = libsafety_py.libsafety
self.safety.set_safety_hooks(CarParams.SafetyModel.toyota,
self.EPS_SCALE | ToyotaSafetyFlags.STOCK_LONGITUDINAL | ToyotaSafetyFlags.SECOC)
self.safety.init_tests()
class TestToyotaSecOcSafety(TestToyotaSecOcSafetyBase):
RELAY_MALFUNCTION_ADDRS = {0: (0x2E4, 0x191, 0x412, 0x131, 0x343, 0x183)}
FWD_BLACKLISTED_ADDRS = {2: [0x2E4, 0x191, 0x412, 0x131, 0x343, 0x183]}
def setUp(self):
self.packer = CANPackerSafety("toyota_secoc_pt_generated")
self.safety = libsafety_py.libsafety
self.safety.set_safety_hooks(CarParams.SafetyModel.toyota, self.EPS_SCALE | ToyotaSafetyFlags.SECOC)
self.safety.init_tests()
@unittest.skip("test not applicable for cars without a DSU")
def test_block_aeb(self, stock_longitudinal: bool = False):
pass
def test_343_actuation_blocked(self):
"""
For SecOC cars, longitudinal acceleration must be sent in ACC_CONTROL_2, but all other ACC
data remains in ACC_CONTROL. Verify no actuation is sent via ACC_CONTROL.
"""
for controls_allowed in [True, False]:
self.safety.set_controls_allowed(controls_allowed)
for accel in np.arange(self.MIN_ACCEL - 1, self.MAX_ACCEL + 1, 0.1):
should_tx = np.isclose(accel, self.INACTIVE_ACCEL, atol=0.0001)
self.assertEqual(should_tx, self._tx(self._accel_msg_343(accel)))
self.assertEqual(should_tx, self._tx(self._accel_msg_343(accel, cancel_req=1)))
if __name__ == "__main__":
unittest.main()
@@ -0,0 +1,381 @@
#!/usr/bin/env python3
import numpy as np
import unittest
from opendbc.car.structs import CarParams
from opendbc.car.volkswagen.values import VolkswagenSafetyFlags
from opendbc.safety.tests.libsafety import libsafety_py
import opendbc.safety.tests.common as common
from opendbc.safety.tests.common import CANPackerSafety
MAX_ACCEL = 2.0
MIN_ACCEL = -3.5
# MEB message IDs
MSG_LH_EPS_03 = 0x9F
MSG_ESC_51 = 0xFC
MSG_Motor_51 = 0x10B
MSG_GRA_ACC_01 = 0x12B
MSG_QFK_01 = 0x13D
MSG_ACC_18 = 0x14D
MSG_KLR_01 = 0x25D
MSG_TA_01 = 0x26B
MSG_ACC_19 = 0x300
MSG_HCA_03 = 0x303
MSG_LDW_02 = 0x397
MSG_MOTOR_14 = 0x3BE
class TestVolkswagenMebSafetyBase(common.CarSafetyTest, common.CurvatureSteeringSafetyTest):
STANDSTILL_THRESHOLD = 0
RELAY_MALFUNCTION_ADDRS = {0: (MSG_HCA_03, MSG_LDW_02), 2: (MSG_KLR_01,)}
MAX_CURVATURE = 29105
MAX_CURVATURE_TEST = 0.195
CURVATURE_TO_CAN = 149253.7313
MAX_POWER = 125
MAX_POWER_TEST = 50
SEND_RATE = 0.02
LATERAL_FREQUENCY = 50 # Hz
cnt_curvature_cmd = 0
def _set_prev_desired_power(self, power: int):
# init with local tx sequence
prev_allowed = self.safety.get_controls_allowed()
self.safety.set_controls_allowed(True)
self._tx(self._curvature_cmd_msg(0, steer_req=True, power=power))
self.safety.set_controls_allowed(prev_allowed)
def test_power_limit(self):
max_power_can = self.MAX_POWER
max_power = self.MAX_POWER_TEST
self._set_prev_desired_power(max_power_can)
self.safety.set_controls_allowed(True)
self.assertTrue(self._tx(self._curvature_cmd_msg(0, steer_req=True, power=max_power)))
self.assertTrue(self.safety.get_controls_allowed())
self.assertFalse(self._tx(self._curvature_cmd_msg(0, steer_req=True, power=max_power + 1)))
self.safety.set_controls_allowed(True)
self.assertTrue(self._tx(self._curvature_cmd_msg(0, steer_req=True, power=max_power - 1)))
self.assertTrue(self.safety.get_controls_allowed())
self.assertFalse(self._tx(self._curvature_cmd_msg(0, steer_req=False, power=max_power)))
self.safety.set_controls_allowed(True)
self.assertTrue(self._tx(self._curvature_cmd_msg(0, steer_req=True, power=max_power)))
self.assertTrue(self.safety.get_controls_allowed())
self.assertFalse(self._tx(self._curvature_cmd_msg(0, steer_req=False, power=-max_power)))
def test_power_without_control(self):
max_power_can = self.MAX_POWER
max_power = self.MAX_POWER_TEST
self._set_prev_desired_power(max_power_can)
self.safety.set_controls_allowed(False)
self.assertFalse(self._tx(self._curvature_cmd_msg(0, steer_req=False, power=max_power)))
self.assertTrue(self._tx(self._curvature_cmd_msg(0, steer_req=False, power=0)))
self._set_prev_desired_power(max_power - 1)
self.assertFalse(self._tx(self._curvature_cmd_msg(0, steer_req=False, power=max_power)))
# When controls are not allowed, steer_req=True is gated only by the power check:
# steady or rising power is disallowed, but a strictly decreasing power (steering
# wind-down at disengagement) is permitted so the EPS torque authority ramps to zero
self._set_prev_desired_power(max_power - 1)
self.assertFalse(self._tx(self._curvature_cmd_msg(0, steer_req=True, power=max_power))) # rising power
self.assertFalse(self._tx(self._curvature_cmd_msg(0, steer_req=True, power=max_power))) # steady power
self.assertTrue(self._tx(self._curvature_cmd_msg(0, steer_req=True, power=max_power - 1))) # decreasing power
def _speed_msg(self, speed_mps: float):
spd_kph = speed_mps * 3.6
values = {f"{s}_Radgeschw": spd_kph for s in ("VL", "VR", "HL", "HR")}
return self.packer.make_can_msg_safety("ESC_51", 0, values)
def _speed_msg_2(self, speed_mps: float):
values = {"ESP_v_Signal": speed_mps * 3.6}
return self.packer.make_can_msg_safety("ESP_21", 0, values)
def _motor_14_msg(self, brake):
values = {"MO_Fahrer_bremst": brake}
return self.packer.make_can_msg_safety("Motor_14", 0, values)
def _user_brake_msg(self, brake):
return self._motor_14_msg(brake)
def _user_gas_msg(self, gas):
values = {"Accel_Pedal_Pressure": 1 if gas else 0, "TSK_Status": 3}
return self.packer.make_can_msg_safety("Motor_51", 0, values)
def _vehicle_moving_msg(self, speed_mps: float):
return self._speed_msg(speed_mps)
def _curvature_meas_msg(self, curvature):
values = {"Curvature": abs(curvature), "Curvature_VZ": curvature > 0}
return self.packer.make_can_msg_safety("QFK_01", 0, values)
def _curvature_cmd_msg(self, curvature, steer_req=1, power=50, increment_timer=True):
if increment_timer:
self.safety.set_timer(self.cnt_curvature_cmd * int(1e6 / self.LATERAL_FREQUENCY))
self.__class__.cnt_curvature_cmd += 1
values = {
"Curvature": abs(curvature),
"Curvature_VZ": curvature > 0,
"RequestStatus": 4 if steer_req else 0,
"Power": power,
}
return self.packer.make_can_msg_safety("HCA_03", 0, values)
def _accel_msg(self, accel):
values = {"ACC_Sollbeschleunigung_02": accel}
return self.packer.make_can_msg_safety("ACC_18", 0, values)
def _tsk_status_msg(self, enable, main_switch=True):
if main_switch:
tsk_status = 3 if enable else 2
else:
tsk_status = 0
values = {"TSK_Status": tsk_status}
return self.packer.make_can_msg_safety("Motor_51", 0, values)
def _pcm_status_msg(self, enable):
return self._tsk_status_msg(enable)
def _torque_driver_msg(self, torque):
values = {"EPS_Lenkmoment": abs(torque), "EPS_VZ_Lenkmoment": torque < 0}
return self.packer.make_can_msg_safety("LH_EPS_03", 0, values)
def _button_msg(self, cancel=0, resume=0, _set=0, bus=2):
values = {"GRA_Abbrechen": cancel, "GRA_Tip_Setzen": _set, "GRA_Tip_Wiederaufnahme": resume}
return self.packer.make_can_msg_safety("GRA_ACC_01", bus, values)
def test_curvature_measurements(self):
self._rx(self._curvature_meas_msg(0.15))
self._rx(self._curvature_meas_msg(-0.1))
self._rx(self._curvature_meas_msg(0))
self._rx(self._curvature_meas_msg(0))
self._rx(self._curvature_meas_msg(0))
self._rx(self._curvature_meas_msg(0))
self.assertEqual(int(-0.1 * self.CURVATURE_TO_CAN), self.safety.get_curvature_meas_min())
self.assertEqual(int(0.15 * self.CURVATURE_TO_CAN), self.safety.get_curvature_meas_max())
self._rx(self._curvature_meas_msg(0))
self.assertEqual(0, self.safety.get_curvature_meas_max())
self.assertEqual(int(-0.1 * self.CURVATURE_TO_CAN), self.safety.get_curvature_meas_min())
self._rx(self._curvature_meas_msg(0))
self.assertEqual(0, self.safety.get_curvature_meas_max())
self.assertEqual(0, self.safety.get_curvature_meas_min())
def test_brake_signal(self):
self._rx(self._user_brake_msg(False))
self.assertFalse(self.safety.get_brake_pressed_prev())
self._rx(self._user_brake_msg(True))
self.assertTrue(self.safety.get_brake_pressed_prev())
def test_torque_driver_measurements(self):
for t in (0, 100, -100, 250, -250):
self._rx(self._torque_driver_msg(t))
def test_main_switch_off_disables_controls(self):
self.safety.set_controls_allowed(True)
self._rx(self._tsk_status_msg(False, main_switch=False))
self.assertFalse(self.safety.get_controls_allowed())
def test_cancel_button_rising_edge(self):
self.safety.set_controls_allowed(True)
self._rx(self._button_msg(cancel=1, bus=0))
self.assertFalse(self.safety.get_controls_allowed())
def test_rx_hook_speed_mismatch(self):
for speed in np.arange(0, 40, 0.5):
for speed_delta in np.arange(-5, 5, 0.1):
speed_2 = round(max(speed + speed_delta, 0), 1)
self._rx(self._speed_msg(speed))
self._rx(self._speed_msg_2(speed_2))
self.safety.set_controls_allowed(True)
self._tx(self._curvature_cmd_msg(0, steer_req=True))
within_delta = abs(speed - speed_2) <= common.MAX_SPEED_DELTA
self.assertEqual(self.safety.get_controls_allowed(), within_delta)
def test_curvature_violation(self):
# if violation occurs, MEB resets desired_curvature_last to measured curvature
meas = self.MAX_CURVATURE_TEST / 4
self.safety.set_controls_allowed(True)
self._reset_curvature_measurement(meas)
self._set_prev_desired_curvature(0)
# cause a violation by sending a command far from prev=0
self.assertFalse(self._tx(self._curvature_cmd_msg(self.MAX_CURVATURE_TEST, steer_req=True, power=50)))
# prev should track curvature_meas
self.assertEqual(self.safety.get_curvature_meas_min(), self.safety.get_desired_curvature_last())
def test_curvature_cmd_when_not_steering(self):
# Tests that only a zero curvature is allowed while the steer
# actuation bit is 0, regardless of controls allowed or meas
for controls_allowed in (True, False):
self.safety.set_controls_allowed(controls_allowed)
for steer_req in (True, False):
for curvature_meas in np.arange(-self.MAX_CURVATURE_TEST, self.MAX_CURVATURE_TEST, self.MAX_CURVATURE_TEST / 5):
self._reset_curvature_measurement(curvature_meas)
for curvature_cmd in np.arange(-self.MAX_CURVATURE_TEST, self.MAX_CURVATURE_TEST, self.MAX_CURVATURE_TEST / 5):
self._set_prev_desired_curvature(curvature_cmd)
# controls_allowed is checked if actuation bit is 1, else the curvature must be zero (inactive)
should_tx = controls_allowed if steer_req else round(curvature_cmd * self.CURVATURE_TO_CAN) == 0
self.assertEqual(should_tx, self._tx(self._curvature_cmd_msg(curvature_cmd, steer_req=steer_req, power=50 if steer_req else 0)))
class TestVolkswagenMebStockSafety(TestVolkswagenMebSafetyBase):
TX_MSGS = [[MSG_HCA_03, 0], [MSG_LDW_02, 0], [MSG_GRA_ACC_01, 0], [MSG_GRA_ACC_01, 2],
[MSG_KLR_01, 0], [MSG_KLR_01, 2]]
FWD_BLACKLISTED_ADDRS = {0: [MSG_KLR_01], 2: [MSG_HCA_03, MSG_LDW_02]}
def setUp(self):
self.packer = CANPackerSafety("vw_meb_generated")
self.safety = libsafety_py.libsafety
self.safety.set_safety_hooks(CarParams.SafetyModel.volkswagenMeb, 0)
self.safety.init_tests()
def test_spam_cancel_safety_check(self):
self.safety.set_controls_allowed(0)
self.assertTrue(self._tx(self._button_msg(cancel=1)))
self.assertFalse(self._tx(self._button_msg(resume=1)))
self.assertFalse(self._tx(self._button_msg(_set=1)))
self.safety.set_controls_allowed(1)
self.assertTrue(self._tx(self._button_msg(resume=1)))
class TestVolkswagenMebGen2StockSafety(TestVolkswagenMebStockSafety):
def setUp(self):
self.packer = CANPackerSafety("vw_meb_2024_generated")
self.safety = libsafety_py.libsafety
self.safety.set_safety_hooks(CarParams.SafetyModel.volkswagenMeb, VolkswagenSafetyFlags.MEB_ALT_CRC)
self.safety.init_tests()
class TestVolkswagenMebLongSafety(TestVolkswagenMebSafetyBase):
TX_MSGS = [[MSG_HCA_03, 0], [MSG_LDW_02, 0], [MSG_ACC_19, 0], [MSG_ACC_18, 0],
[MSG_TA_01, 0], [MSG_KLR_01, 0], [MSG_KLR_01, 2]]
FWD_BLACKLISTED_ADDRS = {0: [MSG_KLR_01],
2: [MSG_HCA_03, MSG_LDW_02, MSG_ACC_19, MSG_ACC_18, MSG_TA_01]}
RELAY_MALFUNCTION_ADDRS = {0: (MSG_HCA_03, MSG_LDW_02, MSG_ACC_19, MSG_ACC_18, MSG_TA_01),
2: (MSG_KLR_01,)}
ALLOW_OVERRIDE = True
ACCEL_OVERRIDE = 0
INACTIVE_ACCEL = 3.01
def setUp(self):
self.packer = CANPackerSafety("vw_meb_generated")
self.safety = libsafety_py.libsafety
self.safety.set_safety_hooks(CarParams.SafetyModel.volkswagenMeb, VolkswagenSafetyFlags.LONG_CONTROL)
self.safety.init_tests()
# stock cruise controls are entirely bypassed under openpilot longitudinal control
def test_disable_control_allowed_from_cruise(self):
pass
def test_enable_control_allowed_from_cruise(self):
pass
def test_cruise_engaged_prev(self):
pass
def test_set_and_resume_buttons(self):
for button in ["set", "resume"]:
# ACC main switch must be on, engage on falling edge
self.safety.set_controls_allowed(0)
self._rx(self._tsk_status_msg(False, main_switch=False))
self._rx(self._button_msg(_set=(button == "set"), resume=(button == "resume"), bus=0))
self.assertFalse(self.safety.get_controls_allowed(), f"controls allowed on {button} with main switch off")
self._rx(self._tsk_status_msg(False, main_switch=True))
self._rx(self._button_msg(_set=(button == "set"), resume=(button == "resume"), bus=0))
self.assertFalse(self.safety.get_controls_allowed(), f"controls allowed on {button} rising edge")
self._rx(self._button_msg(bus=0))
self.assertTrue(self.safety.get_controls_allowed(), f"controls not allowed on {button} falling edge")
def test_cancel_button(self):
# Disable on rising edge of cancel button
self._rx(self._tsk_status_msg(False, main_switch=True))
self.safety.set_controls_allowed(1)
self._rx(self._button_msg(cancel=True, bus=0))
self.assertFalse(self.safety.get_controls_allowed(), "controls allowed after cancel")
def test_main_switch(self):
# Disable as soon as main switch turns off
self._rx(self._tsk_status_msg(False, main_switch=True))
self.safety.set_controls_allowed(1)
self._rx(self._tsk_status_msg(False, main_switch=False))
self.assertFalse(self.safety.get_controls_allowed(), "controls allowed after ACC main switch off")
def test_accel_safety_check(self):
for controls_allowed in [True, False]:
for accel in np.concatenate((np.arange(MIN_ACCEL - 2, MAX_ACCEL + 2, 0.03), [0, self.INACTIVE_ACCEL])):
accel = round(accel, 2)
is_inactive_accel = accel == self.INACTIVE_ACCEL
send = (controls_allowed and MIN_ACCEL <= accel <= MAX_ACCEL) or is_inactive_accel
self.safety.set_controls_allowed(controls_allowed)
self.assertEqual(send, self._tx(self._accel_msg(accel)), (controls_allowed, accel))
def test_accel_override_with_gas(self):
if not self.ALLOW_OVERRIDE:
pass
self.safety.set_controls_allowed(True)
self.safety.set_gas_pressed_prev(True)
self.assertTrue(self._tx(self._accel_msg(self.ACCEL_OVERRIDE)))
self.assertFalse(self._tx(self._accel_msg(MAX_ACCEL)))
class TestVolkswagenMebGen2LongSafety(TestVolkswagenMebLongSafety):
def setUp(self):
self.packer = CANPackerSafety("vw_meb_2024_generated")
self.safety = libsafety_py.libsafety
self.safety.set_safety_hooks(CarParams.SafetyModel.volkswagenMeb,
VolkswagenSafetyFlags.LONG_CONTROL | VolkswagenSafetyFlags.MEB_ALT_CRC)
self.safety.init_tests()
# ZAS_Kl_15=1
class TestVolkswagenMebIgnition(unittest.TestCase):
TX_MSGS: list = []
def setUp(self):
self.safety = libsafety_py.libsafety
self.safety.init_tests()
self.packer = CANPackerSafety("vw_meb_generated")
def _msg(self, counter, ign):
return self.packer.make_can_msg_safety("Klemmen_Status_01", 0,
{"Klemmen_Status_01_BZ": counter,
"ZAS_Kl_15": ign})
def test_ignition_on(self):
for i in range(16):
self.safety.init_tests()
self.safety.ignition_can_hook(self._msg(i, 1))
self.assertFalse(self.safety.get_ignition_can())
self.safety.ignition_can_hook(self._msg((i + 1) % 16, 1))
self.assertTrue(self.safety.get_ignition_can())
def test_ignition_off(self):
self.safety.ignition_can_hook(self._msg(0, 1))
self.safety.ignition_can_hook(self._msg(1, 1))
self.assertTrue(self.safety.get_ignition_can())
self.safety.ignition_can_hook(self._msg(2, 0))
self.safety.ignition_can_hook(self._msg(3, 0))
self.assertFalse(self.safety.get_ignition_can())
if __name__ == "__main__":
unittest.main()
+141
View File
@@ -0,0 +1,141 @@
#!/usr/bin/env python3
import unittest
from opendbc.car.structs import CarParams
from opendbc.safety.tests.libsafety import libsafety_py
import opendbc.safety.tests.common as common
from opendbc.safety.tests.common import CANPackerSafety
MSG_LH_EPS_03 = 0x9F # RX from EPS, for driver steering torque
MSG_ESP_03 = 0x103 # RX from ABS, for wheel speeds
MSG_MOTOR_03 = 0x105 # RX from ECU, for driver throttle input and driver brake input
MSG_ESP_05 = 0x106 # RX from ABS, for brake light state
MSG_LS_01 = 0x10B # TX by OP, ACC control buttons for cancel/resume
MSG_TSK_04 = 0x10E # RX from ECU, for ACC status from drivetrain coordinator
MSG_HCA_01 = 0x126 # TX by OP, Heading Control Assist steering torque
MSG_LDW_02 = 0x397 # TX by OP, Lane line recognition and text alerts
class TestVolkswagenMlbSafetyBase(common.CarSafetyTest, common.DriverTorqueSteeringSafetyTest):
RELAY_MALFUNCTION_ADDRS = {0: (MSG_HCA_01, MSG_LDW_02)}
MAX_RATE_UP = 9
MAX_RATE_DOWN = 10
MAX_TORQUE_LOOKUP = [0], [300]
MAX_RT_DELTA = 169
DRIVER_TORQUE_ALLOWANCE = 60
DRIVER_TORQUE_FACTOR = 3
# Wheel speeds _esp_03_msg
def _speed_msg(self, speed):
values = {"ESP_%s_Radgeschw" % s: speed for s in ["HL", "HR", "VL", "VR"]}
return self.packer.make_can_msg_safety("ESP_03", 0, values)
# Driver brake pressure over threshold
def _esp_05_msg(self, brake):
values = {"ESP_Fahrer_bremst": brake}
return self.packer.make_can_msg_safety("ESP_05", 0, values)
# Brake pedal switch
def _motor_03_msg(self, brake_signal=False, gas_signal=0):
values = {
"MO_Fahrer_bremst": brake_signal,
"MO_Fahrpedalrohwert_01": gas_signal,
}
return self.packer.make_can_msg_safety("Motor_03", 0, values)
def _user_brake_msg(self, brake):
return self._motor_03_msg(brake_signal=brake)
def _user_gas_msg(self, gas):
return self._motor_03_msg(gas_signal=gas)
# ACC engagement status
def _tsk_status_msg(self, enable):
values = {"TSK_Status_GRA_ACC_02": 1 if enable else 0}
return self.packer.make_can_msg_safety("TSK_04", 1, values)
def _pcm_status_msg(self, enable):
return self._tsk_status_msg(enable)
# Driver steering input torque
def _torque_driver_msg(self, torque):
values = {"EPS_Lenkmoment": abs(torque), "EPS_VZ_Lenkmoment": torque < 0}
return self.packer.make_can_msg_safety("LH_EPS_03", 0, values)
# openpilot steering output torque
def _torque_cmd_msg(self, torque, steer_req=1):
values = {"HCA_01_LM_Offset": abs(torque),
"HCA_01_LM_OffSign": torque < 0,
"HCA_01_Sendestatus": steer_req,
"HCA_01_Status_HCA": 7 if steer_req else 3}
return self.packer.make_can_msg_safety("HCA_01", 0, values)
# Cruise control buttons
def _ls_01_msg(self, cancel=0, resume=0, _set=0, bus=2):
values = {"LS_Abbrechen": cancel, "LS_Tip_Setzen": _set, "LS_Tip_Wiederaufnahme": resume}
return self.packer.make_can_msg_safety("LS_01", bus, values)
# Verify brake_pressed is true if either the switch or pressure threshold signals are true
def test_redundant_brake_signals(self):
test_combinations = [(True, True, True), (True, True, False), (True, False, True), (False, False, False)]
for brake_pressed, motor_03_signal, esp_05_signal in test_combinations:
self._rx(self._motor_03_msg(brake_signal=False))
self._rx(self._esp_05_msg(False))
self.assertFalse(self.safety.get_brake_pressed_prev())
self._rx(self._motor_03_msg(brake_signal=motor_03_signal))
self._rx(self._esp_05_msg(esp_05_signal))
self.assertEqual(brake_pressed, self.safety.get_brake_pressed_prev(),
f"expected {brake_pressed=} with {motor_03_signal=} and {esp_05_signal=}")
def test_torque_measurements(self):
# TODO: make this test work with all cars
self._rx(self._torque_driver_msg(50))
self._rx(self._torque_driver_msg(-50))
self._rx(self._torque_driver_msg(0))
self._rx(self._torque_driver_msg(0))
self._rx(self._torque_driver_msg(0))
self._rx(self._torque_driver_msg(0))
self.assertEqual(-50, self.safety.get_torque_driver_min())
self.assertEqual(50, self.safety.get_torque_driver_max())
self._rx(self._torque_driver_msg(0))
self.assertEqual(0, self.safety.get_torque_driver_max())
self.assertEqual(-50, self.safety.get_torque_driver_min())
self._rx(self._torque_driver_msg(0))
self.assertEqual(0, self.safety.get_torque_driver_max())
self.assertEqual(0, self.safety.get_torque_driver_min())
class TestVolkswagenMlbStockSafety(TestVolkswagenMlbSafetyBase):
TX_MSGS = [[MSG_HCA_01, 0], [MSG_LDW_02, 0], [MSG_LS_01, 0], [MSG_LS_01, 2]]
FWD_BLACKLISTED_ADDRS = {2: [MSG_HCA_01, MSG_LDW_02]}
FWD_BUS_LOOKUP = {0: 2, 2: 0}
def setUp(self):
self.packer = CANPackerSafety("vw_mlb")
self.safety = libsafety_py.libsafety
self.safety.set_safety_hooks(CarParams.SafetyModel.volkswagenMlb, 0)
self.safety.init_tests()
def test_spam_cancel_safety_check(self):
self.safety.set_controls_allowed(0)
self.assertTrue(self._tx(self._ls_01_msg(cancel=1)))
self.assertFalse(self._tx(self._ls_01_msg(resume=1)))
self.assertFalse(self._tx(self._ls_01_msg(_set=1)))
# do not block resume if we are engaged already
self.safety.set_controls_allowed(1)
self.assertTrue(self._tx(self._ls_01_msg(resume=1)))
def test_cancel_button(self):
# Disable on rising edge of cancel button
self._rx(self._tsk_status_msg(False))
self.safety.set_controls_allowed(1)
self._rx(self._ls_01_msg(cancel=True, bus=0))
self.assertFalse(self.safety.get_controls_allowed(), "controls allowed after cancel")
if __name__ == "__main__":
unittest.main()
+216
View File
@@ -0,0 +1,216 @@
#!/usr/bin/env python3
import unittest
import numpy as np
from opendbc.car.structs import CarParams
from opendbc.safety.tests.libsafety import libsafety_py
import opendbc.safety.tests.common as common
from opendbc.safety.tests.common import CANPackerSafety
from opendbc.car.volkswagen.values import VolkswagenSafetyFlags
MAX_ACCEL = 2.0
MIN_ACCEL = -3.5
MSG_ESP_19 = 0xB2 # RX from ABS, for wheel speeds
MSG_LH_EPS_03 = 0x9F # RX from EPS, for driver steering torque
MSG_ESP_05 = 0x106 # RX from ABS, for brake light state
MSG_TSK_06 = 0x120 # RX from ECU, for ACC status from drivetrain coordinator
MSG_MOTOR_20 = 0x121 # RX from ECU, for driver throttle input
MSG_ACC_06 = 0x122 # TX by OP, ACC control instructions to the drivetrain coordinator
MSG_HCA_01 = 0x126 # TX by OP, Heading Control Assist steering torque
MSG_GRA_ACC_01 = 0x12B # TX by OP, ACC control buttons for cancel/resume
MSG_ACC_07 = 0x12E # TX by OP, ACC control instructions to the drivetrain coordinator
MSG_ACC_02 = 0x30C # TX by OP, ACC HUD data to the instrument cluster
MSG_LDW_02 = 0x397 # TX by OP, Lane line recognition and text alerts
class TestVolkswagenMqbSafetyBase(common.CarSafetyTest, common.DriverTorqueSteeringSafetyTest):
RELAY_MALFUNCTION_ADDRS = {0: (MSG_HCA_01, MSG_LDW_02), 2: (MSG_LH_EPS_03,)}
MAX_RATE_UP = 4
MAX_RATE_DOWN = 10
MAX_TORQUE_LOOKUP = [0], [300]
MAX_RT_DELTA = 75
DRIVER_TORQUE_ALLOWANCE = 80
DRIVER_TORQUE_FACTOR = 3
# Wheel speeds _esp_19_msg
def _speed_msg(self, speed):
values = {"ESP_%s_Radgeschw_02" % s: speed for s in ["HL", "HR", "VL", "VR"]}
return self.packer.make_can_msg_safety("ESP_19", 0, values)
# Driver brake pressure over threshold
def _esp_05_msg(self, brake):
values = {"ESP_Fahrer_bremst": brake}
return self.packer.make_can_msg_safety("ESP_05", 0, values)
# Brake pedal switch
def _motor_14_msg(self, brake):
values = {"MO_Fahrer_bremst": brake}
return self.packer.make_can_msg_safety("Motor_14", 0, values)
def _user_brake_msg(self, brake):
return self._motor_14_msg(brake)
# Driver throttle input
def _user_gas_msg(self, gas):
values = {"MO_Fahrpedalrohwert_01": gas}
return self.packer.make_can_msg_safety("Motor_20", 0, values)
# ACC engagement status
def _tsk_status_msg(self, enable, main_switch=True):
if main_switch:
tsk_status = 3 if enable else 2
else:
tsk_status = 0
values = {"TSK_Status": tsk_status}
return self.packer.make_can_msg_safety("TSK_06", 0, values)
def _pcm_status_msg(self, enable):
return self._tsk_status_msg(enable)
# Driver steering input torque
def _torque_driver_msg(self, torque):
values = {"EPS_Lenkmoment": abs(torque), "EPS_VZ_Lenkmoment": torque < 0}
return self.packer.make_can_msg_safety("LH_EPS_03", 0, values)
# openpilot steering output torque
def _torque_cmd_msg(self, torque, steer_req=1):
values = {"HCA_01_LM_Offset": abs(torque), "HCA_01_LM_OffSign": torque < 0, "HCA_01_Sendestatus": steer_req}
return self.packer.make_can_msg_safety("HCA_01", 0, values)
# Cruise control buttons
def _gra_acc_01_msg(self, cancel=0, resume=0, _set=0, bus=2):
values = {"GRA_Abbrechen": cancel, "GRA_Tip_Setzen": _set, "GRA_Tip_Wiederaufnahme": resume}
return self.packer.make_can_msg_safety("GRA_ACC_01", bus, values)
# Acceleration request to drivetrain coordinator
def _acc_06_msg(self, accel):
values = {"ACC_Sollbeschleunigung_02": accel}
return self.packer.make_can_msg_safety("ACC_06", 0, values)
# Acceleration request to drivetrain coordinator
def _acc_07_msg(self, accel, secondary_accel=3.02):
values = {"ACC_Sollbeschleunigung_02": accel, "ACC_Folgebeschl": secondary_accel}
return self.packer.make_can_msg_safety("ACC_07", 0, values)
# Verify brake_pressed is true if either the switch or pressure threshold signals are true
def test_redundant_brake_signals(self):
test_combinations = [(True, True, True), (True, True, False), (True, False, True), (False, False, False)]
for brake_pressed, motor_14_signal, esp_05_signal in test_combinations:
self._rx(self._motor_14_msg(False))
self._rx(self._esp_05_msg(False))
self.assertFalse(self.safety.get_brake_pressed_prev())
self._rx(self._motor_14_msg(motor_14_signal))
self._rx(self._esp_05_msg(esp_05_signal))
self.assertEqual(brake_pressed, self.safety.get_brake_pressed_prev(),
f"expected {brake_pressed=} with {motor_14_signal=} and {esp_05_signal=}")
def test_torque_measurements(self):
# TODO: make this test work with all cars
self._rx(self._torque_driver_msg(50))
self._rx(self._torque_driver_msg(-50))
self._rx(self._torque_driver_msg(0))
self._rx(self._torque_driver_msg(0))
self._rx(self._torque_driver_msg(0))
self._rx(self._torque_driver_msg(0))
self.assertEqual(-50, self.safety.get_torque_driver_min())
self.assertEqual(50, self.safety.get_torque_driver_max())
self._rx(self._torque_driver_msg(0))
self.assertEqual(0, self.safety.get_torque_driver_max())
self.assertEqual(-50, self.safety.get_torque_driver_min())
self._rx(self._torque_driver_msg(0))
self.assertEqual(0, self.safety.get_torque_driver_max())
self.assertEqual(0, self.safety.get_torque_driver_min())
class TestVolkswagenMqbStockSafety(TestVolkswagenMqbSafetyBase):
TX_MSGS = [[MSG_HCA_01, 0], [MSG_LDW_02, 0], [MSG_LH_EPS_03, 2], [MSG_GRA_ACC_01, 0], [MSG_GRA_ACC_01, 2]]
FWD_BLACKLISTED_ADDRS = {0: [MSG_LH_EPS_03], 2: [MSG_HCA_01, MSG_LDW_02]}
def setUp(self):
self.packer = CANPackerSafety("vw_mqb")
self.safety = libsafety_py.libsafety
self.safety.set_safety_hooks(CarParams.SafetyModel.volkswagen, 0)
self.safety.init_tests()
def test_spam_cancel_safety_check(self):
self.safety.set_controls_allowed(0)
self.assertTrue(self._tx(self._gra_acc_01_msg(cancel=1)))
self.assertFalse(self._tx(self._gra_acc_01_msg(resume=1)))
self.assertFalse(self._tx(self._gra_acc_01_msg(_set=1)))
# do not block resume if we are engaged already
self.safety.set_controls_allowed(1)
self.assertTrue(self._tx(self._gra_acc_01_msg(resume=1)))
class TestVolkswagenMqbLongSafety(TestVolkswagenMqbSafetyBase):
TX_MSGS = [[MSG_HCA_01, 0], [MSG_LDW_02, 0], [MSG_LH_EPS_03, 2], [MSG_ACC_02, 0], [MSG_ACC_06, 0], [MSG_ACC_07, 0]]
FWD_BLACKLISTED_ADDRS = {0: [MSG_LH_EPS_03], 2: [MSG_HCA_01, MSG_LDW_02, MSG_ACC_02, MSG_ACC_06, MSG_ACC_07]}
RELAY_MALFUNCTION_ADDRS = {0: (MSG_HCA_01, MSG_LDW_02, MSG_ACC_02, MSG_ACC_06, MSG_ACC_07), 2: (MSG_LH_EPS_03,)}
INACTIVE_ACCEL = 3.01
def setUp(self):
self.packer = CANPackerSafety("vw_mqb")
self.safety = libsafety_py.libsafety
self.safety.set_safety_hooks(CarParams.SafetyModel.volkswagen, VolkswagenSafetyFlags.LONG_CONTROL)
self.safety.init_tests()
# stock cruise controls are entirely bypassed under openpilot longitudinal control
def test_disable_control_allowed_from_cruise(self):
pass
def test_enable_control_allowed_from_cruise(self):
pass
def test_cruise_engaged_prev(self):
pass
def test_set_and_resume_buttons(self):
for button in ["set", "resume"]:
# ACC main switch must be on, engage on falling edge
self.safety.set_controls_allowed(0)
self._rx(self._tsk_status_msg(False, main_switch=False))
self._rx(self._gra_acc_01_msg(_set=(button == "set"), resume=(button == "resume"), bus=0))
self.assertFalse(self.safety.get_controls_allowed(), f"controls allowed on {button} with main switch off")
self._rx(self._tsk_status_msg(False, main_switch=True))
self._rx(self._gra_acc_01_msg(_set=(button == "set"), resume=(button == "resume"), bus=0))
self.assertFalse(self.safety.get_controls_allowed(), f"controls allowed on {button} rising edge")
self._rx(self._gra_acc_01_msg(bus=0))
self.assertTrue(self.safety.get_controls_allowed(), f"controls not allowed on {button} falling edge")
def test_cancel_button(self):
# Disable on rising edge of cancel button
self._rx(self._tsk_status_msg(False, main_switch=True))
self.safety.set_controls_allowed(1)
self._rx(self._gra_acc_01_msg(cancel=True, bus=0))
self.assertFalse(self.safety.get_controls_allowed(), "controls allowed after cancel")
def test_main_switch(self):
# Disable as soon as main switch turns off
self._rx(self._tsk_status_msg(False, main_switch=True))
self.safety.set_controls_allowed(1)
self._rx(self._tsk_status_msg(False, main_switch=False))
self.assertFalse(self.safety.get_controls_allowed(), "controls allowed after ACC main switch off")
def test_accel_safety_check(self):
for controls_allowed in [True, False]:
# enforce we don't skip over 0 or inactive accel
for accel in np.concatenate((np.arange(MIN_ACCEL - 2, MAX_ACCEL + 2, 0.03), [0, self.INACTIVE_ACCEL])):
accel = round(accel, 2) # floats might not hit exact boundary conditions without rounding
is_inactive_accel = accel == self.INACTIVE_ACCEL
send = (controls_allowed and MIN_ACCEL <= accel <= MAX_ACCEL) or is_inactive_accel
self.safety.set_controls_allowed(controls_allowed)
# primary accel request used by ECU
self.assertEqual(send, self._tx(self._acc_06_msg(accel)), (controls_allowed, accel))
# additional accel request used by ABS/ESP
self.assertEqual(send, self._tx(self._acc_07_msg(accel)), (controls_allowed, accel))
# ensure the optional secondary accel field remains inactive for now
self.assertEqual(is_inactive_accel, self._tx(self._acc_07_msg(accel, secondary_accel=accel)), (controls_allowed, accel))
if __name__ == "__main__":
unittest.main()
+192
View File
@@ -0,0 +1,192 @@
#!/usr/bin/env python3
import unittest
from opendbc.car.volkswagen.values import VolkswagenSafetyFlags
from opendbc.car.structs import CarParams
from opendbc.safety.tests.libsafety import libsafety_py
import opendbc.safety.tests.common as common
from opendbc.safety.tests.common import CANPackerSafety
MSG_LENKHILFE_3 = 0x0D0 # RX from EPS, for steering angle and driver steering torque
MSG_HCA_1 = 0x0D2 # TX by OP, Heading Control Assist steering torque
MSG_BREMSE_1 = 0x1A0 # RX from ABS, for ego speed
MSG_MOTOR_2 = 0x288 # RX from ECU, for CC state and brake switch state
MSG_ACC_SYSTEM = 0x368 # TX by OP, longitudinal acceleration controls
MSG_MOTOR_3 = 0x380 # RX from ECU, for driver throttle input
MSG_GRA_NEU = 0x38A # TX by OP, ACC control buttons for cancel/resume
MSG_MOTOR_5 = 0x480 # RX from ECU, for ACC main switch state
MSG_ACC_GRA_ANZEIGE = 0x56A # TX by OP, ACC HUD
MSG_LDW_1 = 0x5BE # TX by OP, Lane line recognition and text alerts
class TestVolkswagenPqSafetyBase(common.CarSafetyTest, common.DriverTorqueSteeringSafetyTest):
cruise_engaged = False
RELAY_MALFUNCTION_ADDRS = {0: (MSG_HCA_1, MSG_LDW_1)}
MAX_RATE_UP = 6
MAX_RATE_DOWN = 10
MAX_TORQUE_LOOKUP = [0], [300]
MAX_RT_DELTA = 113
DRIVER_TORQUE_ALLOWANCE = 80
DRIVER_TORQUE_FACTOR = 3
def _set_prev_torque(self, t):
self.safety.set_desired_torque_last(t)
self.safety.set_rt_torque_last(t)
# Ego speed (Bremse_1)
def _speed_msg(self, speed):
values = {"BR1_Rad_kmh": speed}
return self.packer.make_can_msg_safety("Bremse_1", 0, values)
# Brake light switch (shared message Motor_2)
def _user_brake_msg(self, brake):
# since this signal is used for engagement status, preserve current state
return self._motor_2_msg(brake_pressed=brake, cruise_engaged=self.safety.get_controls_allowed())
# ACC engaged status (shared message Motor_2)
def _pcm_status_msg(self, enable):
self.__class__.cruise_engaged = enable
return self._motor_2_msg(cruise_engaged=enable)
# Acceleration request to drivetrain coordinator
def _accel_msg(self, accel):
values = {"ACS_Sollbeschl": accel}
return self.packer.make_can_msg_safety("ACC_System", 0, values)
# Driver steering input torque
def _torque_driver_msg(self, torque):
values = {"LH3_LM": abs(torque), "LH3_LMSign": torque < 0}
return self.packer.make_can_msg_safety("Lenkhilfe_3", 0, values)
# openpilot steering output torque
def _torque_cmd_msg(self, torque, steer_req=1, hca_status=7):
values = {"LM_Offset": abs(torque), "LM_OffSign": torque < 0, "HCA_Status": hca_status if steer_req else 3}
return self.packer.make_can_msg_safety("HCA_1", 0, values)
# ACC engagement and brake light switch status
# Called indirectly for compatibility with common.py tests
def _motor_2_msg(self, brake_pressed=False, cruise_engaged=False):
values = {"MO2_BLS": brake_pressed,
"MO2_Sta_GRA": cruise_engaged}
return self.packer.make_can_msg_safety("Motor_2", 0, values)
# ACC main switch status
def _motor_5_msg(self, main_switch=False):
values = {"MO5_GRA_Hauptsch": main_switch}
return self.packer.make_can_msg_safety("Motor_5", 0, values)
# Driver throttle input (Motor_3)
def _user_gas_msg(self, gas):
values = {"MO3_Pedalwert": gas}
return self.packer.make_can_msg_safety("Motor_3", 0, values)
# Cruise control buttons (GRA_Neu)
def _button_msg(self, _set=False, resume=False, cancel=False, bus=2):
values = {"GRA_Neu_Setzen": _set, "GRA_Recall": resume, "GRA_Abbrechen": cancel}
return self.packer.make_can_msg_safety("GRA_Neu", bus, values)
def test_torque_measurements(self):
# TODO: make this test work with all cars
self._rx(self._torque_driver_msg(50))
self._rx(self._torque_driver_msg(-50))
self._rx(self._torque_driver_msg(0))
self._rx(self._torque_driver_msg(0))
self._rx(self._torque_driver_msg(0))
self._rx(self._torque_driver_msg(0))
self.assertEqual(-50, self.safety.get_torque_driver_min())
self.assertEqual(50, self.safety.get_torque_driver_max())
self._rx(self._torque_driver_msg(0))
self.assertEqual(0, self.safety.get_torque_driver_max())
self.assertEqual(-50, self.safety.get_torque_driver_min())
self._rx(self._torque_driver_msg(0))
self.assertEqual(0, self.safety.get_torque_driver_max())
self.assertEqual(0, self.safety.get_torque_driver_min())
class TestVolkswagenPqStockSafety(TestVolkswagenPqSafetyBase):
# Transmit of GRA_Neu is allowed on bus 0 and 2 to keep compatibility with gateway and camera integration
TX_MSGS = [[MSG_HCA_1, 0], [MSG_GRA_NEU, 0], [MSG_GRA_NEU, 2], [MSG_LDW_1, 0]]
FWD_BLACKLISTED_ADDRS = {2: [MSG_HCA_1, MSG_LDW_1]}
def setUp(self):
self.packer = CANPackerSafety("vw_pq")
self.safety = libsafety_py.libsafety
self.safety.set_safety_hooks(CarParams.SafetyModel.volkswagenPq, 0)
self.safety.init_tests()
def test_spam_cancel_safety_check(self):
self.safety.set_controls_allowed(0)
self.assertTrue(self._tx(self._button_msg(cancel=True)))
self.assertFalse(self._tx(self._button_msg(resume=True)))
self.assertFalse(self._tx(self._button_msg(_set=True)))
# do not block resume if we are engaged already
self.safety.set_controls_allowed(1)
self.assertTrue(self._tx(self._button_msg(resume=True)))
class TestVolkswagenPqLongSafety(TestVolkswagenPqSafetyBase, common.LongitudinalAccelSafetyTest):
TX_MSGS = [[MSG_HCA_1, 0], [MSG_LDW_1, 0], [MSG_ACC_SYSTEM, 0], [MSG_ACC_GRA_ANZEIGE, 0]]
FWD_BLACKLISTED_ADDRS = {2: [MSG_HCA_1, MSG_LDW_1, MSG_ACC_SYSTEM, MSG_ACC_GRA_ANZEIGE]}
RELAY_MALFUNCTION_ADDRS = {0: (MSG_HCA_1, MSG_LDW_1, MSG_ACC_SYSTEM, MSG_ACC_GRA_ANZEIGE)}
INACTIVE_ACCEL = 3.01
def setUp(self):
self.packer = CANPackerSafety("vw_pq")
self.safety = libsafety_py.libsafety
self.safety.set_safety_hooks(CarParams.SafetyModel.volkswagenPq, VolkswagenSafetyFlags.LONG_CONTROL)
self.safety.init_tests()
# stock cruise controls are entirely bypassed under openpilot longitudinal control
def test_disable_control_allowed_from_cruise(self):
pass
def test_enable_control_allowed_from_cruise(self):
pass
def test_cruise_engaged_prev(self):
pass
def test_set_and_resume_buttons(self):
for button in ["set", "resume"]:
# ACC main switch must be on, engage on falling edge
self.safety.set_controls_allowed(0)
self._rx(self._motor_5_msg(main_switch=False))
self._rx(self._button_msg(_set=(button == "set"), resume=(button == "resume"), bus=0))
self._rx(self._button_msg(bus=0))
self.assertFalse(self.safety.get_controls_allowed(), f"controls allowed on {button} with main switch off")
self._rx(self._motor_5_msg(main_switch=True))
self._rx(self._button_msg(_set=(button == "set"), resume=(button == "resume"), bus=0))
self.assertFalse(self.safety.get_controls_allowed(), f"controls allowed on {button} rising edge")
self._rx(self._button_msg(bus=0))
self.assertTrue(self.safety.get_controls_allowed(), f"controls not allowed on {button} falling edge")
def test_cancel_button(self):
# Disable on rising edge of cancel button
self._rx(self._motor_5_msg(main_switch=True))
self.safety.set_controls_allowed(1)
self._rx(self._button_msg(cancel=True, bus=0))
self.assertFalse(self.safety.get_controls_allowed(), "controls allowed after cancel")
def test_main_switch(self):
# Disable as soon as main switch turns off
self._rx(self._motor_5_msg(main_switch=True))
self.safety.set_controls_allowed(1)
self._rx(self._motor_5_msg(main_switch=False))
self.assertFalse(self.safety.get_controls_allowed(), "controls allowed after ACC main switch off")
def test_torque_cmd_enable_variants(self):
# The EPS rack accepts either 5 or 7 for an enabled status, with different low speed tuning behavior
self.safety.set_controls_allowed(1)
for enabled_status in (5, 7):
self.assertTrue(self._tx(self._torque_cmd_msg(self.MAX_RATE_UP, steer_req=1, hca_status=enabled_status)),
f"torque cmd rejected with {enabled_status=}")
if __name__ == "__main__":
unittest.main()