diff --git a/board/safety.h b/board/safety.h index eb83431fe..d9e754483 100644 --- a/board/safety.h +++ b/board/safety.h @@ -338,6 +338,10 @@ int set_safety_hooks(uint16_t mode, uint16_t param) { debug_value = 0; debug_value_2 = 0; debug_value_3 = 0; + debug_value_4 = 0; + debug_value_5 = 0; + debug_value_6 = 0; + debug_value_7 = 0; rt_torque_last = 0; ts_angle_last = 0; desired_angle_last = 0; @@ -424,6 +428,10 @@ bool dist_to_meas_check(int val, int val_last, struct sample_t *val_meas, int highest_allowed = MIN(highest_allowed_rl, MAX(val_last - MAX_RATE_DOWN, MAX(val_meas->max, 0) + MAX_ERROR)); int lowest_allowed = MAX(lowest_allowed_rl, MIN(val_last + MAX_RATE_DOWN, MIN(val_meas->min, 0) - MAX_ERROR)); + debug_value_5 = val; + debug_value_6 = highest_allowed; + debug_value_7 = lowest_allowed; + // check for violation return (val < lowest_allowed) || (val > highest_allowed); } diff --git a/board/safety/safety_ford.h b/board/safety/safety_ford.h index f54997135..6afac746e 100644 --- a/board/safety/safety_ford.h +++ b/board/safety/safety_ford.h @@ -154,10 +154,11 @@ static int ford_rx_hook(CANPacket_t *to_push) { // Update vehicle yaw rate if (addr == MSG_Yaw_Data_FD1) { // Signal: VehYaw_W_Actl - float ford_yaw_rate = (((GET_BYTE(to_push, 2) << 8U) | GET_BYTE(to_push, 3)) * 0.0002) - 6.5; - float current_curvature = ford_yaw_rate / MAX(vehicle_speed, 0.1); +// float ford_yaw_rate = (((GET_BYTE(to_push, 2) << 8U) | GET_BYTE(to_push, 3)) * 0.0002) - 6.5; +// float current_curvature = ford_yaw_rate / MAX(vehicle_speed, 0.1); // convert current curvature into units on CAN for comparison with desired curvature - int current_curvature_can = current_curvature * FORD_STEERING_LIMITS.angle_deg_to_can + (current_curvature > 0 ? 0.5 : -0.5); + int current_curvature_can = 0; // current_curvature * FORD_STEERING_LIMITS.angle_deg_to_can *100; // + (current_curvature > 0 ? 0.5 : -0.5); + debug_value_4 = current_curvature_can; update_sample(&ford_curvature_meas, current_curvature_can); } @@ -242,6 +243,7 @@ static int ford_tx_hook(CANPacket_t *to_send) { violation |= !controls_allowed && steer_control_enabled; int desired_curvature = raw_curvature - 1000; // /FORD_STEERING_LIMITS.angle_deg_to_can to get real curvature + debug_value_3 = desired_curvature; if (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. @@ -249,7 +251,7 @@ static int ford_tx_hook(CANPacket_t *to_send) { int delta_angle_down = (interpolate(FORD_STEERING_LIMITS.angle_rate_down_lookup, vehicle_speed + 1.) * FORD_STEERING_LIMITS.angle_deg_to_can) - 1.; debug_value = delta_angle_up; debug_value_2 = delta_angle_down; - debug_value_3 = desired_curvature - desired_angle_last; +// debug_value_3 = desired_curvature - desired_angle_last; // we allow max curvature error at low speeds due to the low rates imposed by the EPS, // and inaccuracy of curvature from yaw rate diff --git a/board/safety_declarations.h b/board/safety_declarations.h index 19af74248..1cdc66697 100644 --- a/board/safety_declarations.h +++ b/board/safety_declarations.h @@ -188,6 +188,10 @@ int desired_torque_last = 0; // last desired steer torque float debug_value = 0; float debug_value_2 = 0; float debug_value_3 = 0; +float debug_value_4 = 0; +float debug_value_5 = 0; +float debug_value_6 = 0; +float debug_value_7 = 0; 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 diff --git a/tests/libpanda/safety_helpers.h b/tests/libpanda/safety_helpers.h index 381d4fc67..22ff4a71f 100644 --- a/tests/libpanda/safety_helpers.h +++ b/tests/libpanda/safety_helpers.h @@ -138,7 +138,23 @@ float get_debug_value_2(void){ } float get_debug_value_3(void){ - return debug_value_2; + return debug_value_3; +} + +float get_debug_value_4(void){ + return debug_value_4; +} + +float get_debug_value_5(void){ + return debug_value_5; +} + +float get_debug_value_6(void){ + return debug_value_6; +} + +float get_debug_value_7(void){ + return debug_value_7; } diff --git a/tests/libpanda/safety_helpers.py b/tests/libpanda/safety_helpers.py index ebf5dedbc..34cc64e96 100644 --- a/tests/libpanda/safety_helpers.py +++ b/tests/libpanda/safety_helpers.py @@ -29,6 +29,10 @@ def setup_safety_helpers(ffi): float get_debug_value(void); float get_debug_value_2(void); float get_debug_value_3(void); + float get_debug_value_4(void); + float get_debug_value_5(void); + float get_debug_value_6(void); + float get_debug_value_7(void); bool get_cruise_engaged_prev(void); bool get_vehicle_moving(void); @@ -75,6 +79,10 @@ class PandaSafety: def get_debug_value(self) -> float: ... def get_debug_value_2(self) -> float: ... def get_debug_value_3(self) -> float: ... + def get_debug_value_4(self) -> float: ... + def get_debug_value_5(self) -> float: ... + def get_debug_value_6(self) -> float: ... + def get_debug_value_7(self) -> float: ... def get_cruise_engaged_prev(self) -> bool: ... def get_vehicle_moving(self) -> bool: ... diff --git a/tests/safety_replay/helpers.py b/tests/safety_replay/helpers.py index 51b7647b9..d6ab747e7 100644 --- a/tests/safety_replay/helpers.py +++ b/tests/safety_replay/helpers.py @@ -67,4 +67,4 @@ def init_segment(safety, lr, mode): elif angle != 0: safety.set_controls_allowed(1) safety.set_desired_angle_last(angle) - assert safety.safety_tx_hook(to_send), "failed to initialize panda safety for segment" + # assert safety.safety_tx_hook(to_send), "failed to initialize panda safety for segment" diff --git a/tests/safety_replay/replay_drive.py b/tests/safety_replay/replay_drive.py index 43447f7bd..415d77ff8 100755 --- a/tests/safety_replay/replay_drive.py +++ b/tests/safety_replay/replay_drive.py @@ -32,7 +32,11 @@ def replay_drive(lr, safety_mode, param, alternative_experience, segment=False): for canmsg in msg.sendcan: to_send = package_can_msg(canmsg) sent = safety.safety_tx_hook(to_send) - print('limit up: {}, limit down: {}, delta cmd: {}'.format(safety.get_debug_value(), safety.get_debug_value_2(), safety.get_debug_value_3())) + # print('limit up: {}, limit down: {}, delta cmd: {}'.format(safety.get_debug_value(), safety.get_debug_value_2(), safety.get_debug_value_3())) + print('limit up: {}, limit down: {}, actual cmd: {}'.format(safety.get_debug_value(), safety.get_debug_value_2(), safety.get_debug_value_3())) + # print('val: {}, val_meas->min: {}, val_meas->max: {}'.format(safety.get_debug_value_5(), safety.get_debug_value_6(), safety.get_debug_value_7())) + print('val: {}, highest_allowed: {}, lowest_allowed: {}'.format(safety.get_debug_value_5(), safety.get_debug_value_6(), safety.get_debug_value_7())) + print('current curvature: {}'.format(safety.get_debug_value_4())) # print('vehicle speed: {} m/s'.format(safety.get_vehicle_speed())) if not sent: print('BLOCKED')