#!/usr/bin/env python
from common . realtime import sec_since_boot
from cereal import car , log
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
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 )
# 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 == CAR . JEEP_CHEROKEE :
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
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 . CS . update ( self . cp )
# 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
# 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 == ' drive ' :
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 , perception_state = log . Live20Data . new_message ( ) ) :
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
