brianhansonxyz/clearpilot
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

FrogPilot Setup

Browse Source
This commit is contained in:
FrogAi
2024-03-01 09:26:59 -07:00
parent 505ad1fbfe
commit 6c946fc97a
106 changed files with 2349 additions and 185 deletions
Download Patch File Download Diff File Expand all files Collapse all files

19
selfdrive/car/interfaces.py
View File

@@ -11,12 +11,15 @@ 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
from openpilot.selfdrive.controls.lib.events import Events
from openpilot.selfdrive.controls.lib.vehicle_model import VehicleModel
from openpilot.selfdrive.frogpilot.functions.frogpilot_functions import FrogPilotFunctions
ButtonType = car.CarState.ButtonEvent.Type
GearShifter = car.CarState.GearShifter
EventName = car.CarEvent.EventName
@@ -95,6 +98,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
@@ -107,9 +113,9 @@ class CarInterfaceBase(ABC):
return cls.get_params(candidate, gen_empty_fingerprint(), list(), False, False)
@classmethod
def get_params(cls, candidate: str, fingerprint: Dict[int, Dict[int, int]], car_fw: List[car.CarParams.CarFw], experimental_long: bool, docs: bool):
def get_params(cls, params, candidate: str, fingerprint: Dict[int, Dict[int, int]], car_fw: List[car.CarParams.CarFw], experimental_long: bool, docs: bool):
ret = CarInterfaceBase.get_std_params(candidate)
ret = cls._get_params(ret, candidate, fingerprint, car_fw, experimental_long, docs)
ret = cls._get_params(ret, params, candidate, fingerprint, car_fw, experimental_long, docs)
# Vehicle mass is published curb weight plus assumed payload such as a human driver; notCars have no assumed payload
if not ret.notCar:
@@ -204,14 +210,14 @@ class CarInterfaceBase(ABC):
def _update(self, c: car.CarControl) -> car.CarState:
pass
def update(self, c: car.CarControl, can_strings: List[bytes]) -> car.CarState:
def update(self, c: car.CarControl, can_strings: List[bytes], frogpilot_variables) -> car.CarState:
# parse can
for cp in self.can_parsers:
if cp is not None:
cp.update_strings(can_strings)
# get CarState
ret = self._update(c)
ret = self._update(c, frogpilot_variables)
ret.canValid = all(cp.can_valid for cp in self.can_parsers if cp is not None)
ret.canTimeout = any(cp.bus_timeout for cp in self.can_parsers if cp is not None)
@@ -241,7 +247,7 @@ class CarInterfaceBase(ABC):
def apply(self, c: car.CarControl, now_nanos: int) -> Tuple[car.CarControl.Actuators, List[bytes]]:
pass
def create_common_events(self, cs_out, extra_gears=None, pcm_enable=True, allow_enable=True,
def create_common_events(self, cs_out, frogpilot_variables, extra_gears=None, pcm_enable=True, allow_enable=True,
enable_buttons=(ButtonType.accelCruise, ButtonType.decelCruise)):
events = Events()
@@ -354,6 +360,9 @@ 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.fpf = FrogPilotFunctions()
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]])