FrogPilot setup

This commit is contained in:
FrogAi
2024-01-12 22:39:30 -07:00
parent 9d97e0ecc1
commit 7308c1b35c
60 changed files with 1660 additions and 49 deletions

View File

@@ -11,6 +11,7 @@ from openpilot.common.basedir import BASEDIR
from openpilot.common.conversions import Conversions as CV
from openpilot.common.simple_kalman import KF1D, get_kalman_gain
from openpilot.common.numpy_fast import clip
from openpilot.common.params import Params
from openpilot.common.realtime import DT_CTRL
from openpilot.selfdrive.car import apply_hysteresis, gen_empty_fingerprint, scale_rot_inertia, scale_tire_stiffness, STD_CARGO_KG
from openpilot.selfdrive.controls.lib.drive_helpers import V_CRUISE_MAX, get_friction
@@ -86,6 +87,9 @@ class CarInterfaceBase(ABC):
if CarController is not None:
self.CC = CarController(self.cp.dbc_name, CP, self.VM)
# FrogPilot variables
params = Params()
@staticmethod
def get_pid_accel_limits(CP, current_speed, cruise_speed):
return ACCEL_MIN, ACCEL_MAX
@@ -306,6 +310,9 @@ class CarInterfaceBase(ABC):
return events
def update_frogpilot_params(self, params):
if hasattr(self.CC, 'update_frogpilot_variables'):
self.CC.update_frogpilot_variables(params)
class RadarInterfaceBase(ABC):
def __init__(self, CP):
@@ -345,6 +352,10 @@ class CarStateBase(ABC):
K = get_kalman_gain(DT_CTRL, np.array(A), np.array(C), np.array(Q), R)
self.v_ego_kf = KF1D(x0=x0, A=A, C=C[0], K=K)
# FrogPilot variables
self.param = Params()
self.param_memory = Params("/dev/shm/params")
def update_speed_kf(self, v_ego_raw):
if abs(v_ego_raw - self.v_ego_kf.x[0][0]) > 2.0: # Prevent large accelerations when car starts at non zero speed
self.v_ego_kf.set_x([[v_ego_raw], [0.0]])
@@ -433,6 +444,7 @@ class CarStateBase(ABC):
def get_loopback_can_parser(CP):
return None
def update_frogpilot_params(self, params):
INTERFACE_ATTR_FILE = {
"FINGERPRINTS": "fingerprints",