#!/usr/bin/env python import os import time import numpy as np from common.numpy_fast import clip from common.realtime import sec_since_boot from selfdrive.config import Conversions as CV from selfdrive.controls.lib.drive_helpers import create_event, EventTypes as ET, get_events from cereal import car from selfdrive.services import service_list import selfdrive.messaging as messaging from selfdrive.car.honda.carstate import CarState from selfdrive.car.honda.carcontroller import CAMERA_MSGS from selfdrive.car.honda.values import CruiseButtons, CM, BP, AH try: from .carcontroller import CarController except ImportError: CarController = None def get_compute_gb(): # generate a function that takes in [desired_accel, current_speed] -> [-1.0, 1.0] # where -1.0 is max brake and 1.0 is max gas # see debug/dump_accel_from_fiber.py to see how those parameters were generated w0 = np.array([[ 1.22056961, -0.39625418, 0.67952657], [ 1.03691769, 0.78210306, -0.41343188]]) b0 = np.array([ 0.01536703, -0.14335321, -0.26932889]) w2 = np.array([[-0.59124422, 0.42899439, 0.38660881], [ 0.79973811, 0.13178682, 0.08550351], [-0.15651935, -0.44360259, 0.76910877]]) b2 = np.array([ 0.15624429, 0.02294923, -0.0341086 ]) w4 = np.array([[-0.31521443], [-0.38626176], [ 0.52667892]]) b4 = np.array([-0.02922216]) def compute_output(dat, w0, b0, w2, b2, w4, b4): m0 = np.dot(dat, w0) + b0 m0 = leakyrelu(m0, 0.1) m2 = np.dot(m0, w2) + b2 m2 = leakyrelu(m2, 0.1) m4 = np.dot(m2, w4) + b4 return m4 def leakyrelu(x, alpha): return np.maximum(x, alpha * x) def _compute_gb(accel, speed): #linearly extrap below v1 using v1 and v2 data v1 = 5. v2 = 10. dat = np.array([accel, speed]) if speed > 5.: m4 = compute_output(dat, w0, b0, w2, b2, w4, b4) else: dat[1] = v1 m4v1 = compute_output(dat, w0, b0, w2, b2, w4, b4) dat[1] = v2 m4v2 = compute_output(dat, w0, b0, w2, b2, w4, b4) m4 = (speed - v1) * (m4v2 - m4v1) / (v2 - v1) + m4v1 return float(m4) return _compute_gb class CarInterface(object): def __init__(self, CP, logcan, sendcan=None): self.logcan = logcan self.CP = CP self.frame = 0 self.last_enable_pressed = 0 self.last_enable_sent = 0 self.gas_pressed_prev = False self.brake_pressed_prev = False self.can_invalid_count = 0 # *** init the major players *** self.CS = CarState(CP, self.logcan) # sending if read only is False if sendcan is not None: self.sendcan = sendcan self.CC = CarController(CP.enableCamera) if self.CS.accord: # self.accord_msg = [] raise NotImplementedError @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 = "honda" ret.radarName = "nidec" ret.carFingerprint = candidate ret.safetyModel = car.CarParams.SafetyModels.honda ret.enableSteer = True ret.enableBrake = True ret.enableCamera = not any(x for x in CAMERA_MSGS if x in fingerprint) ret.enableGas = 0x201 in fingerprint print "ECU Camera Simulated: ", ret.enableCamera print "ECU Gas Interceptor: ", ret.enableGas ret.enableCruise = not ret.enableGas # FIXME: hardcoding honda civic 2016 touring params so they can be used to # scale unknown params for other cars m_civic = 2923./2.205 + std_cargo l_civic = 2.70 aF_civic = l_civic * 0.4 aR_civic = l_civic - aF_civic j_civic = 2500 cF_civic = 85400 cR_civic = 90000 if candidate == "HONDA CIVIC 2016 TOURING": stop_and_go = True ret.m = m_civic ret.l = l_civic ret.aF = aF_civic ret.sR = 13.0 # Civic at comma has modified steering FW, so different tuning for the Neo in that car is_fw_modified = os.getenv("DONGLE_ID") in ['b0f5a01cf604185c'] ret.steerKp, ret.steerKi = [0.4, 0.12] if is_fw_modified else [0.8, 0.24] elif candidate == "ACURA ILX 2016 ACURAWATCH PLUS": stop_and_go = False ret.m = 3095./2.205 + std_cargo ret.l = 2.67 ret.aF = ret.l * 0.37 ret.sR = 15.3 # Acura at comma has modified steering FW, so different tuning for the Neo in that car is_fw_modified = os.getenv("DONGLE_ID") in ['cb38263377b873ee'] ret.steerKp, ret.steerKi = [0.4, 0.12] if is_fw_modified else [0.8, 0.24] elif candidate == "HONDA ACCORD 2016 TOURING": stop_and_go = False ret.m = 3580./2.205 + std_cargo ret.l = 2.74 ret.aF = ret.l * 0.38 ret.sR = 15.3 ret.steerKp, ret.steerKi = 0.8, 0.24 elif candidate == "HONDA CR-V 2016 TOURING": stop_and_go = False ret.m = 3572./2.205 + std_cargo ret.l = 2.62 ret.aF = ret.l * 0.41 ret.sR = 15.3 ret.steerKp, ret.steerKi = 0.8, 0.24 else: raise ValueError("unsupported car %s" % candidate) ret.steerKf = 0. # TODO: investigate FF steer control for Honda # min speed to enable ACC. if car can do stop and go, then set enabling speed # to a negative value, so it won't matter. Otherwise, add 0.5 mph margin to not # conflict with PCM acc ret.minEnableSpeed = -1. if (stop_and_go or ret.enableGas) else 25.5 * CV.MPH_TO_MS ret.aR = ret.l - ret.aF # TODO: get actual value, for now starting with reasonable value for # civic and scaling by mass and wheelbase ret.j = j_civic * ret.m * ret.l**2 / (m_civic * l_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.cF = cF_civic * ret.m / m_civic * (ret.aR / ret.l) / (aR_civic / l_civic) ret.cR = cR_civic * ret.m / m_civic * (ret.aF / ret.l) / (aF_civic / l_civic) # no rear steering, at least on the listed cars above ret.chi = 0. # no max steer limit VS speed ret.steerMaxBP = [0.] # m/s ret.steerMaxV = [1.] # max steer allowed ret.gasMaxBP = [0.] # m/s ret.gasMaxV = [0.6] if ret.enableGas else [0.] # max gas allowed ret.brakeMaxBP = [5., 20.] # m/s ret.brakeMaxV = [1., 0.8] # max brake allowed ret.steerLimitAlert = True return ret compute_gb = staticmethod(get_compute_gb()) # returns a car.CarState def update(self, c): # ******************* do can recv ******************* can_pub_main = [] canMonoTimes = [] self.CS.update(can_pub_main) # create message ret = car.CarState.new_message() # speeds ret.vEgo = self.CS.v_ego ret.aEgo = self.CS.a_ego ret.vEgoRaw = self.CS.v_ego_raw 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 # gas pedal ret.gas = self.CS.car_gas / 256.0 if not self.CP.enableGas: ret.gasPressed = self.CS.pedal_gas > 0 else: ret.gasPressed = self.CS.user_gas_pressed # brake pedal ret.brake = self.CS.user_brake ret.brakePressed = self.CS.brake_pressed != 0 # steering wheel ret.steeringAngle = self.CS.angle_steers ret.steeringRate = self.CS.angle_steers_rate # gear shifter lever ret.gearShifter = self.CS.gear_shifter ret.steeringTorque = self.CS.cp.vl[0x18F]['STEER_TORQUE_SENSOR'] ret.steeringPressed = self.CS.steer_override # cruise state ret.cruiseState.enabled = self.CS.pcm_acc_status != 0 ret.cruiseState.speed = self.CS.v_cruise_pcm * CV.KPH_TO_MS ret.cruiseState.available = bool(self.CS.main_on) ret.cruiseState.speedOffset = self.CS.cruise_speed_offset # 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) if self.CS.cruise_buttons != self.CS.prev_cruise_buttons: be = car.CarState.ButtonEvent.new_message() be.type = 'unknown' if self.CS.cruise_buttons != 0: be.pressed = True but = self.CS.cruise_buttons else: be.pressed = False but = self.CS.prev_cruise_buttons if but == CruiseButtons.RES_ACCEL: be.type = 'accelCruise' elif but == CruiseButtons.DECEL_SET: be.type = 'decelCruise' elif but == CruiseButtons.CANCEL: be.type = 'cancel' elif but == CruiseButtons.MAIN: be.type = 'altButton3' buttonEvents.append(be) if self.CS.cruise_setting != self.CS.prev_cruise_setting: be = car.CarState.ButtonEvent.new_message() be.type = 'unknown' if self.CS.cruise_setting != 0: be.pressed = True but = self.CS.cruise_setting else: be.pressed = False but = self.CS.prev_cruise_setting if but == 1: be.type = 'altButton1' # TODO: more buttons? buttonEvents.append(be) ret.buttonEvents = buttonEvents # events # TODO: I don't like the way capnp does enums # These strings aren't checked at compile time 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 self.CS.steer_error: events.append(create_event('steerUnavailable', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) elif self.CS.steer_not_allowed: events.append(create_event('steerTempUnavailable', [ET.NO_ENTRY, ET.WARNING])) if self.CS.brake_error: events.append(create_event('brakeUnavailable', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) if not ret.gearShifter == 'drive': events.append(create_event('wrongGear', [ET.NO_ENTRY, ET.SOFT_DISABLE])) if not self.CS.door_all_closed: events.append(create_event('doorOpen', [ET.NO_ENTRY, ET.SOFT_DISABLE])) if not self.CS.seatbelt: 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.brake_hold: events.append(create_event('brakeHold', [ET.NO_ENTRY, ET.USER_DISABLE])) if self.CS.park_brake: events.append(create_event('parkBrake', [ET.NO_ENTRY, ET.USER_DISABLE])) if self.CP.enableCruise and ret.vEgo < self.CP.minEnableSpeed: events.append(create_event('speedTooLow', [ET.NO_ENTRY])) # disable on pedals rising edge or when brake is pressed and speed isn't zero if (ret.gasPressed and not self.gas_pressed_prev) or \ (ret.brakePressed and (not self.brake_pressed_prev or ret.vEgo > 0.001)): events.append(create_event('pedalPressed', [ET.NO_ENTRY, ET.USER_DISABLE])) #if (ret.brakePressed and ret.vEgo < 0.001) or ret.gasPressed: if ret.gasPressed: events.append(create_event('pedalPressed', [ET.PRE_ENABLE])) # it can happen that car cruise disables while comma system is enabled: need to # keep braking if needed or if the speed is very low # TODO: for the Acura, cancellation below 25mph is normal. Issue a non loud alert if self.CP.enableCruise and not ret.cruiseState.enabled and \ (c.actuators.brake <= 0. or ret.vEgo < 0.3): events.append(create_event("cruiseDisabled", [ET.IMMEDIATE_DISABLE])) cur_time = sec_since_boot() enable_pressed = False # handle button presses for b in ret.buttonEvents: # do enable on both accel and decel buttons if b.type in ["accelCruise", "decelCruise"] and not b.pressed: print "enabled pressed at", cur_time self.last_enable_pressed = cur_time enable_pressed = True # do disable on button down if b.type == "cancel" and b.pressed: events.append(create_event('buttonCancel', [ET.USER_DISABLE])) if self.CP.enableCruise: # KEEP THIS EVENT LAST! send enable event if button is pressed and there are # NO_ENTRY events, so controlsd will display alerts. Also not send enable events # too close in time, so a no_entry will not be followed by another one. # TODO: button press should be the only thing that triggers enble if ((cur_time - self.last_enable_pressed) < 0.2 and (cur_time - self.last_enable_sent) > 0.2 and ret.cruiseState.enabled) or \ (enable_pressed and get_events(events, [ET.NO_ENTRY])): events.append(create_event('buttonEnable', [ET.ENABLE])) self.last_enable_sent = cur_time elif enable_pressed: events.append(create_event('buttonEnable', [ET.ENABLE])) ret.events = events ret.canMonoTimes = canMonoTimes # update previous brake/gas pressed self.gas_pressed_prev = ret.gasPressed self.brake_pressed_prev = ret.brakePressed # cast to reader so it can't be modified #print ret return ret.as_reader() # pass in a car.CarControl # to be called @ 100hz def apply(self, c): #print c if c.hudControl.speedVisible: hud_v_cruise = c.hudControl.setSpeed * CV.MS_TO_KPH else: hud_v_cruise = 255 hud_alert = { "none": AH.NONE, "fcw": AH.FCW, "steerRequired": AH.STEER, "brakePressed": AH.BRAKE_PRESSED, "wrongGear": AH.GEAR_NOT_D, "seatbeltUnbuckled": AH.SEATBELT, "speedTooHigh": AH.SPEED_TOO_HIGH}[str(c.hudControl.visualAlert)] snd_beep, snd_chime = { "none": (BP.MUTE, CM.MUTE), "beepSingle": (BP.SINGLE, CM.MUTE), "beepTriple": (BP.TRIPLE, CM.MUTE), "beepRepeated": (BP.REPEATED, CM.MUTE), "chimeSingle": (BP.MUTE, CM.SINGLE), "chimeDouble": (BP.MUTE, CM.DOUBLE), "chimeRepeated": (BP.MUTE, CM.REPEATED), "chimeContinuous": (BP.MUTE, CM.CONTINUOUS)}[str(c.hudControl.audibleAlert)] pcm_accel = int(clip(c.cruiseControl.accelOverride,0,1)*0xc6) self.CC.update(self.sendcan, c.enabled, self.CS, self.frame, \ c.actuators, \ c.cruiseControl.speedOverride, \ c.cruiseControl.override, \ c.cruiseControl.cancel, \ pcm_accel, \ hud_v_cruise, c.hudControl.lanesVisible, \ hud_show_car = c.hudControl.leadVisible, \ hud_alert = hud_alert, \ snd_beep = snd_beep, \ snd_chime = snd_chime) self.frame += 1