sadmen
/
openpilot_comma
mirror of https://github.com/commaai/openpilot.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Getting rid of openpilot.common.numpy_fast (#34368)

Browse Source 
* Got rid openpilot.common.numpy_fast

* fixed some data type erros

* importing numpy instead of importing specific functions

* fixing some numpy importing mistakes

* Update selfdrive/car/cruise.py

---------

Co-authored-by: Adeeb Shihadeh <adeebshihadeh@gmail.com>
pull/34382/head
Sammohana 3 months ago committed by GitHub
parent c54cd4569a
commit 8eebce75ac
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
21 changed files with 81 additions and 107 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 15
      common/pid.py
  2. 21
      common/tests/test_numpy_fast.py
  3. 6
      selfdrive/car/cruise.py
  4. 11
      selfdrive/controls/controlsd.py
  5. 8
      selfdrive/controls/lib/drive_helpers.py
  6. 4
      selfdrive/controls/lib/latcontrol.py
  7. 2
      selfdrive/controls/lib/latcontrol_angle.py
  8. 10
      selfdrive/controls/lib/latcontrol_pid.py
  9. 24
      selfdrive/controls/lib/latcontrol_torque.py
  10. 4
      selfdrive/controls/lib/longcontrol.py
  11. 7
      selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py
  12. 21
      selfdrive/controls/lib/longitudinal_planner.py
  13. 4
      selfdrive/controls/radard.py
  14. 6
      selfdrive/debug/live_cpu_and_temp.py
  15. 15
      selfdrive/locationd/paramsd.py
  16. 6
      selfdrive/monitoring/helpers.py
  17. 2
      selfdrive/test/longitudinal_maneuvers/plant.py
  18. 4
      system/hardware/fan_controller.py
  19. 6
      tools/joystick/joystick_control.py
  20. 6
      tools/joystick/joystickd.py
  21. 6
      tools/sim/bridge/common.py

15
common/pid.py
Unescape View File

@ -1,9 +1,6 @@
import numpy as np
from numbers import Number
from openpilot.common.numpy_fast import clip, interp
class PIDController:
def __init__(self, k_p, k_i, k_f=0., k_d=0., pos_limit=1e308, neg_limit=-1e308, rate=100):
self._k_p = k_p
@ -28,15 +25,15 @@ class PIDController:
@property
def k_p(self):
return interp(self.speed, self._k_p[0], self._k_p[1])
return np.interp(self.speed, self._k_p[0], self._k_p[1])
@property
def k_i(self):
return interp(self.speed, self._k_i[0], self._k_i[1])
return np.interp(self.speed, self._k_i[0], self._k_i[1])
@property
def k_d(self):
return interp(self.speed, self._k_d[0], self._k_d[1])
return np.interp(self.speed, self._k_d[0], self._k_d[1])
@property
def error_integral(self):
@ -64,10 +61,10 @@ class PIDController:
# Clip i to prevent exceeding control limits
control_no_i = self.p + self.d + self.f
control_no_i = clip(control_no_i, self.neg_limit, self.pos_limit)
self.i = clip(self.i, self.neg_limit - control_no_i, self.pos_limit - control_no_i)
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 = self.p + self.i + self.d + self.f
self.control = clip(control, self.neg_limit, self.pos_limit)
self.control = np.clip(control, self.neg_limit, self.pos_limit)
return self.control

21
common/tests/test_numpy_fast.py
Unescape View File

@ -1,21 +0,0 @@
import numpy as np
from openpilot.common.numpy_fast import interp
class TestInterp:
def test_correctness_controls(self):
_A_CRUISE_MIN_BP = np.asarray([0., 5., 10., 20., 40.])
_A_CRUISE_MIN_V = np.asarray([-1.0, -.8, -.67, -.5, -.30])
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 = 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 = interp(v_ego, _A_CRUISE_MIN_BP, _A_CRUISE_MIN_V)
np.testing.assert_equal(actual, expected)

6
selfdrive/car/cruise.py
Unescape View File

@ -1,8 +1,8 @@
import math
import numpy as np
from cereal import car
from openpilot.common.conversions import Conversions as CV
from openpilot.common.numpy_fast import clip
# WARNING: this value was determined based on the model's training distribution,
@ -106,7 +106,7 @@ class VCruiseHelper:
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 = clip(round(self.v_cruise_kph, 1), V_CRUISE_MIN, V_CRUISE_MAX)
self.v_cruise_kph = 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
@ -130,6 +130,6 @@ class VCruiseHelper:
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(clip(CS.vEgo * CV.MS_TO_KPH, initial, V_CRUISE_MAX)))
self.v_cruise_kph = int(round(np.clip(CS.vEgo * CV.MS_TO_KPH, initial, V_CRUISE_MAX)))
self.v_cruise_cluster_kph = self.v_cruise_kph

11
selfdrive/controls/controlsd.py
Unescape View File

@ -106,15 +106,16 @@ class Controls:
# accel PID loop
pid_accel_limits = self.CI.get_pid_accel_limits(self.CP, CS.vEgo, CS.vCruise * CV.KPH_TO_MS)
actuators.accel = self.LoC.update(CC.longActive, CS, long_plan.aTarget, long_plan.shouldStop, pid_accel_limits)
actuators.accel = float(self.LoC.update(CC.longActive, CS, long_plan.aTarget, long_plan.shouldStop, pid_accel_limits))
# Steering PID loop and lateral MPC
self.desired_curvature = clip_curvature(CS.vEgo, self.desired_curvature, model_v2.action.desiredCurvature)
actuators.curvature = self.desired_curvature
actuators.steer, actuators.steeringAngleDeg, lac_log = self.LaC.update(CC.latActive, CS, self.VM, lp,
actuators.curvature = float(self.desired_curvature)
steer, steeringAngleDeg, lac_log = self.LaC.update(CC.latActive, CS, self.VM, lp,
self.steer_limited, self.desired_curvature,
self.calibrated_pose) # TODO what if not available
actuators.steer = float(steer)
actuators.steeringAngleDeg = float(steeringAngleDeg)
# Ensure no NaNs/Infs
for p in ACTUATOR_FIELDS:
attr = getattr(actuators, p)
@ -179,7 +180,7 @@ class Controls:
cs.longitudinalPlanMonoTime = self.sm.logMonoTime['longitudinalPlan']
cs.lateralPlanMonoTime = self.sm.logMonoTime['modelV2']
cs.desiredCurvature = self.desired_curvature
cs.desiredCurvature = float(self.desired_curvature)
cs.longControlState = self.LoC.long_control_state
cs.upAccelCmd = float(self.LoC.pid.p)
cs.uiAccelCmd = float(self.LoC.pid.i)

8
selfdrive/controls/lib/drive_helpers.py
Unescape View File

@ -1,5 +1,5 @@
import numpy as np
from cereal import log
from openpilot.common.numpy_fast import clip
from openpilot.common.realtime import DT_CTRL
MIN_SPEED = 1.0
@ -13,10 +13,10 @@ MAX_LATERAL_JERK = 5.0
MAX_VEL_ERR = 5.0
def clip_curvature(v_ego, prev_curvature, new_curvature):
new_curvature = clip(new_curvature, -MAX_CURVATURE, MAX_CURVATURE)
new_curvature = 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 = clip(new_curvature,
safe_desired_curvature = np.clip(new_curvature,
prev_curvature - max_curvature_rate * DT_CTRL,
prev_curvature + max_curvature_rate * DT_CTRL)
@ -26,6 +26,6 @@ def clip_curvature(v_ego, prev_curvature, new_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 = clip(modelV2.temporalPose.trans[0] - v_ego, -MAX_VEL_ERR, MAX_VEL_ERR)
vel_err = np.clip(modelV2.temporalPose.trans[0] - v_ego, -MAX_VEL_ERR, MAX_VEL_ERR)
return float(vel_err)
return 0.0

4
selfdrive/controls/lib/latcontrol.py
Unescape View File

@ -1,6 +1,6 @@
import numpy as np
from abc import abstractmethod, ABC
from openpilot.common.numpy_fast import clip
from openpilot.common.realtime import DT_CTRL
MIN_LATERAL_CONTROL_SPEED = 0.3 # m/s
@ -28,5 +28,5 @@ class LatControl(ABC):
self.sat_count += self.sat_count_rate
else:
self.sat_count -= self.sat_count_rate
self.sat_count = clip(self.sat_count, 0.0, self.sat_limit)
self.sat_count = np.clip(self.sat_count, 0.0, self.sat_limit)
return self.sat_count > (self.sat_limit - 1e-3)

2
selfdrive/controls/lib/latcontrol_angle.py
Unescape View File

@ -23,7 +23,7 @@ class LatControlAngle(LatControl):
angle_steers_des += params.angleOffsetDeg
angle_control_saturated = abs(angle_steers_des - CS.steeringAngleDeg) > STEER_ANGLE_SATURATION_THRESHOLD
angle_log.saturated = self._check_saturation(angle_control_saturated, CS, False)
angle_log.saturated = bool(self._check_saturation(angle_control_saturated, CS, False))
angle_log.steeringAngleDeg = float(CS.steeringAngleDeg)
angle_log.steeringAngleDesiredDeg = angle_steers_des
return 0, float(angle_steers_des), angle_log

10
selfdrive/controls/lib/latcontrol_pid.py
Unescape View File

@ -39,10 +39,10 @@ class LatControlPID(LatControl):
output_steer = self.pid.update(error, override=CS.steeringPressed,
feedforward=steer_feedforward, speed=CS.vEgo)
pid_log.active = True
pid_log.p = self.pid.p
pid_log.i = self.pid.i
pid_log.f = self.pid.f
pid_log.output = output_steer
pid_log.saturated = self._check_saturation(self.steer_max - abs(output_steer) < 1e-3, CS, steer_limited)
pid_log.p = float(self.pid.p)
pid_log.i = float(self.pid.i)
pid_log.f = float(self.pid.f)
pid_log.output = float(output_steer)
pid_log.saturated = bool(self._check_saturation(self.steer_max - abs(output_steer) < 1e-3, CS, steer_limited))
return output_steer, angle_steers_des, pid_log

24
selfdrive/controls/lib/latcontrol_torque.py
Unescape View File

@ -1,8 +1,8 @@
import math
import numpy as np
from cereal import log
from opendbc.car.interfaces import LatControlInputs
from openpilot.common.numpy_fast import interp
from openpilot.selfdrive.controls.lib.latcontrol import LatControl
from openpilot.common.pid import PIDController
from openpilot.selfdrive.controls.lib.vehicle_model import ACCELERATION_DUE_TO_GRAVITY
@ -51,7 +51,7 @@ class LatControlTorque(LatControl):
else:
assert calibrated_pose is not None
actual_curvature_pose = calibrated_pose.angular_velocity.yaw / CS.vEgo
actual_curvature = interp(CS.vEgo, [2.0, 5.0], [actual_curvature_vm, actual_curvature_pose])
actual_curvature = 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
@ -60,7 +60,7 @@ class LatControlTorque(LatControl):
actual_lateral_accel = actual_curvature * CS.vEgo ** 2
lateral_accel_deadzone = curvature_deadzone * CS.vEgo ** 2
low_speed_factor = interp(CS.vEgo, LOW_SPEED_X, LOW_SPEED_Y)**2
low_speed_factor = 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
@ -68,7 +68,7 @@ class LatControlTorque(LatControl):
setpoint, lateral_accel_deadzone, friction_compensation=False, gravity_adjusted=False)
torque_from_measurement = self.torque_from_lateral_accel(LatControlInputs(measurement, roll_compensation, CS.vEgo, CS.aEgo), self.torque_params,
measurement, lateral_accel_deadzone, friction_compensation=False, gravity_adjusted=False)
pid_log.error = torque_from_setpoint - torque_from_measurement
pid_log.error = float(torque_from_setpoint - torque_from_measurement)
ff = self.torque_from_lateral_accel(LatControlInputs(gravity_adjusted_lateral_accel, roll_compensation, CS.vEgo, CS.aEgo), self.torque_params,
desired_lateral_accel - actual_lateral_accel, lateral_accel_deadzone, friction_compensation=True,
gravity_adjusted=True)
@ -80,14 +80,14 @@ class LatControlTorque(LatControl):
freeze_integrator=freeze_integrator)
pid_log.active = True
pid_log.p = self.pid.p
pid_log.i = self.pid.i
pid_log.d = self.pid.d
pid_log.f = self.pid.f
pid_log.output = -output_torque
pid_log.actualLateralAccel = actual_lateral_accel
pid_log.desiredLateralAccel = desired_lateral_accel
pid_log.saturated = self._check_saturation(self.steer_max - abs(output_torque) < 1e-3, CS, steer_limited)
pid_log.p = float(self.pid.p)
pid_log.i = float(self.pid.i)
pid_log.d = float(self.pid.d)
pid_log.f = float(self.pid.f)
pid_log.output = float(-output_torque)
pid_log.actualLateralAccel = float(actual_lateral_accel)
pid_log.desiredLateralAccel = float(desired_lateral_accel)
pid_log.saturated = bool(self._check_saturation(self.steer_max - abs(output_torque) < 1e-3, CS, steer_limited))
# TODO left is positive in this convention
return -output_torque, 0.0, pid_log

4
selfdrive/controls/lib/longcontrol.py
Unescape View File

@ -1,5 +1,5 @@
import numpy as np
from cereal import car
from openpilot.common.numpy_fast import clip
from openpilot.common.realtime import DT_CTRL
from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N
from openpilot.common.pid import PIDController
@ -84,5 +84,5 @@ class LongControl:
output_accel = self.pid.update(error, speed=CS.vEgo,
feedforward=a_target)
self.last_output_accel = clip(output_accel, accel_limits[0], accel_limits[1])
self.last_output_accel = np.clip(output_accel, accel_limits[0], accel_limits[1])
return self.last_output_accel

7
selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py
Unescape View File

@ -4,7 +4,6 @@ import time
import numpy as np
from cereal import log
from opendbc.car.interfaces import ACCEL_MIN
from openpilot.common.numpy_fast import clip
from openpilot.common.realtime import DT_MDL
from openpilot.common.swaglog import cloudlog
# WARNING: imports outside of constants will not trigger a rebuild
@ -320,9 +319,9 @@ class LongitudinalMpc:
# 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 = clip(x_lead, min_x_lead, 1e8)
v_lead = clip(v_lead, 0.0, 1e8)
a_lead = clip(a_lead, -10., 5.)
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.)
lead_xv = self.extrapolate_lead(x_lead, v_lead, a_lead, a_lead_tau)
return lead_xv

21
selfdrive/controls/lib/longitudinal_planner.py
Unescape View File

@ -1,7 +1,6 @@
#!/usr/bin/env python3
import math
import numpy as np
from openpilot.common.numpy_fast import clip, interp
import cereal.messaging as messaging
from opendbc.car.interfaces import ACCEL_MIN, ACCEL_MAX
@ -30,7 +29,7 @@ _A_TOTAL_MAX_BP = [20., 40.]
def get_max_accel(v_ego):
return interp(v_ego, A_CRUISE_MAX_BP, A_CRUISE_MAX_VALS)
return 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
@ -43,7 +42,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 = interp(v_ego, _A_TOTAL_MAX_BP, _A_TOTAL_MAX_V)
a_total_max = 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.))
@ -55,9 +54,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 = interp(action_t, CONTROL_N_T_IDX, speeds)
v_target = np.interp(action_t, CONTROL_N_T_IDX, speeds)
a_target = 2 * (v_target - v_now) / (action_t) - a_now
v_target_1sec = interp(action_t + 1.0, CONTROL_N_T_IDX, speeds)
v_target_1sec = np.interp(action_t + 1.0, CONTROL_N_T_IDX, speeds)
else:
v_target = 0.0
v_target_1sec = 0.0
@ -139,7 +138,7 @@ class LongitudinalPlanner:
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 = clip(sm['carState'].aEgo, accel_limits[0], accel_limits[1])
self.a_desired = 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))
@ -151,7 +150,7 @@ class LongitudinalPlanner:
if not self.allow_throttle:
clipped_accel_coast = max(accel_coast, accel_limits_turns[0])
clipped_accel_coast_interp = interp(v_ego, [MIN_ALLOW_THROTTLE_SPEED, MIN_ALLOW_THROTTLE_SPEED*2], [accel_limits_turns[1], clipped_accel_coast])
clipped_accel_coast_interp = 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:
@ -176,7 +175,7 @@ class LongitudinalPlanner:
# Interpolate 0.05 seconds and save as starting point for next iteration
a_prev = self.a_desired
self.a_desired = float(interp(self.dt, CONTROL_N_T_IDX, self.a_desired_trajectory))
self.a_desired = float(np.interp(self.dt, CONTROL_N_T_IDX, self.a_desired_trajectory))
self.v_desired_filter.x = self.v_desired_filter.x + self.dt * (self.a_desired + a_prev) / 2.0
def publish(self, sm, pm):
@ -200,9 +199,9 @@ class LongitudinalPlanner:
action_t = self.CP.longitudinalActuatorDelay + DT_MDL
a_target, should_stop = get_accel_from_plan(longitudinalPlan.speeds, longitudinalPlan.accels,
action_t=action_t, vEgoStopping=self.CP.vEgoStopping)
longitudinalPlan.aTarget = a_target
longitudinalPlan.shouldStop = should_stop
longitudinalPlan.aTarget = float(a_target)
longitudinalPlan.shouldStop = bool(should_stop)
longitudinalPlan.allowBrake = True
longitudinalPlan.allowThrottle = self.allow_throttle
longitudinalPlan.allowThrottle = bool(self.allow_throttle)
pm.send('longitudinalPlan', plan_send)

4
selfdrive/controls/radard.py
Unescape View File

@ -1,11 +1,11 @@
#!/usr/bin/env python3
import math
import numpy as np
from collections import deque
from typing import Any
import capnp
from cereal import messaging, log, car
from openpilot.common.numpy_fast import interp
from openpilot.common.params import Params
from openpilot.common.realtime import DT_MDL, Priority, config_realtime_process
from openpilot.common.swaglog import cloudlog
@ -44,7 +44,7 @@ class KalmanParams:
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 = [[interp(dt, dts, K0)], [interp(dt, dts, K1)]]
self.K = [[np.interp(dt, dts, K0)], [np.interp(dt, dts, K1)]]
class Track:

6
selfdrive/debug/live_cpu_and_temp.py
Unescape View File

@ -1,10 +1,10 @@
#!/usr/bin/env python3
import argparse
import numpy as np
import capnp
from collections import defaultdict
from cereal.messaging import SubMaster
from openpilot.common.numpy_fast import mean
def cputime_total(ct):
return ct.user + ct.nice + ct.system + ct.idle + ct.iowait + ct.irq + ct.softirq
@ -49,7 +49,7 @@ if __name__ == "__main__":
if sm.updated['deviceState']:
t = sm['deviceState']
last_temp = mean(t.cpuTempC)
last_temp = np.mean(t.cpuTempC)
last_mem = t.memoryUsagePercent
if sm.updated['procLog']:
@ -72,7 +72,7 @@ if __name__ == "__main__":
total_times = total_times_new[:]
busy_times = busy_times_new[:]
print(f"CPU {100.0 * mean(cores):.2f}% - RAM: {last_mem:.2f}% - Temp {last_temp:.2f}C")
print(f"CPU {100.0 * 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)

15
selfdrive/locationd/paramsd.py
Unescape View File

@ -8,7 +8,6 @@ import cereal.messaging as messaging
from cereal import car, log
from openpilot.common.params import Params
from openpilot.common.realtime import config_realtime_process, DT_MDL
from openpilot.common.numpy_fast import clip
from openpilot.selfdrive.locationd.models.car_kf import CarKalman, ObservationKind, States
from openpilot.selfdrive.locationd.models.constants import GENERATED_DIR
from openpilot.selfdrive.locationd.helpers import PoseCalibrator, Pose
@ -66,7 +65,7 @@ class ParamsLearner:
# 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 = clip(roll, self.roll - ROLL_MAX_DELTA, self.roll + ROLL_MAX_DELTA)
self.roll = 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
@ -203,11 +202,11 @@ def main():
learner = ParamsLearner(CP, CP.steerRatio, 1.0, 0.0)
x = learner.kf.x
angle_offset_average = clip(math.degrees(x[States.ANGLE_OFFSET].item()),
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 = clip(math.degrees(x[States.ANGLE_OFFSET].item() + x[States.ANGLE_OFFSET_FAST].item()),
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 = clip(float(x[States.ROAD_ROLL].item()), roll - ROLL_MAX_DELTA, roll + ROLL_MAX_DELTA)
roll = 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
@ -226,9 +225,9 @@ def main():
liveParameters.sensorValid = sensors_valid
liveParameters.steerRatio = float(x[States.STEER_RATIO].item())
liveParameters.stiffnessFactor = float(x[States.STIFFNESS].item())
liveParameters.roll = roll
liveParameters.angleOffsetAverageDeg = angle_offset_average
liveParameters.angleOffsetDeg = angle_offset
liveParameters.roll = float(roll)
liveParameters.angleOffsetAverageDeg = float(angle_offset_average)
liveParameters.angleOffsetDeg = float(angle_offset)
liveParameters.valid = all((
avg_offset_valid,
total_offset_valid,

6
selfdrive/monitoring/helpers.py
Unescape View File

@ -1,9 +1,9 @@
from math import atan2
import numpy as np
from cereal import car, log
import cereal.messaging as messaging
from openpilot.selfdrive.selfdrived.events import Events
from openpilot.common.numpy_fast import interp
from openpilot.common.realtime import DT_DMON
from openpilot.common.filter_simple import FirstOrderFilter
from openpilot.common.stat_live import RunningStatFilter
@ -205,10 +205,10 @@ class DriverMonitoring:
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 = interp(bp_normal, [0, 0.5],
self.pose.cfactor_pitch = 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 = interp(bp_normal, [0, 0.5],
self.pose.cfactor_yaw = np.interp(bp_normal, [0, 0.5],
[self.settings._POSE_YAW_THRESHOLD_SLACK,
self.settings._POSE_YAW_THRESHOLD_STRICT]) / self.settings._POSE_YAW_THRESHOLD

2
selfdrive/test/longitudinal_maneuvers/plant.py
Unescape View File

@ -121,7 +121,7 @@ class Plant:
ss.selfdriveState.personality = self.personality
control.controlsState.forceDecel = self.force_decel
car_state.carState.vEgo = float(self.speed)
car_state.carState.standstill = self.speed < 0.01
car_state.carState.standstill = bool(self.speed < 0.01)
car_state.carState.vCruise = float(v_cruise * 3.6)
car_control.carControl.orientationNED = [0., float(pitch), 0.]

4
system/hardware/fan_controller.py
Unescape View File

@ -1,8 +1,8 @@
#!/usr/bin/env python3
import numpy as np
from abc import ABC, abstractmethod
from openpilot.common.realtime import DT_HW
from openpilot.common.numpy_fast import interp
from openpilot.common.swaglog import cloudlog
from openpilot.common.pid import PIDController
@ -30,7 +30,7 @@ class TiciFanController(BaseFanController):
error = 70 - cur_temp
fan_pwr_out = -int(self.controller.update(
error=error,
feedforward=interp(cur_temp, [60.0, 100.0], [0, -100])
feedforward=np.interp(cur_temp, [60.0, 100.0], [0, -100])
))
self.last_ignition = ignition

6
tools/joystick/joystick_control.py
Unescape View File

@ -2,10 +2,10 @@
import os
import argparse
import threading
import numpy as np
from inputs import UnpluggedError, get_gamepad
from cereal import messaging
from openpilot.common.numpy_fast import interp, clip
from openpilot.common.params import Params
from openpilot.common.realtime import Ratekeeper
from openpilot.system.hardware import HARDWARE
@ -34,7 +34,7 @@ class Keyboard:
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] = clip(self.axes_values[axis] + incr, -1, 1)
self.axes_values[axis] = np.clip(self.axes_values[axis] + incr, -1, 1)
else:
return False
return True
@ -83,7 +83,7 @@ class Joystick:
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 = -interp(event[1], [self.min_axis_value[event[0]], self.max_axis_value[event[0]]], [-1., 1.])
norm = -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:

6
tools/joystick/joystickd.py
Unescape View File

@ -1,9 +1,9 @@
#!/usr/bin/env python3
import math
import numpy as np
from cereal import messaging, car
from openpilot.common.numpy_fast import clip
from openpilot.common.realtime import DT_CTRL, Ratekeeper
from openpilot.common.params import Params
from openpilot.common.swaglog import cloudlog
@ -45,13 +45,13 @@ def joystickd_thread():
joystick_axes = [0.0, 0.0]
if CC.longActive:
actuators.accel = 4.0 * clip(joystick_axes[0], -1, 1)
actuators.accel = 4.0 * 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 = clip(joystick_axes[1], -1, 1)
actuators.steer = np.clip(joystick_axes[1], -1, 1)
actuators.steeringAngleDeg, actuators.curvature = actuators.steer * max_angle, actuators.steer * -max_curvature
pm.send('carControl', cc_msg)

6
tools/sim/bridge/common.py
Unescape View File

@ -1,6 +1,7 @@
import signal
import threading
import functools
import numpy as np
from collections import namedtuple
from enum import Enum
@ -9,7 +10,6 @@ from abc import ABC, abstractmethod
from opendbc.car.honda.values import CruiseButtons
from openpilot.common.params import Params
from openpilot.common.numpy_fast import clip
from openpilot.common.realtime import Ratekeeper
from openpilot.selfdrive.test.helpers import set_params_enabled
from openpilot.tools.sim.lib.common import SimulatorState, World
@ -173,8 +173,8 @@ Ignition: {self.simulator_state.ignition} Engaged: {self.simulator_state.is_enga
self.simulator_state.is_engaged = self.simulated_car.sm['selfdriveState'].active
if self.simulator_state.is_engaged:
throttle_op = clip(self.simulated_car.sm['carControl'].actuators.accel / 1.6, 0.0, 1.0)
brake_op = clip(-self.simulated_car.sm['carControl'].actuators.accel / 4.0, 0.0, 1.0)
throttle_op = np.clip(self.simulated_car.sm['carControl'].actuators.accel / 1.6, 0.0, 1.0)
brake_op = np.clip(-self.simulated_car.sm['carControl'].actuators.accel / 4.0, 0.0, 1.0)
steer_op = self.simulated_car.sm['carControl'].actuators.steeringAngleDeg
self.past_startup_engaged = True

Write Preview
Loading…
Cancel
Save