add debugging stuff

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);
 }

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

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

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;
 }
 
 

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: ...

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"

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')