mirror of
https://github.com/MoreTore/openpilot.git
synced 2026-08-04 15:56:08 +08:00
rbiasini
6209ad5120
* Before abstraction, adding speed init from VW as well
* strting to abstract carstate class
* fix bug and update lock?
* revert pipfile change
* another bug
* fix linter
* bug fix
* remove a bunch of diplicated kf code
* better to not have class vars. will abstract __init__ anyway later
* abstract common instance vars in carstate init and a generic gear parser static method
* abstract gear parser for chrysler
* abstract gm gear parser too
* remove unnecessary random vars
* Chrysler: carstate returns capnp struct directly
* revert ref commit
* test ref
* WIP
* more WIP
* ops, missed this conflict
* ford as well
* not sure why this got deleted
* no need to copy
* remove copy
* remove copy import
* remove unnecessary intermediate variable
* remove obsolete comments
* GM: have carstate returning capnp struct directly
* Honda carstate also outputing capnp struct
* hyundai too now returns capnp from carstate
* ops, not meant this
* Subaru carstate also returning capnp
* Toyota: capnp struct as output of carstate
* fix bool
* minor simplififcation in Honda
* no need to negate
* VW carstate returning capnp struct (#1118)
* VW carstate also returning capnp struct
* fixed typo
* Remove unused blinker button (#1119)
* remove unused blinker button
* ops, this wasn't meant
* remove blinker button for VW as well
* update ref
Co-authored-by: Willem Melching <willem.melching@gmail.com>
old-commit-hash: 0c67143c92
75 lines
2.2 KiB
Python
75 lines
2.2 KiB
Python
import os
|
|
import time
|
|
from cereal import car
|
|
from common.kalman.simple_kalman import KF1D
|
|
from common.realtime import DT_CTRL
|
|
from selfdrive.car import gen_empty_fingerprint
|
|
|
|
GearShifter = car.CarState.GearShifter
|
|
|
|
# generic car and radar interfaces
|
|
|
|
class CarInterfaceBase():
|
|
def __init__(self, CP, CarController):
|
|
pass
|
|
|
|
@staticmethod
|
|
def calc_accel_override(a_ego, a_target, v_ego, v_target):
|
|
return 1.
|
|
|
|
@staticmethod
|
|
def compute_gb(accel, speed):
|
|
raise NotImplementedError
|
|
|
|
@staticmethod
|
|
def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False, car_fw=[]):
|
|
raise NotImplementedError
|
|
|
|
# returns a car.CarState, pass in car.CarControl
|
|
def update(self, c, can_strings):
|
|
raise NotImplementedError
|
|
|
|
# return sendcan, pass in a car.CarControl
|
|
def apply(self, c):
|
|
raise NotImplementedError
|
|
|
|
class RadarInterfaceBase():
|
|
def __init__(self, CP):
|
|
self.pts = {}
|
|
self.delay = 0
|
|
self.radar_ts = CP.radarTimeStep
|
|
|
|
def update(self, can_strings):
|
|
ret = car.RadarData.new_message()
|
|
|
|
if 'NO_RADAR_SLEEP' not in os.environ:
|
|
time.sleep(self.radar_ts) # radard runs on RI updates
|
|
|
|
return ret
|
|
|
|
class CarStateBase:
|
|
def __init__(self, CP):
|
|
self.CP = CP
|
|
self.car_fingerprint = CP.carFingerprint
|
|
self.cruise_buttons = 0
|
|
|
|
# Q = np.matrix([[10.0, 0.0], [0.0, 100.0]])
|
|
# R = 1e3
|
|
self.v_ego_kf = KF1D(x0=[[0.0], [0.0]],
|
|
A=[[1.0, DT_CTRL], [0.0, 1.0]],
|
|
C=[1.0, 0.0],
|
|
K=[[0.12287673], [0.29666309]])
|
|
|
|
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.x = [[v_ego_raw], [0.0]]
|
|
|
|
v_ego_x = self.v_ego_kf.update(v_ego_raw)
|
|
return float(v_ego_x[0]), float(v_ego_x[1])
|
|
|
|
@staticmethod
|
|
def parse_gear_shifter(gear):
|
|
return {'P': GearShifter.park, 'R': GearShifter.reverse, 'N': GearShifter.neutral,
|
|
'E': GearShifter.eco, 'T': GearShifter.manumatic, 'D': GearShifter.drive,
|
|
'S': GearShifter.sport, 'L': GearShifter.low, 'B': GearShifter.brake}.get(gear, GearShifter.unknown)
|