mirror of
https://github.com/dragonpilot/dragonpilot.git
synced 2026-08-24 01:33:50 +08:00
Drew Hintz
1efa3f0eba
* 2019 Chrysler Pacfica and 2019 Jeep Grand Cherokee support, along with likely support for arbitrary models. This is done by copying most values from the stock LKAS camera which is active with https://github.com/commaai/panda/pull/177 * No longer send LKAS_HEARTBIT because Panda now forwards it for us. * Pacifica Hybrid 2018 combine fingerprints, add 808: 8 * panda chrysler: forward bus 0 to bus 2 copy of Panda commit: https://github.com/commaai/panda/commit/104950264473274e0012a39b7ea2d1468ef46d8d
274 lines
9.7 KiB
Python
Executable File
274 lines
9.7 KiB
Python
Executable File
#!/usr/bin/env python
|
|
from common.realtime import sec_since_boot
|
|
from cereal import car
|
|
from selfdrive.config import Conversions as CV
|
|
from selfdrive.controls.lib.drive_helpers import EventTypes as ET, create_event
|
|
from selfdrive.controls.lib.vehicle_model import VehicleModel
|
|
from selfdrive.car.chrysler.carstate import CarState, get_can_parser, get_camera_parser
|
|
from selfdrive.car.chrysler.values import ECU, check_ecu_msgs, CAR
|
|
|
|
try:
|
|
from selfdrive.car.chrysler.carcontroller import CarController
|
|
except ImportError:
|
|
CarController = None
|
|
|
|
|
|
class CarInterface(object):
|
|
def __init__(self, CP, sendcan=None):
|
|
self.CP = CP
|
|
self.VM = VehicleModel(CP)
|
|
|
|
self.frame = 0
|
|
self.can_invalid_count = 0
|
|
self.gas_pressed_prev = False
|
|
self.brake_pressed_prev = False
|
|
self.cruise_enabled_prev = False
|
|
self.low_speed_alert = False
|
|
|
|
# *** init the major players ***
|
|
self.CS = CarState(CP)
|
|
self.cp = get_can_parser(CP)
|
|
self.cp_cam = get_camera_parser(CP)
|
|
|
|
# sending if read only is False
|
|
if sendcan is not None:
|
|
self.sendcan = sendcan
|
|
self.CC = CarController(self.cp.dbc_name, CP.carFingerprint, CP.enableCamera)
|
|
|
|
@staticmethod
|
|
def compute_gb(accel, speed):
|
|
return float(accel) / 3.0
|
|
|
|
@staticmethod
|
|
def calc_accel_override(a_ego, a_target, v_ego, v_target):
|
|
return 1.0
|
|
|
|
@staticmethod
|
|
def get_params(candidate, fingerprint):
|
|
|
|
# kg of standard extra cargo to count for drive, gas, etc...
|
|
std_cargo = 136
|
|
|
|
ret = car.CarParams.new_message()
|
|
|
|
ret.carName = "chrysler"
|
|
ret.carFingerprint = candidate
|
|
|
|
ret.safetyModel = car.CarParams.SafetyModels.chrysler
|
|
|
|
# pedal
|
|
ret.enableCruise = True
|
|
|
|
# FIXME: hardcoding honda civic 2016 touring params so they can be used to
|
|
# scale unknown params for other cars
|
|
mass_civic = 2923./2.205 + std_cargo
|
|
wheelbase_civic = 2.70
|
|
centerToFront_civic = wheelbase_civic * 0.4
|
|
centerToRear_civic = wheelbase_civic - centerToFront_civic
|
|
rotationalInertia_civic = 2500
|
|
tireStiffnessFront_civic = 85400 * 2.0
|
|
tireStiffnessRear_civic = 90000 * 2.0
|
|
|
|
# Speed conversion: 20, 45 mph
|
|
ret.steerKpBP, ret.steerKiBP = [[9., 20.], [9., 20.]]
|
|
ret.wheelbase = 3.089 # in meters for Pacifica Hybrid 2017
|
|
ret.steerRatio = 16.2 # Pacifica Hybrid 2017
|
|
ret.mass = 2858 + std_cargo # kg curb weight Pacifica Hybrid 2017
|
|
ret.steerKpV, ret.steerKiV = [[0.15,0.30], [0.03,0.05]]
|
|
ret.steerKf = 0.00006 # full torque for 10 deg at 80mph means 0.00007818594
|
|
ret.steerActuatorDelay = 0.1
|
|
ret.steerRateCost = 0.7
|
|
|
|
if candidate in (CAR.JEEP_CHEROKEE, CAR.JEEP_CHEROKEE_2019):
|
|
ret.wheelbase = 2.91 # in meters
|
|
ret.steerRatio = 12.7
|
|
ret.steerActuatorDelay = 0.2 # in seconds
|
|
|
|
ret.centerToFront = ret.wheelbase * 0.44
|
|
|
|
ret.longPidDeadzoneBP = [0., 9.]
|
|
ret.longPidDeadzoneV = [0., .15]
|
|
|
|
ret.minSteerSpeed = 3.8 # m/s
|
|
ret.minEnableSpeed = -1. # enable is done by stock ACC, so ignore this
|
|
if candidate in (CAR.PACIFICA_2019_HYBRID, CAR.JEEP_CHEROKEE_2019):
|
|
ret.minSteerSpeed = 17.5 # m/s 17 on the way up, 13 on the way down once engaged.
|
|
# TODO allow 2019 cars to steer down to 13 m/s if already engaged.
|
|
|
|
centerToRear = ret.wheelbase - ret.centerToFront
|
|
# TODO: get actual value, for now starting with reasonable value for
|
|
# civic and scaling by mass and wheelbase
|
|
ret.rotationalInertia = rotationalInertia_civic * \
|
|
ret.mass * ret.wheelbase**2 / (mass_civic * wheelbase_civic**2)
|
|
|
|
# TODO: start from empirically derived lateral slip stiffness for the civic and scale by
|
|
# mass and CG position, so all cars will have approximately similar dyn behaviors
|
|
ret.tireStiffnessFront = tireStiffnessFront_civic * \
|
|
ret.mass / mass_civic * \
|
|
(centerToRear / ret.wheelbase) / (centerToRear_civic / wheelbase_civic)
|
|
ret.tireStiffnessRear = tireStiffnessRear_civic * \
|
|
ret.mass / mass_civic * \
|
|
(ret.centerToFront / ret.wheelbase) / (centerToFront_civic / wheelbase_civic)
|
|
|
|
# no rear steering, at least on the listed cars above
|
|
ret.steerRatioRear = 0.
|
|
|
|
# steer, gas, brake limitations VS speed
|
|
ret.steerMaxBP = [16. * CV.KPH_TO_MS, 45. * CV.KPH_TO_MS] # breakpoints at 1 and 40 kph
|
|
ret.steerMaxV = [1., 1.] # 2/3rd torque allowed above 45 kph
|
|
ret.gasMaxBP = [0.]
|
|
ret.gasMaxV = [0.5]
|
|
ret.brakeMaxBP = [5., 20.]
|
|
ret.brakeMaxV = [1., 0.8]
|
|
|
|
ret.enableCamera = not check_ecu_msgs(fingerprint, ECU.CAM)
|
|
print "ECU Camera Simulated: ", ret.enableCamera
|
|
ret.openpilotLongitudinalControl = False
|
|
|
|
ret.steerLimitAlert = True
|
|
ret.stoppingControl = False
|
|
ret.startAccel = 0.0
|
|
|
|
ret.longitudinalKpBP = [0., 5., 35.]
|
|
ret.longitudinalKpV = [3.6, 2.4, 1.5]
|
|
ret.longitudinalKiBP = [0., 35.]
|
|
ret.longitudinalKiV = [0.54, 0.36]
|
|
|
|
return ret
|
|
|
|
# returns a car.CarState
|
|
def update(self, c):
|
|
# ******************* do can recv *******************
|
|
canMonoTimes = []
|
|
self.cp.update(int(sec_since_boot() * 1e9), False)
|
|
self.cp_cam.update(int(sec_since_boot() * 1e9), False)
|
|
self.CS.update(self.cp, self.cp_cam)
|
|
|
|
# create message
|
|
ret = car.CarState.new_message()
|
|
|
|
# speeds
|
|
ret.vEgo = self.CS.v_ego
|
|
ret.vEgoRaw = self.CS.v_ego_raw
|
|
ret.aEgo = self.CS.a_ego
|
|
ret.yawRate = self.VM.yaw_rate(self.CS.angle_steers * CV.DEG_TO_RAD, self.CS.v_ego)
|
|
ret.standstill = self.CS.standstill
|
|
ret.wheelSpeeds.fl = self.CS.v_wheel_fl
|
|
ret.wheelSpeeds.fr = self.CS.v_wheel_fr
|
|
ret.wheelSpeeds.rl = self.CS.v_wheel_rl
|
|
ret.wheelSpeeds.rr = self.CS.v_wheel_rr
|
|
|
|
# gear shifter
|
|
ret.gearShifter = self.CS.gear_shifter
|
|
|
|
# gas pedal
|
|
ret.gas = self.CS.car_gas
|
|
ret.gasPressed = self.CS.pedal_gas > 0
|
|
|
|
# brake pedal
|
|
ret.brake = self.CS.user_brake
|
|
ret.brakePressed = self.CS.brake_pressed
|
|
ret.brakeLights = self.CS.brake_lights
|
|
|
|
# steering wheel
|
|
ret.steeringAngle = self.CS.angle_steers
|
|
ret.steeringRate = self.CS.angle_steers_rate
|
|
|
|
ret.steeringTorque = self.CS.steer_torque_driver
|
|
ret.steeringPressed = self.CS.steer_override
|
|
|
|
# cruise state
|
|
ret.cruiseState.enabled = self.CS.pcm_acc_status # same as main_on
|
|
ret.cruiseState.speed = self.CS.v_cruise_pcm * CV.KPH_TO_MS
|
|
ret.cruiseState.available = self.CS.main_on
|
|
ret.cruiseState.speedOffset = 0.
|
|
# ignore standstill in hybrid rav4, since pcm allows to restart without
|
|
# receiving any special command
|
|
ret.cruiseState.standstill = False
|
|
|
|
# TODO: button presses
|
|
buttonEvents = []
|
|
|
|
if self.CS.left_blinker_on != self.CS.prev_left_blinker_on:
|
|
be = car.CarState.ButtonEvent.new_message()
|
|
be.type = 'leftBlinker'
|
|
be.pressed = self.CS.left_blinker_on != 0
|
|
buttonEvents.append(be)
|
|
|
|
if self.CS.right_blinker_on != self.CS.prev_right_blinker_on:
|
|
be = car.CarState.ButtonEvent.new_message()
|
|
be.type = 'rightBlinker'
|
|
be.pressed = self.CS.right_blinker_on != 0
|
|
buttonEvents.append(be)
|
|
|
|
ret.buttonEvents = buttonEvents
|
|
ret.leftBlinker = bool(self.CS.left_blinker_on)
|
|
ret.rightBlinker = bool(self.CS.right_blinker_on)
|
|
|
|
ret.doorOpen = not self.CS.door_all_closed
|
|
ret.seatbeltUnlatched = not self.CS.seatbelt
|
|
self.low_speed_alert = (ret.vEgo < self.CP.minSteerSpeed)
|
|
|
|
ret.genericToggle = self.CS.generic_toggle
|
|
#ret.lkasCounter = self.CS.lkas_counter
|
|
#ret.lkasCarModel = self.CS.lkas_car_model
|
|
|
|
# events
|
|
events = []
|
|
if not self.CS.can_valid:
|
|
self.can_invalid_count += 1
|
|
if self.can_invalid_count >= 5:
|
|
events.append(create_event('commIssue', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
|
|
else:
|
|
self.can_invalid_count = 0
|
|
if not (ret.gearShifter in ('drive', 'low')):
|
|
events.append(create_event('wrongGear', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
|
|
if ret.doorOpen:
|
|
events.append(create_event('doorOpen', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
|
|
if ret.seatbeltUnlatched:
|
|
events.append(create_event('seatbeltNotLatched', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
|
|
if self.CS.esp_disabled:
|
|
events.append(create_event('espDisabled', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
|
|
if not self.CS.main_on:
|
|
events.append(create_event('wrongCarMode', [ET.NO_ENTRY, ET.USER_DISABLE]))
|
|
if ret.gearShifter == 'reverse':
|
|
events.append(create_event('reverseGear', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
|
|
if self.CS.steer_error:
|
|
events.append(create_event('steerUnavailable', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE, ET.PERMANENT]))
|
|
|
|
if ret.cruiseState.enabled and not self.cruise_enabled_prev:
|
|
events.append(create_event('pcmEnable', [ET.ENABLE]))
|
|
elif not ret.cruiseState.enabled:
|
|
events.append(create_event('pcmDisable', [ET.USER_DISABLE]))
|
|
|
|
# disable on gas pedal and speed isn't zero. Gas pedal is used to resume ACC
|
|
# from a 3+ second stop.
|
|
if (ret.gasPressed and (not self.gas_pressed_prev) and ret.vEgo > 2.0):
|
|
events.append(create_event('pedalPressed', [ET.NO_ENTRY, ET.USER_DISABLE]))
|
|
|
|
if self.low_speed_alert:
|
|
events.append(create_event('belowSteerSpeed', [ET.WARNING]))
|
|
|
|
ret.events = events
|
|
ret.canMonoTimes = canMonoTimes
|
|
|
|
self.gas_pressed_prev = ret.gasPressed
|
|
self.brake_pressed_prev = ret.brakePressed
|
|
self.cruise_enabled_prev = ret.cruiseState.enabled
|
|
|
|
return ret.as_reader()
|
|
|
|
# pass in a car.CarControl
|
|
# to be called @ 100hz
|
|
def apply(self, c):
|
|
|
|
if (self.CS.frame == -1):
|
|
return False # if we haven't seen a frame 220, then do not update.
|
|
|
|
self.frame = self.CS.frame
|
|
self.CC.update(self.sendcan, c.enabled, self.CS, self.frame,
|
|
c.actuators, c.cruiseControl.cancel, c.hudControl.visualAlert,
|
|
c.hudControl.audibleAlert)
|
|
|
|
return False
|