2020-01-17 11:58:43 -07:00
|
|
|
import os
|
|
|
|
import time
|
2021-12-16 07:31:53 -07:00
|
|
|
from abc import abstractmethod, ABC
|
2021-12-16 05:08:20 -07:00
|
|
|
from typing import Dict, Tuple, List
|
2020-11-24 23:45:37 -07:00
|
|
|
|
2020-01-17 11:58:43 -07:00
|
|
|
from cereal import car
|
2020-02-17 12:53:53 -07:00
|
|
|
from common.kalman.simple_kalman import KF1D
|
|
|
|
from common.realtime import DT_CTRL
|
2020-01-17 11:58:43 -07:00
|
|
|
from selfdrive.car import gen_empty_fingerprint
|
2022-03-13 22:42:39 -06:00
|
|
|
from common.conversions import Conversions as CV
|
2020-11-24 23:45:37 -07:00
|
|
|
from selfdrive.controls.lib.drive_helpers import V_CRUISE_MAX
|
2020-05-14 16:21:21 -06:00
|
|
|
from selfdrive.controls.lib.events import Events
|
2020-02-19 21:37:07 -07:00
|
|
|
from selfdrive.controls.lib.vehicle_model import VehicleModel
|
2020-01-17 11:58:43 -07:00
|
|
|
|
2020-02-17 12:53:53 -07:00
|
|
|
GearShifter = car.CarState.GearShifter
|
2020-05-14 16:21:21 -06:00
|
|
|
EventName = car.CarEvent.EventName
|
2021-05-11 16:20:30 -06:00
|
|
|
|
2021-12-08 18:13:43 -07:00
|
|
|
MAX_CTRL_SPEED = (V_CRUISE_MAX + 4) * CV.KPH_TO_MS
|
2021-09-02 15:16:44 -06:00
|
|
|
ACCEL_MAX = 2.0
|
2021-09-06 18:29:32 -06:00
|
|
|
ACCEL_MIN = -3.5
|
2021-09-02 15:16:44 -06:00
|
|
|
|
2020-02-17 12:53:53 -07:00
|
|
|
|
2020-01-17 11:58:43 -07:00
|
|
|
# generic car and radar interfaces
|
|
|
|
|
2020-11-24 23:45:37 -07:00
|
|
|
|
2021-12-16 07:31:53 -07:00
|
|
|
class CarInterfaceBase(ABC):
|
2020-02-20 14:08:43 -07:00
|
|
|
def __init__(self, CP, CarController, CarState):
|
2020-02-19 21:37:07 -07:00
|
|
|
self.CP = CP
|
|
|
|
self.VM = VehicleModel(CP)
|
|
|
|
|
|
|
|
self.frame = 0
|
2021-07-10 02:53:14 -06:00
|
|
|
self.steering_unpressed = 0
|
2020-02-21 19:24:37 -07:00
|
|
|
self.low_speed_alert = False
|
2021-10-30 00:20:59 -06:00
|
|
|
self.silent_steer_warning = True
|
2020-02-19 21:37:07 -07:00
|
|
|
|
2020-05-23 16:39:22 -06:00
|
|
|
if CarState is not None:
|
|
|
|
self.CS = CarState(CP)
|
|
|
|
self.cp = self.CS.get_can_parser(CP)
|
|
|
|
self.cp_cam = self.CS.get_cam_can_parser(CP)
|
2020-07-21 09:41:45 -06:00
|
|
|
self.cp_body = self.CS.get_body_can_parser(CP)
|
2021-10-28 17:54:12 -06:00
|
|
|
self.cp_loopback = self.CS.get_loopback_can_parser(CP)
|
2020-02-19 21:37:07 -07:00
|
|
|
|
|
|
|
self.CC = None
|
|
|
|
if CarController is not None:
|
|
|
|
self.CC = CarController(self.cp.dbc_name, CP, self.VM)
|
2020-01-17 11:58:43 -07:00
|
|
|
|
2021-09-02 15:16:44 -06:00
|
|
|
@staticmethod
|
2021-09-06 18:29:32 -06:00
|
|
|
def get_pid_accel_limits(CP, current_speed, cruise_speed):
|
2021-09-02 15:16:44 -06:00
|
|
|
return ACCEL_MIN, ACCEL_MAX
|
|
|
|
|
2020-01-17 11:58:43 -07:00
|
|
|
@staticmethod
|
2021-12-16 05:08:20 -07:00
|
|
|
@abstractmethod
|
2022-03-16 16:17:47 -06:00
|
|
|
def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None, disable_radar=False):
|
2021-12-16 05:08:20 -07:00
|
|
|
pass
|
2020-01-17 11:58:43 -07:00
|
|
|
|
2021-06-22 08:28:11 -06:00
|
|
|
@staticmethod
|
|
|
|
def init(CP, logcan, sendcan):
|
|
|
|
pass
|
|
|
|
|
2021-10-04 02:03:53 -06:00
|
|
|
@staticmethod
|
2021-10-21 03:38:03 -06:00
|
|
|
def get_steer_feedforward_default(desired_angle, v_ego):
|
2021-10-04 02:03:53 -06:00
|
|
|
# Proportional to realigning tire momentum: lateral acceleration.
|
|
|
|
# TODO: something with lateralPlan.curvatureRates
|
|
|
|
return desired_angle * (v_ego**2)
|
|
|
|
|
2021-10-21 03:38:03 -06:00
|
|
|
@classmethod
|
|
|
|
def get_steer_feedforward_function(cls):
|
|
|
|
return cls.get_steer_feedforward_default
|
|
|
|
|
2020-02-20 15:06:02 -07:00
|
|
|
# returns a set of default params to avoid repetition in car specific params
|
|
|
|
@staticmethod
|
2020-11-03 20:56:25 -07:00
|
|
|
def get_std_params(candidate, fingerprint):
|
2020-02-20 15:06:02 -07:00
|
|
|
ret = car.CarParams.new_message()
|
|
|
|
ret.carFingerprint = candidate
|
|
|
|
|
|
|
|
# standard ALC params
|
|
|
|
ret.steerControlType = car.CarParams.SteerControlType.torque
|
2020-04-30 15:13:33 -06:00
|
|
|
ret.minSteerSpeed = 0.
|
2021-12-03 06:57:53 -07:00
|
|
|
ret.wheelSpeedFactor = 1.0
|
2020-02-20 15:06:02 -07:00
|
|
|
|
2021-07-10 14:56:11 -06:00
|
|
|
ret.pcmCruise = True # openpilot's state is tied to the PCM's cruise state on most cars
|
|
|
|
ret.minEnableSpeed = -1. # enable is done by stock ACC, so ignore this
|
2020-02-20 15:06:02 -07:00
|
|
|
ret.steerRatioRear = 0. # no rear steering, at least on the listed cars aboveA
|
|
|
|
ret.openpilotLongitudinalControl = False
|
2021-09-11 19:01:54 -06:00
|
|
|
ret.stopAccel = -2.0
|
|
|
|
ret.stoppingDecelRate = 0.8 # brake_travel/s while trying to stop
|
2021-09-11 18:13:30 -06:00
|
|
|
ret.vEgoStopping = 0.5
|
2022-03-03 04:27:24 -07:00
|
|
|
ret.vEgoStarting = 0.5
|
2021-06-15 06:00:57 -06:00
|
|
|
ret.stoppingControl = True
|
2020-02-20 15:06:02 -07:00
|
|
|
ret.longitudinalTuning.deadzoneBP = [0.]
|
|
|
|
ret.longitudinalTuning.deadzoneV = [0.]
|
2022-02-16 07:38:22 -07:00
|
|
|
ret.longitudinalTuning.kf = 1.
|
2020-02-20 15:06:02 -07:00
|
|
|
ret.longitudinalTuning.kpBP = [0.]
|
|
|
|
ret.longitudinalTuning.kpV = [1.]
|
|
|
|
ret.longitudinalTuning.kiBP = [0.]
|
|
|
|
ret.longitudinalTuning.kiV = [1.]
|
2022-02-18 01:45:00 -07:00
|
|
|
# TODO estimate car specific lag, use .15s for now
|
2021-09-14 19:57:16 -06:00
|
|
|
ret.longitudinalActuatorDelayLowerBound = 0.15
|
|
|
|
ret.longitudinalActuatorDelayUpperBound = 0.15
|
2022-01-27 04:31:36 -07:00
|
|
|
ret.steerLimitTimer = 1.0
|
2020-02-20 15:06:02 -07:00
|
|
|
return ret
|
|
|
|
|
2021-12-16 05:08:20 -07:00
|
|
|
@abstractmethod
|
|
|
|
def update(self, c: car.CarControl, can_strings: List[bytes]) -> car.CarState:
|
|
|
|
pass
|
2020-01-17 11:58:43 -07:00
|
|
|
|
2021-12-16 05:08:20 -07:00
|
|
|
@abstractmethod
|
|
|
|
def apply(self, c: car.CarControl) -> Tuple[car.CarControl.Actuators, List[bytes]]:
|
|
|
|
pass
|
2020-01-17 11:58:43 -07:00
|
|
|
|
2022-01-13 17:06:56 -07:00
|
|
|
def create_common_events(self, cs_out, extra_gears=None, pcm_enable=True):
|
2020-05-14 16:21:21 -06:00
|
|
|
events = Events()
|
2020-02-20 17:22:25 -07:00
|
|
|
|
|
|
|
if cs_out.doorOpen:
|
2020-05-14 16:21:21 -06:00
|
|
|
events.add(EventName.doorOpen)
|
2020-02-20 17:22:25 -07:00
|
|
|
if cs_out.seatbeltUnlatched:
|
2020-05-14 16:21:21 -06:00
|
|
|
events.add(EventName.seatbeltNotLatched)
|
2021-07-10 02:53:14 -06:00
|
|
|
if cs_out.gearShifter != GearShifter.drive and (extra_gears is None or
|
|
|
|
cs_out.gearShifter not in extra_gears):
|
2020-05-14 16:21:21 -06:00
|
|
|
events.add(EventName.wrongGear)
|
2020-02-20 17:22:25 -07:00
|
|
|
if cs_out.gearShifter == GearShifter.reverse:
|
2020-05-14 16:21:21 -06:00
|
|
|
events.add(EventName.reverseGear)
|
2020-02-20 17:22:25 -07:00
|
|
|
if not cs_out.cruiseState.available:
|
2020-05-14 16:21:21 -06:00
|
|
|
events.add(EventName.wrongCarMode)
|
2020-02-21 19:24:37 -07:00
|
|
|
if cs_out.espDisabled:
|
2020-05-14 16:21:21 -06:00
|
|
|
events.add(EventName.espDisabled)
|
2020-02-21 19:24:37 -07:00
|
|
|
if cs_out.gasPressed:
|
2020-05-14 16:21:21 -06:00
|
|
|
events.add(EventName.gasPressed)
|
|
|
|
if cs_out.stockFcw:
|
|
|
|
events.add(EventName.stockFcw)
|
2020-05-12 16:06:48 -06:00
|
|
|
if cs_out.stockAeb:
|
2020-05-14 16:21:21 -06:00
|
|
|
events.add(EventName.stockAeb)
|
2020-05-18 01:02:55 -06:00
|
|
|
if cs_out.vEgo > MAX_CTRL_SPEED:
|
2020-05-14 16:21:21 -06:00
|
|
|
events.add(EventName.speedTooHigh)
|
2020-06-15 19:11:50 -06:00
|
|
|
if cs_out.cruiseState.nonAdaptive:
|
|
|
|
events.add(EventName.wrongCruiseMode)
|
2021-11-08 15:26:30 -07:00
|
|
|
if cs_out.brakeHoldActive and self.CP.openpilotLongitudinalControl:
|
|
|
|
events.add(EventName.brakeHold)
|
2022-03-14 17:32:20 -06:00
|
|
|
if cs_out.parkingBrake:
|
|
|
|
events.add(EventName.parkBrake)
|
2020-02-20 17:22:25 -07:00
|
|
|
|
2021-07-10 02:53:14 -06:00
|
|
|
# Handle permanent and temporary steering faults
|
2021-10-30 00:20:59 -06:00
|
|
|
self.steering_unpressed = 0 if cs_out.steeringPressed else self.steering_unpressed + 1
|
2022-03-01 23:53:55 -07:00
|
|
|
if cs_out.steerFaultTemporary:
|
2021-10-30 00:20:59 -06:00
|
|
|
# if the user overrode recently, show a less harsh alert
|
|
|
|
if self.silent_steer_warning or cs_out.standstill or self.steering_unpressed < int(1.5 / DT_CTRL):
|
|
|
|
self.silent_steer_warning = True
|
|
|
|
events.add(EventName.steerTempUnavailableSilent)
|
|
|
|
else:
|
|
|
|
events.add(EventName.steerTempUnavailable)
|
|
|
|
else:
|
|
|
|
self.silent_steer_warning = False
|
2022-03-01 23:53:55 -07:00
|
|
|
if cs_out.steerFaultPermanent:
|
2020-05-14 16:21:21 -06:00
|
|
|
events.add(EventName.steerUnavailable)
|
2020-02-20 17:22:25 -07:00
|
|
|
|
2020-04-10 17:12:58 -06:00
|
|
|
# we engage when pcm is active (rising edge)
|
|
|
|
if pcm_enable:
|
|
|
|
if cs_out.cruiseState.enabled and not self.CS.out.cruiseState.enabled:
|
2020-05-14 16:21:21 -06:00
|
|
|
events.add(EventName.pcmEnable)
|
2020-04-10 17:12:58 -06:00
|
|
|
elif not cs_out.cruiseState.enabled:
|
2020-05-14 16:21:21 -06:00
|
|
|
events.add(EventName.pcmDisable)
|
2020-04-10 17:12:58 -06:00
|
|
|
|
2020-02-20 17:22:25 -07:00
|
|
|
return events
|
|
|
|
|
2020-11-24 23:45:37 -07:00
|
|
|
|
2021-12-16 07:31:53 -07:00
|
|
|
class RadarInterfaceBase(ABC):
|
2020-01-17 11:58:43 -07:00
|
|
|
def __init__(self, CP):
|
|
|
|
self.pts = {}
|
|
|
|
self.delay = 0
|
|
|
|
self.radar_ts = CP.radarTimeStep
|
2020-07-25 03:27:56 -06:00
|
|
|
self.no_radar_sleep = 'NO_RADAR_SLEEP' in os.environ
|
2020-01-17 11:58:43 -07:00
|
|
|
|
|
|
|
def update(self, can_strings):
|
|
|
|
ret = car.RadarData.new_message()
|
2020-07-25 03:27:56 -06:00
|
|
|
if not self.no_radar_sleep:
|
2020-01-17 11:58:43 -07:00
|
|
|
time.sleep(self.radar_ts) # radard runs on RI updates
|
|
|
|
return ret
|
2020-02-17 12:53:53 -07:00
|
|
|
|
2020-11-24 23:45:37 -07:00
|
|
|
|
2021-12-16 07:31:53 -07:00
|
|
|
class CarStateBase(ABC):
|
2020-02-17 12:53:53 -07:00
|
|
|
def __init__(self, CP):
|
|
|
|
self.CP = CP
|
|
|
|
self.car_fingerprint = CP.carFingerprint
|
2020-04-10 17:12:58 -06:00
|
|
|
self.out = car.CarState.new_message()
|
2020-02-17 12:53:53 -07:00
|
|
|
|
2020-10-14 19:27:27 -06:00
|
|
|
self.cruise_buttons = 0
|
|
|
|
self.left_blinker_cnt = 0
|
|
|
|
self.right_blinker_cnt = 0
|
2021-06-29 04:10:35 -06:00
|
|
|
self.left_blinker_prev = False
|
|
|
|
self.right_blinker_prev = False
|
2020-10-14 19:27:27 -06:00
|
|
|
|
2020-02-17 12:53:53 -07:00
|
|
|
# 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])
|
|
|
|
|
2021-12-03 06:57:53 -07:00
|
|
|
def get_wheel_speeds(self, fl, fr, rl, rr, unit=CV.KPH_TO_MS):
|
|
|
|
factor = unit * self.CP.wheelSpeedFactor
|
|
|
|
|
|
|
|
wheelSpeeds = car.CarState.WheelSpeeds.new_message()
|
|
|
|
wheelSpeeds.fl = fl * factor
|
|
|
|
wheelSpeeds.fr = fr * factor
|
|
|
|
wheelSpeeds.rl = rl * factor
|
|
|
|
wheelSpeeds.rr = rr * factor
|
|
|
|
return wheelSpeeds
|
|
|
|
|
2021-06-29 04:10:35 -06:00
|
|
|
def update_blinker_from_lamp(self, blinker_time: int, left_blinker_lamp: bool, right_blinker_lamp: bool):
|
|
|
|
"""Update blinkers from lights. Enable output when light was seen within the last `blinker_time`
|
|
|
|
iterations"""
|
|
|
|
# TODO: Handle case when switching direction. Now both blinkers can be on at the same time
|
2020-10-14 19:27:27 -06:00
|
|
|
self.left_blinker_cnt = blinker_time if left_blinker_lamp else max(self.left_blinker_cnt - 1, 0)
|
|
|
|
self.right_blinker_cnt = blinker_time if right_blinker_lamp else max(self.right_blinker_cnt - 1, 0)
|
|
|
|
return self.left_blinker_cnt > 0, self.right_blinker_cnt > 0
|
|
|
|
|
2021-06-29 04:10:35 -06:00
|
|
|
def update_blinker_from_stalk(self, blinker_time: int, left_blinker_stalk: bool, right_blinker_stalk: bool):
|
|
|
|
"""Update blinkers from stalk position. When stalk is seen the blinker will be on for at least blinker_time,
|
|
|
|
or until the stalk is turned off, whichever is longer. If the opposite stalk direction is seen the blinker
|
|
|
|
is forced to the other side. On a rising edge of the stalk the timeout is reset."""
|
|
|
|
|
|
|
|
if left_blinker_stalk:
|
|
|
|
self.right_blinker_cnt = 0
|
|
|
|
if not self.left_blinker_prev:
|
|
|
|
self.left_blinker_cnt = blinker_time
|
|
|
|
|
|
|
|
if right_blinker_stalk:
|
|
|
|
self.left_blinker_cnt = 0
|
|
|
|
if not self.right_blinker_prev:
|
|
|
|
self.right_blinker_cnt = blinker_time
|
|
|
|
|
|
|
|
self.left_blinker_cnt = max(self.left_blinker_cnt - 1, 0)
|
|
|
|
self.right_blinker_cnt = max(self.right_blinker_cnt - 1, 0)
|
|
|
|
|
|
|
|
self.left_blinker_prev = left_blinker_stalk
|
|
|
|
self.right_blinker_prev = right_blinker_stalk
|
|
|
|
|
|
|
|
return bool(left_blinker_stalk or self.left_blinker_cnt > 0), bool(right_blinker_stalk or self.right_blinker_cnt > 0)
|
|
|
|
|
2020-02-17 12:53:53 -07:00
|
|
|
@staticmethod
|
2020-11-24 23:45:37 -07:00
|
|
|
def parse_gear_shifter(gear: str) -> car.CarState.GearShifter:
|
|
|
|
d: Dict[str, car.CarState.GearShifter] = {
|
2020-12-04 03:09:34 -07:00
|
|
|
'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
|
2020-11-24 23:45:37 -07:00
|
|
|
}
|
|
|
|
return d.get(gear, GearShifter.unknown)
|
2020-02-20 14:08:43 -07:00
|
|
|
|
|
|
|
@staticmethod
|
|
|
|
def get_cam_can_parser(CP):
|
|
|
|
return None
|
2020-07-21 09:41:45 -06:00
|
|
|
|
|
|
|
@staticmethod
|
|
|
|
def get_body_can_parser(CP):
|
|
|
|
return None
|
2021-10-28 17:54:12 -06:00
|
|
|
|
|
|
|
@staticmethod
|
|
|
|
def get_loopback_can_parser(CP):
|
|
|
|
return None
|