Skip to content

Commit

Permalink
patch of pycap
Browse files Browse the repository at this point in the history
  • Loading branch information
Shaikimram committed Jan 12, 2025
1 parent 55a45ce commit b33c651
Show file tree
Hide file tree
Showing 17 changed files with 60 additions and 59 deletions.
12 changes: 6 additions & 6 deletions common/pid.py
Original file line number Diff line number Diff line change
Expand Up @@ -25,15 +25,15 @@ def __init__(self, k_p, k_i, k_f=0., k_d=0., pos_limit=1e308, neg_limit=-1e308,

@property
def k_p(self):
return np.interp(self.speed, self._k_p[0], self._k_p[1])
return float(np.interp(self.speed, self._k_p[0], self._k_p[1]))

@property
def k_i(self):
return np.interp(self.speed, self._k_i[0], self._k_i[1])
return float(np.interp(self.speed, self._k_i[0], self._k_i[1]))

@property
def k_d(self):
return np.interp(self.speed, self._k_d[0], self._k_d[1])
return float(np.interp(self.speed, self._k_d[0], self._k_d[1]))

@property
def error_integral(self):
Expand Down Expand Up @@ -61,10 +61,10 @@ def update(self, error, error_rate=0.0, speed=0.0, override=False, feedforward=0

# Clip i to prevent exceeding control limits
control_no_i = self.p + self.d + self.f
control_no_i = np.clip(control_no_i, self.neg_limit, self.pos_limit)
self.i = np.clip(self.i, self.neg_limit - control_no_i, self.pos_limit - control_no_i)
control_no_i = float(np.clip(control_no_i, self.neg_limit, self.pos_limit))
self.i = float(np.clip(self.i, self.neg_limit - control_no_i, self.pos_limit - control_no_i))

control = self.p + self.i + self.d + self.f

self.control = np.clip(control, self.neg_limit, self.pos_limit)
self.control = float(np.clip(control, self.neg_limit, self.pos_limit))
return self.control
8 changes: 4 additions & 4 deletions common/tests/test_numpy_fast.py
Original file line number Diff line number Diff line change
Expand Up @@ -9,12 +9,12 @@ def test_correctness_controls(self):
v_ego_arr = [-1, -1e-12, 0, 4, 5, 6, 7, 10, 11, 15.2, 20, 21, 39,
39.999999, 40, 41]

expected = np.interp(v_ego_arr, _A_CRUISE_MIN_BP, _A_CRUISE_MIN_V)
actual = np.interp(v_ego_arr, _A_CRUISE_MIN_BP, _A_CRUISE_MIN_V)
expected = float(np.interp(v_ego_arr, _A_CRUISE_MIN_BP, _A_CRUISE_MIN_V))
actual = float(np.interp(v_ego_arr, _A_CRUISE_MIN_BP, _A_CRUISE_MIN_V))

np.testing.assert_equal(actual, expected)

for v_ego in v_ego_arr:
expected = np.interp(v_ego, _A_CRUISE_MIN_BP, _A_CRUISE_MIN_V)
actual = np.interp(v_ego, _A_CRUISE_MIN_BP, _A_CRUISE_MIN_V)
expected = float(np.interp(v_ego, _A_CRUISE_MIN_BP, _A_CRUISE_MIN_V))
actual = float(np.interp(v_ego, _A_CRUISE_MIN_BP, _A_CRUISE_MIN_V))
np.testing.assert_equal(actual, expected)
4 changes: 2 additions & 2 deletions selfdrive/car/cruise.py
Original file line number Diff line number Diff line change
Expand Up @@ -106,7 +106,7 @@ def _update_v_cruise_non_pcm(self, CS, enabled, is_metric):
if CS.gasPressed and button_type in (ButtonType.decelCruise, ButtonType.setCruise):
self.v_cruise_kph = max(self.v_cruise_kph, CS.vEgo * CV.MS_TO_KPH)

self.v_cruise_kph = np.clip(round(self.v_cruise_kph, 1), V_CRUISE_MIN, V_CRUISE_MAX)
self.v_cruise_kph = float(np.clip(round(self.v_cruise_kph, 1), V_CRUISE_MIN, V_CRUISE_MAX))

def update_button_timers(self, CS, enabled):
# increment timer for buttons still pressed
Expand All @@ -130,6 +130,6 @@ def initialize_v_cruise(self, CS, experimental_mode: bool) -> None:
if any(b.type in (ButtonType.accelCruise, ButtonType.resumeCruise) for b in CS.buttonEvents) and self.v_cruise_initialized:
self.v_cruise_kph = self.v_cruise_kph_last
else:
self.v_cruise_kph = int(round(np.clip(CS.vEgo * CV.MS_TO_KPH, initial, V_CRUISE_MAX)))
self.v_cruise_kph = int(round(float(np.clip(CS.vEgo * CV.MS_TO_KPH, initial, V_CRUISE_MAX))))

self.v_cruise_cluster_kph = self.v_cruise_kph
8 changes: 4 additions & 4 deletions selfdrive/controls/lib/drive_helpers.py
Original file line number Diff line number Diff line change
Expand Up @@ -13,19 +13,19 @@
MAX_VEL_ERR = 5.0

def clip_curvature(v_ego, prev_curvature, new_curvature):
new_curvature = np.clip(new_curvature, -MAX_CURVATURE, MAX_CURVATURE)
new_curvature = float(np.clip(new_curvature, -MAX_CURVATURE, MAX_CURVATURE))
v_ego = max(MIN_SPEED, v_ego)
max_curvature_rate = MAX_LATERAL_JERK / (v_ego**2) # inexact calculation, check https://github.com/commaai/openpilot/pull/24755
safe_desired_curvature = np.clip(new_curvature,
safe_desired_curvature = float(np.clip(new_curvature,
prev_curvature - max_curvature_rate * DT_CTRL,
prev_curvature + max_curvature_rate * DT_CTRL)
prev_curvature + max_curvature_rate * DT_CTRL))

return safe_desired_curvature


def get_speed_error(modelV2: log.ModelDataV2, v_ego: float) -> float:
# ToDo: Try relative error, and absolute speed
if len(modelV2.temporalPose.trans):
vel_err = np.clip(modelV2.temporalPose.trans[0] - v_ego, -MAX_VEL_ERR, MAX_VEL_ERR)
vel_err = float(np.clip(modelV2.temporalPose.trans[0] - v_ego, -MAX_VEL_ERR, MAX_VEL_ERR))
return float(vel_err)
return 0.0
2 changes: 1 addition & 1 deletion selfdrive/controls/lib/latcontrol.py
Original file line number Diff line number Diff line change
Expand Up @@ -28,5 +28,5 @@ def _check_saturation(self, saturated, CS, steer_limited):
self.sat_count += self.sat_count_rate
else:
self.sat_count -= self.sat_count_rate
self.sat_count = np.clip(self.sat_count, 0.0, self.sat_limit)
self.sat_count = float(np.clip(self.sat_count, 0.0, self.sat_limit))
return self.sat_count > (self.sat_limit - 1e-3)
4 changes: 2 additions & 2 deletions selfdrive/controls/lib/latcontrol_torque.py
Original file line number Diff line number Diff line change
Expand Up @@ -51,7 +51,7 @@ def update(self, active, CS, VM, params, steer_limited, desired_curvature, calib
else:
assert calibrated_pose is not None
actual_curvature_pose = calibrated_pose.angular_velocity.yaw / CS.vEgo
actual_curvature = np.interp(CS.vEgo, [2.0, 5.0], [actual_curvature_vm, actual_curvature_pose])
actual_curvature = float(np.interp(CS.vEgo, [2.0, 5.0], [actual_curvature_vm, actual_curvature_pose]))
curvature_deadzone = 0.0
desired_lateral_accel = desired_curvature * CS.vEgo ** 2

Expand All @@ -60,7 +60,7 @@ def update(self, active, CS, VM, params, steer_limited, desired_curvature, calib
actual_lateral_accel = actual_curvature * CS.vEgo ** 2
lateral_accel_deadzone = curvature_deadzone * CS.vEgo ** 2

low_speed_factor = np.interp(CS.vEgo, LOW_SPEED_X, LOW_SPEED_Y)**2
low_speed_factor = float(np.interp(CS.vEgo, LOW_SPEED_X, LOW_SPEED_Y))**2
setpoint = desired_lateral_accel + low_speed_factor * desired_curvature
measurement = actual_lateral_accel + low_speed_factor * actual_curvature
gravity_adjusted_lateral_accel = desired_lateral_accel - roll_compensation
Expand Down
2 changes: 1 addition & 1 deletion selfdrive/controls/lib/longcontrol.py
Original file line number Diff line number Diff line change
Expand Up @@ -84,5 +84,5 @@ def update(self, active, CS, a_target, should_stop, accel_limits):
output_accel = self.pid.update(error, speed=CS.vEgo,
feedforward=a_target)

self.last_output_accel = np.clip(output_accel, accel_limits[0], accel_limits[1])
self.last_output_accel = float(np.clip(output_accel, accel_limits[0], accel_limits[1]))
return self.last_output_accel
14 changes: 7 additions & 7 deletions selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py
Original file line number Diff line number Diff line change
Expand Up @@ -297,7 +297,7 @@ def set_cur_state(self, v, a):
@staticmethod
def extrapolate_lead(x_lead, v_lead, a_lead, a_lead_tau):
a_lead_traj = a_lead * np.exp(-a_lead_tau * (T_IDXS**2)/2.)
v_lead_traj = np.clip(v_lead + np.cumsum(T_DIFFS * a_lead_traj), 0.0, 1e8)
v_lead_traj = float(np.clip(v_lead + np.cumsum(T_DIFFS * a_lead_traj), 0.0, 1e8))
x_lead_traj = x_lead + np.cumsum(T_DIFFS * v_lead_traj)
lead_xv = np.column_stack((x_lead_traj, v_lead_traj))
return lead_xv
Expand All @@ -319,9 +319,9 @@ def process_lead(self, lead):
# MPC will not converge if immediate crash is expected
# Clip lead distance to what is still possible to brake for
min_x_lead = ((v_ego + v_lead)/2) * (v_ego - v_lead) / (-ACCEL_MIN * 2)
x_lead = np.clip(x_lead, min_x_lead, 1e8)
v_lead = np.clip(v_lead, 0.0, 1e8)
a_lead = np.clip(a_lead, -10., 5.)
x_lead = float(np.clip(x_lead, min_x_lead, 1e8))
v_lead = float(np.clip(v_lead, 0.0, 1e8))
a_lead = float(np.clip(a_lead, -10., 5.))
lead_xv = self.extrapolate_lead(x_lead, v_lead, a_lead, a_lead_tau)
return lead_xv

Expand Down Expand Up @@ -358,9 +358,9 @@ def update(self, radarstate, v_cruise, x, v, a, j, personality=log.LongitudinalP
v_lower = v_ego + (T_IDXS * self.cruise_min_a * 1.05)
# TODO does this make sense when max_a is negative?
v_upper = v_ego + (T_IDXS * self.max_a * 1.05)
v_cruise_clipped = np.clip(v_cruise * np.ones(N+1),
v_cruise_clipped = float(np.clip(v_cruise * np.ones(N+1),
v_lower,
v_upper)
v_upper))
cruise_obstacle = np.cumsum(T_DIFFS * v_cruise_clipped) + get_safe_obstacle_distance(v_cruise_clipped, t_follow)
x_obstacles = np.column_stack([lead_0_obstacle, lead_1_obstacle, cruise_obstacle])
self.source = SOURCES[np.argmin(x_obstacles[0])]
Expand All @@ -373,7 +373,7 @@ def update(self, radarstate, v_cruise, x, v, a, j, personality=log.LongitudinalP

x_obstacles = np.column_stack([lead_0_obstacle,
lead_1_obstacle])
cruise_target = T_IDXS * np.clip(v_cruise, v_ego - 2.0, 1e3) + x[0]
cruise_target = T_IDXS * float(np.clip(v_cruise, v_ego - 2.0, 1e3)) + x[0]
xforward = ((v[1:] + v[:-1]) / 2) * (T_IDXS[1:] - T_IDXS[:-1])
x = np.cumsum(np.insert(xforward, 0, x[0]))

Expand Down
24 changes: 12 additions & 12 deletions selfdrive/controls/lib/longitudinal_planner.py
Original file line number Diff line number Diff line change
Expand Up @@ -30,7 +30,7 @@


def get_max_accel(v_ego):
return np.interp(v_ego, A_CRUISE_MAX_BP, A_CRUISE_MAX_VALS)
return float(np.interp(v_ego, A_CRUISE_MAX_BP, A_CRUISE_MAX_VALS))

def get_coast_accel(pitch):
return np.sin(pitch) * -5.65 - 0.3 # fitted from data using xx/projects/allow_throttle/compute_coast_accel.py
Expand All @@ -43,7 +43,7 @@ def limit_accel_in_turns(v_ego, angle_steers, a_target, CP):
"""
# FIXME: This function to calculate lateral accel is incorrect and should use the VehicleModel
# The lookup table for turns should also be updated if we do this
a_total_max = np.interp(v_ego, _A_TOTAL_MAX_BP, _A_TOTAL_MAX_V)
a_total_max = float(np.interp(v_ego, _A_TOTAL_MAX_BP, _A_TOTAL_MAX_V))
a_y = v_ego ** 2 * angle_steers * CV.DEG_TO_RAD / (CP.steerRatio * CP.wheelbase)
a_x_allowed = math.sqrt(max(a_total_max ** 2 - a_y ** 2, 0.))

Expand All @@ -55,9 +55,9 @@ def get_accel_from_plan(speeds, accels, action_t=DT_MDL, vEgoStopping=0.05):
v_now = speeds[0]
a_now = accels[0]

v_target = np.interp(action_t, CONTROL_N_T_IDX, speeds)
v_target = float(np.interp(action_t, CONTROL_N_T_IDX, speeds))
a_target = 2 * (v_target - v_now) / (action_t) - a_now
v_target_1sec = np.interp(action_t + 1.0, CONTROL_N_T_IDX, speeds)
v_target_1sec = float(np.interp(action_t + 1.0, CONTROL_N_T_IDX, speeds))
else:
v_target = 0.0
v_target_1sec = 0.0
Expand Down Expand Up @@ -89,9 +89,9 @@ def parse_model(model_msg, model_error):
if (len(model_msg.position.x) == ModelConstants.IDX_N and
len(model_msg.velocity.x) == ModelConstants.IDX_N and
len(model_msg.acceleration.x) == ModelConstants.IDX_N):
x = np.interp(T_IDXS_MPC, ModelConstants.T_IDXS, model_msg.position.x) - model_error * T_IDXS_MPC
v = np.interp(T_IDXS_MPC, ModelConstants.T_IDXS, model_msg.velocity.x) - model_error
a = np.interp(T_IDXS_MPC, ModelConstants.T_IDXS, model_msg.acceleration.x)
x = float(np.interp(T_IDXS_MPC, ModelConstants.T_IDXS, model_msg.position.x)) - model_error * T_IDXS_MPC
v = float(np.interp(T_IDXS_MPC, ModelConstants.T_IDXS, model_msg.velocity.x)) - model_error
a = float(np.interp(T_IDXS_MPC, ModelConstants.T_IDXS, model_msg.acceleration.x))
j = np.zeros(len(T_IDXS_MPC))
else:
x = np.zeros(len(T_IDXS_MPC))
Expand Down Expand Up @@ -139,7 +139,7 @@ def update(self, sm):
if reset_state:
self.v_desired_filter.x = v_ego
# Clip aEgo to cruise limits to prevent large accelerations when becoming active
self.a_desired = np.clip(sm['carState'].aEgo, accel_limits[0], accel_limits[1])
self.a_desired = float(np.clip(sm['carState'].aEgo, accel_limits[0], accel_limits[1]))

# Prevent divergence, smooth in current v_ego
self.v_desired_filter.x = max(0.0, self.v_desired_filter.update(v_ego))
Expand All @@ -151,7 +151,7 @@ def update(self, sm):

if not self.allow_throttle:
clipped_accel_coast = max(accel_coast, accel_limits_turns[0])
clipped_accel_coast_interp = np.interp(v_ego, [MIN_ALLOW_THROTTLE_SPEED, MIN_ALLOW_THROTTLE_SPEED*2], [accel_limits_turns[1], clipped_accel_coast])
clipped_accel_coast_interp = float(np.interp(v_ego, [MIN_ALLOW_THROTTLE_SPEED, MIN_ALLOW_THROTTLE_SPEED*2], [accel_limits_turns[1], clipped_accel_coast]))
accel_limits_turns[1] = min(accel_limits_turns[1], clipped_accel_coast_interp)

if force_slow_decel:
Expand All @@ -165,9 +165,9 @@ def update(self, sm):
self.mpc.set_cur_state(self.v_desired_filter.x, self.a_desired)
self.mpc.update(sm['radarState'], v_cruise, x, v, a, j, personality=sm['selfdriveState'].personality)

self.v_desired_trajectory = np.interp(CONTROL_N_T_IDX, T_IDXS_MPC, self.mpc.v_solution)
self.a_desired_trajectory = np.interp(CONTROL_N_T_IDX, T_IDXS_MPC, self.mpc.a_solution)
self.j_desired_trajectory = np.interp(CONTROL_N_T_IDX, T_IDXS_MPC[:-1], self.mpc.j_solution)
self.v_desired_trajectory = float(np.interp(CONTROL_N_T_IDX, T_IDXS_MPC, self.mpc.v_solution))
self.a_desired_trajectory = float(np.interp(CONTROL_N_T_IDX, T_IDXS_MPC, self.mpc.a_solution))
self.j_desired_trajectory = float(np.interp(CONTROL_N_T_IDX, T_IDXS_MPC[:-1], self.mpc.j_solution))

# TODO counter is only needed because radar is glitchy, remove once radar is gone
self.fcw = self.mpc.crash_cnt > 2 and not sm['carState'].standstill
Expand Down
3 changes: 2 additions & 1 deletion selfdrive/controls/radard.py
Original file line number Diff line number Diff line change
Expand Up @@ -44,7 +44,8 @@ def __init__(self, dt: float):
0.28144091, 0.27958406, 0.27783249, 0.27617149, 0.27458948, 0.27307714,
0.27162685, 0.27023228, 0.26888809, 0.26758976, 0.26633338, 0.26511557,
0.26393339, 0.26278425]
self.K = [[np.interp(dt, dts, K0)], [np.interp(dt, dts, K1)]]
self.K = [[float(np.interp(dt, dts, K0))], [float(np.interp(dt, dts, K1))]]



class Track:
Expand Down
4 changes: 2 additions & 2 deletions selfdrive/debug/live_cpu_and_temp.py
Original file line number Diff line number Diff line change
Expand Up @@ -49,7 +49,7 @@ def proc_name(proc):

if sm.updated['deviceState']:
t = sm['deviceState']
last_temp = np.mean(t.cpuTempC)
last_temp = float(np.mean(t.cpuTempC))
last_mem = t.memoryUsagePercent

if sm.updated['procLog']:
Expand All @@ -72,7 +72,7 @@ def proc_name(proc):
total_times = total_times_new[:]
busy_times = busy_times_new[:]

print(f"CPU {100.0 * np.mean(cores):.2f}% - RAM: {last_mem:.2f}% - Temp {last_temp:.2f}C")
print(f"CPU {100.0 * float(np.mean(cores)):.2f}% - RAM: {last_mem:.2f}% - Temp {last_temp:.2f}C")

if args.cpu and prev_proclog is not None and prev_proclog_t is not None:
procs: dict[str, float] = defaultdict(float)
Expand Down
12 changes: 6 additions & 6 deletions selfdrive/locationd/paramsd.py
Original file line number Diff line number Diff line change
Expand Up @@ -65,7 +65,7 @@ def handle_log(self, t, which, msg):
# This is done to bound the road roll estimate when localizer values are invalid
roll = 0.0
roll_std = np.radians(10.0)
self.roll = np.clip(roll, self.roll - ROLL_MAX_DELTA, self.roll + ROLL_MAX_DELTA)
self.roll = float(np.clip(roll, self.roll - ROLL_MAX_DELTA, self.roll + ROLL_MAX_DELTA))

yaw_rate_valid = msg.angularVelocityDevice.valid and self.calibrator.calib_valid
yaw_rate_valid = yaw_rate_valid and 0 < self.yaw_rate_std < 10 # rad/s
Expand Down Expand Up @@ -202,11 +202,11 @@ def main():
learner = ParamsLearner(CP, CP.steerRatio, 1.0, 0.0)
x = learner.kf.x

angle_offset_average = np.clip(math.degrees(x[States.ANGLE_OFFSET].item()),
angle_offset_average - MAX_ANGLE_OFFSET_DELTA, angle_offset_average + MAX_ANGLE_OFFSET_DELTA)
angle_offset = np.clip(math.degrees(x[States.ANGLE_OFFSET].item() + x[States.ANGLE_OFFSET_FAST].item()),
angle_offset - MAX_ANGLE_OFFSET_DELTA, angle_offset + MAX_ANGLE_OFFSET_DELTA)
roll = np.clip(float(x[States.ROAD_ROLL].item()), roll - ROLL_MAX_DELTA, roll + ROLL_MAX_DELTA)
angle_offset_average = float(np.clip(math.degrees(x[States.ANGLE_OFFSET].item()),
angle_offset_average - MAX_ANGLE_OFFSET_DELTA, angle_offset_average + MAX_ANGLE_OFFSET_DELTA))
angle_offset = float(np.clip(math.degrees(x[States.ANGLE_OFFSET].item() + x[States.ANGLE_OFFSET_FAST].item()),
angle_offset - MAX_ANGLE_OFFSET_DELTA, angle_offset + MAX_ANGLE_OFFSET_DELTA))
roll = float(np.clip(float(x[States.ROAD_ROLL].item()), roll - ROLL_MAX_DELTA, roll + ROLL_MAX_DELTA))
roll_std = float(P[States.ROAD_ROLL].item())
if learner.active and learner.speed > LOW_ACTIVE_SPEED:
# Account for the opposite signs of the yaw rates
Expand Down
8 changes: 4 additions & 4 deletions selfdrive/monitoring/helpers.py
Original file line number Diff line number Diff line change
Expand Up @@ -205,12 +205,12 @@ def _set_policy(self, model_data, car_speed):
bp = model_data.meta.disengagePredictions.brakeDisengageProbs[0] # brake disengage prob in next 2s
k1 = max(-0.00156*((car_speed-16)**2)+0.6, 0.2)
bp_normal = max(min(bp / k1, 0.5),0)
self.pose.cfactor_pitch = np.interp(bp_normal, [0, 0.5],
self.pose.cfactor_pitch = float(np.interp(bp_normal, [0, 0.5],
[self.settings._POSE_PITCH_THRESHOLD_SLACK,
self.settings._POSE_PITCH_THRESHOLD_STRICT]) / self.settings._POSE_PITCH_THRESHOLD
self.pose.cfactor_yaw = np.interp(bp_normal, [0, 0.5],
self.settings._POSE_PITCH_THRESHOLD_STRICT])) / self.settings._POSE_PITCH_THRESHOLD
self.pose.cfactor_yaw = float(np.interp(bp_normal, [0, 0.5],
[self.settings._POSE_YAW_THRESHOLD_SLACK,
self.settings._POSE_YAW_THRESHOLD_STRICT]) / self.settings._POSE_YAW_THRESHOLD
self.settings._POSE_YAW_THRESHOLD_STRICT])) / self.settings._POSE_YAW_THRESHOLD

def _get_distracted_types(self):
distracted_types = []
Expand Down
2 changes: 1 addition & 1 deletion system/hardware/fan_controller.py
Original file line number Diff line number Diff line change
Expand Up @@ -30,7 +30,7 @@ def update(self, cur_temp: float, ignition: bool) -> int:
error = 70 - cur_temp
fan_pwr_out = -int(self.controller.update(
error=error,
feedforward=np.interp(cur_temp, [60.0, 100.0], [0, -100])
feedforward=float(np.interp(cur_temp, [60.0, 100.0], [0, -100]))
))

self.last_ignition = ignition
Expand Down
4 changes: 2 additions & 2 deletions tools/joystick/joystick_control.py
Original file line number Diff line number Diff line change
Expand Up @@ -34,7 +34,7 @@ def update(self):
elif key in self.axes_map:
axis = self.axes_map[key]
incr = self.axis_increment if key in ['w', 'a'] else -self.axis_increment
self.axes_values[axis] = np.clip(self.axes_values[axis] + incr, -1, 1)
self.axes_values[axis] = float(np.clip(self.axes_values[axis] + incr, -1, 1))
else:
return False
return True
Expand Down Expand Up @@ -83,7 +83,7 @@ def update(self):
self.max_axis_value[event[0]] = max(event[1], self.max_axis_value[event[0]])
self.min_axis_value[event[0]] = min(event[1], self.min_axis_value[event[0]])

norm = -np.interp(event[1], [self.min_axis_value[event[0]], self.max_axis_value[event[0]]], [-1., 1.])
norm = float(-np.interp(event[1], [self.min_axis_value[event[0]], self.max_axis_value[event[0]]], [-1., 1.]))
norm = norm if abs(norm) > 0.03 else 0. # center can be noisy, deadzone of 3%
self.axes_values[event[0]] = EXPO * norm ** 3 + (1 - EXPO) * norm # less action near center for fine control
else:
Expand Down
4 changes: 2 additions & 2 deletions tools/joystick/joystickd.py
Original file line number Diff line number Diff line change
Expand Up @@ -45,13 +45,13 @@ def joystickd_thread():
joystick_axes = [0.0, 0.0]

if CC.longActive:
actuators.accel = 4.0 * np.clip(joystick_axes[0], -1, 1)
actuators.accel = 4.0 * float(np.clip(joystick_axes[0], -1, 1))

if CC.latActive:
max_curvature = MAX_LAT_ACCEL / max(sm['carState'].vEgo ** 2, 5)
max_angle = math.degrees(VM.get_steer_from_curvature(max_curvature, sm['carState'].vEgo, sm['liveParameters'].roll))

actuators.steer = np.clip(joystick_axes[1], -1, 1)
actuators.steer = float(np.clip(joystick_axes[1], -1, 1))
actuators.steeringAngleDeg, actuators.curvature = actuators.steer * max_angle, actuators.steer * -max_curvature

pm.send('carControl', cc_msg)
Expand Down
Loading

0 comments on commit b33c651

Please sign in to comment.