def update_ui_buttons(self,id,btn_status):
if self.cstm_btns.btns[id].btn_status > 0:
if (id == 1) and (btn_status == 0) and self.cstm_btns.btns[id].btn_name=="alca":
if self.cstm_btns.btns[id].btn_label2 == self.alcaLabels[self.alcaMode]:
self.alcaMode = (self.alcaMode + 1 ) % 4
kegman.save({'lastALCAMode': int(self.alcaMode)}) # write last distance bar setting to file
else:
self.alcaMode = 0
kegman.save({'lastALCAMode': int(self.alcaMode)}) # write last distance bar setting to file
self.cstm_btns.btns[id].btn_label2 = self.alcaLabels[self.alcaMode]
self.cstm_btns.hasChanges = True
if self.alcaMode == 3:
self.cstm_btns.set_button_status("alca", 0)
elif (id == 4) and (btn_status == 0) and self.cstm_btns.btns[id].btn_name=="gas":
if self.cstm_btns.btns[id].btn_label2 == self.gasLabels[self.gasMode]:
self.gasMode = (self.gasMode + 1 ) % 3
kegman.save({'lastGasMode': int(self.gasMode)}) # write last GasMode setting to file
else:
self.gasMode = 0
kegman.save({'lastGasMode': int(self.gasMode)}) # write last GasMode setting to file
self.cstm_btns.btns[id].btn_label2 = self.gasLabels[self.gasMode]
self.cstm_btns.hasChanges = True
else:
self.cstm_btns.btns[id].btn_status = btn_status * self.cstm_btns.btns[id].btn_status
else:
self.cstm_btns.btns[id].btn_status = btn_status
if (id == 1) and self.cstm_btns.btns[id].btn_name=="alca":
self.alcaMode = (self.alcaMode + 1 ) % 4
kegman.save({'lastALCAMode': int(self.alcaMode)}) # write last distance bar setting to file
self.cstm_btns.btns[id].btn_label2 = self.alcaLabels[self.alcaMode]
self.cstm_btns.hasChanges = True
def mod_sshd_config(
self
): # this disables dns lookup when connecting to EON to speed up commands from phantom app, reboot required
sshd_config_file = "/system/comma/usr/etc/ssh/sshd_config"
try:
result = subprocess.check_call([
"mount", "-o", "remount,rw", "/system"
]) # mount /system as rw so we can modify sshd_config file
except:
result = 1
if result == 0:
with open(sshd_config_file, "r") as f:
sshd_config = f.read()
if "UseDNS no" not in sshd_config:
if sshd_config[-1:] != "\n":
use_dns = "\nUseDNS no\n"
else:
use_dns = "UseDNS no\n"
with open(sshd_config_file, "w") as f:
f.write(sshd_config + use_dns)
kegman.save({"UseDNS": True})
else:
kegman.save({"UseDNS": True})
try:
subprocess.call(["mount", "-o", "remount,ro",
"/system"]) # remount system as read only
except:
pass
else:
kegman.save({"UseDNS": False})
def upload_data():
filepath = "/data/openpilot/selfdrive/data_collection/gps-data"
if os.path.isfile(filepath):
if kegman.get("uniqueID") is None:
kegman.save({"uniqueID": ''.join([random.choice(string.lowercase+string.uppercase+string.digits) for i in range(15)])})
try:
username = kegman.get("uniqueID", ''.join([random.choice(string.lowercase+string.uppercase+string.digits) for i in range(15)]))
try:
with open("/data/data/ai.comma.plus.offroad/files/persistStore/persist-auth", "r") as f:
auth = json.loads(f.read())
auth = json.loads(auth['commaUser'])
if auth and str(auth['username']) != "":
username = str(auth['username'])
except:
pass
params = Params()
car = params.get('CachedFingerprint')
if car is not None:
car = json.loads(car)
username+="-{}".format(car[0])
filename = "gps-data.{}".format(random.randint(1,99999))
ftp = ftplib.FTP("arneschwarck.dyndns.org")
ftp.login("openpilot", "communitypilot")
with open(filepath, "rb") as f:
try:
ftp.mkd("/{}".format(username))
except:
pass
ftp.storbinary("STOR /{}/{}".format(username, filename), f)
ftp.quit()
os.remove(filepath)
return True
except:
return False
else:
return False
def update_ui_buttons(self, id, btn_status):
if self.cstm_btns.btns[id].btn_status > 0:
if (id == 1) and (btn_status == 0
) and self.cstm_btns.btns[id].btn_name == "alca":
if self.cstm_btns.btns[id].btn_label2 == self.alcaLabels[
self.alcaMode]:
self.alcaMode = (self.alcaMode + 1) % 4
if self.CP.carFingerprint == CAR.PRIUS:
self.alcaMode = 3
self.cstm_btns.set_button_status("alca", 0)
kegman.save({'lastALCAMode': int(self.alcaMode)
}) # write last ALCAMode setting to file
else:
self.alcaMode = 0
if self.CP.carFingerprint == CAR.PRIUS:
self.alcaMode = 3
self.cstm_btns.set_button_status("alca", 0)
kegman.save({'lastALCAMode': int(self.alcaMode)
}) # write last ALCAMode setting to file
self.cstm_btns.btns[id].btn_label2 = self.alcaLabels[
self.alcaMode]
self.cstm_btns.hasChanges = True
if self.CP.carFingerprint == CAR.PRIUS:
self.alcaMode = 3
if self.alcaMode == 3:
self.cstm_btns.set_button_status("alca", 0)
elif (id == 2) and (
btn_status
== 0) and self.cstm_btns.btns[id].btn_name == "slow":
if self.cstm_btns.btns[id].btn_label2 == self.sloLabels[
self.sloMode]:
self.sloMode = (self.sloMode + 1) % 2
kegman.save({'lastSloMode': int(self.sloMode)
}) # write last SloMode setting to file
else:
self.sloMode = 0
kegman.save({'lastSloMode': int(self.sloMode)
}) # write last SloMode setting to file
self.cstm_btns.btns[id].btn_label2 = self.sloLabels[
self.sloMode]
self.cstm_btns.hasChanges = True
if self.sloMode == 0:
self.cstm_btns.set_button_status(
"slow", 0) # this might not be needed
elif (id == 5) and (
btn_status
== 0) and self.cstm_btns.btns[id].btn_name == "gas":
if self.cstm_btns.btns[id].btn_label2 == self.gasLabels[
self.gasMode]:
self.gasMode = (self.gasMode + 1) % 3
kegman.save({'lastGasMode': int(self.gasMode)
}) # write last GasMode setting to file
else:
self.gasMode = 0
kegman.save({'lastGasMode': int(self.gasMode)
}) # write last GasMode setting to file
self.cstm_btns.btns[id].btn_label2 = self.gasLabels[
self.gasMode]
self.cstm_btns.hasChanges = True
else:
self.cstm_btns.btns[
id].btn_status = btn_status * self.cstm_btns.btns[
id].btn_status
else:
self.cstm_btns.btns[id].btn_status = btn_status
if (id == 1) and self.cstm_btns.btns[id].btn_name == "alca":
self.alcaMode = (self.alcaMode + 1) % 4
kegman.save({'lastALCAMode': int(self.alcaMode)
}) # write last ALCAMode setting to file
self.cstm_btns.btns[id].btn_label2 = self.alcaLabels[
self.alcaMode]
self.cstm_btns.hasChanges = True
elif (id == 2) and self.cstm_btns.btns[id].btn_name == "slow":
self.sloMode = (self.sloMode + 1) % 2
kegman.save({'lastSloMode': int(self.sloMode)
}) # write last SloMode setting to file
self.cstm_btns.btns[id].btn_label2 = self.sloLabels[
self.sloMode]
self.cstm_btns.hasChanges = True
def update(self, c):
self.pt_cp.update(int(sec_since_boot() * 1e9), False)
self.CS.update(self.pt_cp)
# 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.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
# gas pedal information.
ret.gas = self.CS.pedal_gas / 254.0
ret.gasPressed = self.CS.user_gas_pressed
# brake pedal
ret.brake = self.CS.user_brake / 0xd0
ret.brakePressed = self.CS.brake_pressed
# steering wheel
ret.steeringAngle = self.CS.angle_steers + self.angleSteersoffset
# torque and user override. Driver awareness
# timer resets when the user uses the steering wheel.
ret.steeringPressed = self.CS.steer_override
ret.steeringTorque = self.CS.steer_torque_driver
# cruise state
ret.cruiseState.available = bool(self.CS.main_on)
cruiseEnabled = self.CS.pcm_acc_status != 0
ret.cruiseState.enabled = cruiseEnabled
ret.cruiseState.standstill = False
ret.leftBlinker = self.CS.left_blinker_on
ret.rightBlinker = self.CS.right_blinker_on
ret.doorOpen = not self.CS.door_all_closed
ret.seatbeltUnlatched = not self.CS.seatbelt
ret.gearShifter = self.CS.gear_shifter
ret.readdistancelines = self.CS.follow_level
ret.genericToggle = False
ret.laneDepartureToggle = False
ret.distanceToggle = self.CS.follow_level
ret.accSlowToggle = False
ret.blindspot = self.CS.blind_spot_on
buttonEvents = []
# blinkers
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
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
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 != CruiseButtons.UNPRESS:
be.pressed = True
but = self.CS.cruise_buttons
else:
be.pressed = False
but = self.CS.prev_cruise_buttons
if but == CruiseButtons.RES_ACCEL:
if not (cruiseEnabled and self.CS.standstill):
be.type = 'accelCruise' # Suppress resume button if we're resuming from stop so we don't adjust speed.
elif but == CruiseButtons.DECEL_SET:
be.type = 'decelCruise'
if not cruiseEnabled and not self.CS.lkMode:
self.CS.lkMode = True
elif but == CruiseButtons.CANCEL:
be.type = 'cancel'
elif but == CruiseButtons.MAIN:
be.type = 'altButton3'
buttonEvents.append(be)
ret.buttonEvents = buttonEvents
if self.CS.lka_button and self.CS.lka_button != self.CS.prev_lka_button:
if self.CS.lkMode:
self.CS.lkMode = False
else:
self.CS.lkMode = True
if self.CS.distance_button and self.CS.distance_button != self.CS.prev_distance_button:
self.CS.follow_level -= 1
if self.CS.follow_level < 1:
self.CS.follow_level = 3
kegman.save({'lastTrMode': int(self.CS.follow_level)}) # write last distance bar setting to file
ret.gasbuttonstatus = self.CS.gasMode
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 ret.cruiseState.enabled and not self.cruise_enabled_prev:
disengage_event = True
else:
disengage_event = False
if self.CS.steer_error:
events.append(create_event('steerUnavailable', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE, ET.PERMANENT]))
if self.CS.steer_not_allowed:
events.append(create_event('steerTempUnavailable', [ET.NO_ENTRY, ET.WARNING]))
if ret.doorOpen and disengage_event:
events.append(create_event('doorOpen', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if ret.seatbeltUnlatched and disengage_event:
events.append(create_event('seatbeltNotLatched', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if self.CS.car_fingerprint in SUPERCRUISE_CARS:
if self.CS.acc_active and not self.acc_active_prev:
events.append(create_event('pcmEnable', [ET.ENABLE]))
if not self.CS.acc_active:
events.append(create_event('pcmDisable', [ET.USER_DISABLE]))
else:
if self.CS.brake_error:
events.append(create_event('brakeUnavailable', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE, ET.PERMANENT]))
if not self.CS.gear_shifter_valid:
events.append(create_event('wrongGear', [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 self.CS.gear_shifter == 3:
events.append(create_event('reverseGear', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
if ret.vEgo < self.CP.minEnableSpeed:
events.append(create_event('speedTooLow', [ET.NO_ENTRY]))
if self.CS.park_brake:
events.append(create_event('parkBrake', [ET.NO_ENTRY, ET.USER_DISABLE]))
# 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.gasPressed:
events.append(create_event('pedalPressed', [ET.PRE_ENABLE]))
# 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:
events.append(create_event('buttonEnable', [ET.ENABLE]))
# do disable on button down
if b.type == "cancel" and b.pressed:
events.append(create_event('buttonCancel', [ET.USER_DISABLE]))
ret.events = events
# update previous brake/gas pressed
self.acc_active_prev = self.CS.acc_active
self.gas_pressed_prev = ret.gasPressed
self.brake_pressed_prev = ret.brakePressed
self.cruise_enabled_prev = ret.cruiseState.enabled
# cast to reader so it can't be modified
return ret.as_reader()
def update(self, cp, cp_cam):
# copy can_valid on buses 0 and 2
self.can_valid = cp.can_valid
self.cam_can_valid = cp_cam.can_valid
# car params
v_weight_v = [
0., 1.
] # don't trust smooth speed at low values to avoid premature zero snapping
v_weight_bp = [
1., 6.
] # smooth blending, below ~0.6m/s the smooth speed snaps to zero
# update prevs, update must run once per loop
self.prev_cruise_buttons = self.cruise_buttons
self.prev_blinker_on = self.blinker_on
self.prev_lead_distance = self.lead_distance
self.prev_left_blinker_on = self.left_blinker_on
self.prev_right_blinker_on = self.right_blinker_on
# ******************* parse out can *******************
if self.CP.carFingerprint in (
CAR.ACCORD, CAR.ACCORD_15,
CAR.ACCORDH): # TODO: find wheels moving bit in dbc
self.standstill = cp.vl["ENGINE_DATA"]['XMISSION_SPEED'] < 0.1
self.door_all_closed = not cp.vl["SCM_FEEDBACK"][
'DRIVERS_DOOR_OPEN']
self.lead_distance = cp.vl["RADAR_HUD"]['LEAD_DISTANCE']
elif self.CP.carFingerprint in (CAR.CIVIC_BOSCH):
self.standstill = cp.vl["ENGINE_DATA"]['XMISSION_SPEED'] < 0.1
self.door_all_closed = not cp.vl["SCM_FEEDBACK"][
'DRIVERS_DOOR_OPEN']
self.hud_lead = cp.vl["ACC_HUD"]['HUD_LEAD']
else:
self.standstill = not cp.vl["STANDSTILL"]['WHEELS_MOVING']
self.door_all_closed = not any([
cp.vl["DOORS_STATUS"]['DOOR_OPEN_FL'],
cp.vl["DOORS_STATUS"]['DOOR_OPEN_FR'],
cp.vl["DOORS_STATUS"]['DOOR_OPEN_RL'],
cp.vl["DOORS_STATUS"]['DOOR_OPEN_RR']
])
self.seatbelt = not cp.vl["SEATBELT_STATUS"][
'SEATBELT_DRIVER_LAMP'] and cp.vl["SEATBELT_STATUS"][
'SEATBELT_DRIVER_LATCHED']
# 2 = temporary; 3 = TBD; 4 = significant steering wheel torque; 5 = (permanent); 6 = temporary; 7 = (permanent)
# TODO: Use values from DBC to parse this field
self.steer_error = cp.vl["STEER_STATUS"]['STEER_STATUS'] not in [
0, 2, 3, 4, 6
]
self.steer_not_allowed = cp.vl["STEER_STATUS"]['STEER_STATUS'] not in [
0, 4
] # 4 can be caused by bump OR steering nudge from driver
self.steer_warning = cp.vl["STEER_STATUS"]['STEER_STATUS'] not in [
0, 3, 4
] # 3 is low speed lockout, not worth a warning
if self.CP.radarOffCan:
self.brake_error = 0
else:
self.brake_error = cp.vl["STANDSTILL"]['BRAKE_ERROR_1'] or cp.vl[
"STANDSTILL"]['BRAKE_ERROR_2']
self.esp_disabled = cp.vl["VSA_STATUS"]['ESP_DISABLED']
# calc best v_ego estimate, by averaging two opposite corners
speed_factor = SPEED_FACTOR[self.CP.carFingerprint]
self.v_wheel_fl = cp.vl["WHEEL_SPEEDS"][
'WHEEL_SPEED_FL'] * CV.KPH_TO_MS * speed_factor
self.v_wheel_fr = cp.vl["WHEEL_SPEEDS"][
'WHEEL_SPEED_FR'] * CV.KPH_TO_MS * speed_factor
self.v_wheel_rl = cp.vl["WHEEL_SPEEDS"][
'WHEEL_SPEED_RL'] * CV.KPH_TO_MS * speed_factor
self.v_wheel_rr = cp.vl["WHEEL_SPEEDS"][
'WHEEL_SPEED_RR'] * CV.KPH_TO_MS * speed_factor
v_wheel = (self.v_wheel_fl + self.v_wheel_fr + self.v_wheel_rl +
self.v_wheel_rr) / 4.
# blend in transmission speed at low speed, since it has more low speed accuracy
self.v_weight = interp(v_wheel, v_weight_bp, v_weight_v)
speed = (1. - self.v_weight) * cp.vl["ENGINE_DATA"]['XMISSION_SPEED'] * CV.KPH_TO_MS * speed_factor + \
self.v_weight * v_wheel
if abs(
speed - self.v_ego
) > 2.0: # Prevent large accelerations when car starts at non zero speed
self.v_ego_kf.x = [[speed], [0.0]]
self.v_ego_raw = speed
v_ego_x = self.v_ego_kf.update(speed)
self.v_ego = float(v_ego_x[0])
self.a_ego = float(v_ego_x[1])
# this is a hack for the interceptor. This is now only used in the simulation
# TODO: Replace tests by toyota so this can go away
if self.CP.enableGasInterceptor:
self.user_gas = cp.vl["GAS_SENSOR"]['INTERCEPTOR_GAS']
self.user_gas_pressed = self.user_gas > 0 # this works because interceptor read < 0 when pedal position is 0. Once calibrated, this will change
self.gear = 0 if self.CP.carFingerprint == CAR.CIVIC else cp.vl[
"GEARBOX"]['GEAR']
self.angle_steers = cp.vl["STEERING_SENSORS"]['STEER_ANGLE']
self.angle_steers_rate = cp.vl["STEERING_SENSORS"]['STEER_ANGLE_RATE']
self.cruise_buttons = cp.vl["SCM_BUTTONS"]['CRUISE_BUTTONS']
self.blinker_on = cp.vl["SCM_FEEDBACK"]['LEFT_BLINKER'] or cp.vl[
"SCM_FEEDBACK"]['RIGHT_BLINKER']
self.left_blinker_on = cp.vl["SCM_FEEDBACK"]['LEFT_BLINKER']
self.right_blinker_on = cp.vl["SCM_FEEDBACK"]['RIGHT_BLINKER']
self.brake_hold = cp.vl["VSA_STATUS"]['BRAKE_HOLD_ACTIVE']
if self.CP.carFingerprint in (CAR.CIVIC, CAR.ODYSSEY, CAR.CRV_5G,
CAR.ACCORD, CAR.ACCORD_15, CAR.ACCORDH,
CAR.CIVIC_BOSCH):
self.park_brake = cp.vl["EPB_STATUS"]['EPB_STATE'] != 0
self.main_on = cp.vl["SCM_FEEDBACK"]['MAIN_ON']
else:
self.park_brake = 0 # TODO
self.main_on = cp.vl["SCM_BUTTONS"]['MAIN_ON']
can_gear_shifter = int(cp.vl["GEARBOX"]['GEAR_SHIFTER'])
self.gear_shifter = parse_gear_shifter(can_gear_shifter,
self.shifter_values)
self.pedal_gas = cp.vl["POWERTRAIN_DATA"]['PEDAL_GAS']
# crv doesn't include cruise control
if self.CP.carFingerprint in (CAR.CRV, CAR.ODYSSEY, CAR.ACURA_RDX,
CAR.RIDGELINE, CAR.PILOT_2019):
self.car_gas = self.pedal_gas
else:
self.car_gas = cp.vl["GAS_PEDAL_2"]['CAR_GAS']
self.steer_torque_driver = cp.vl["STEER_STATUS"]['STEER_TORQUE_SENSOR']
self.steer_override = abs(
self.steer_torque_driver) > STEER_THRESHOLD[self.CP.carFingerprint]
self.brake_switch = cp.vl["POWERTRAIN_DATA"]['BRAKE_SWITCH']
if self.CP.radarOffCan:
self.stopped = cp.vl["ACC_HUD"]['CRUISE_SPEED'] == 252.
self.cruise_speed_offset = calc_cruise_offset(0, self.v_ego)
if self.CP.carFingerprint in (CAR.CIVIC_BOSCH, CAR.ACCORDH):
self.brake_switch = cp.vl["POWERTRAIN_DATA"]['BRAKE_SWITCH']
self.brake_pressed = cp.vl["POWERTRAIN_DATA"]['BRAKE_PRESSED'] or \
(self.brake_switch and self.brake_switch_prev and \
cp.ts["POWERTRAIN_DATA"]['BRAKE_SWITCH'] != self.brake_switch_ts)
self.brake_switch_prev = self.brake_switch
self.brake_switch_ts = cp.ts["POWERTRAIN_DATA"]['BRAKE_SWITCH']
else:
self.brake_pressed = cp.vl["BRAKE_MODULE"]['BRAKE_PRESSED']
# On set, cruise set speed pulses between 254~255 and the set speed prev is set to avoid this.
self.v_cruise_pcm = self.v_cruise_pcm_prev if cp.vl["ACC_HUD"][
'CRUISE_SPEED'] > 160.0 else cp.vl["ACC_HUD"]['CRUISE_SPEED']
self.v_cruise_pcm_prev = self.v_cruise_pcm
else:
self.brake_switch = cp.vl["POWERTRAIN_DATA"]['BRAKE_SWITCH']
self.cruise_speed_offset = calc_cruise_offset(
cp.vl["CRUISE_PARAMS"]['CRUISE_SPEED_OFFSET'], self.v_ego)
self.v_cruise_pcm = cp.vl["CRUISE"]['CRUISE_SPEED_PCM']
# brake switch has shown some single time step noise, so only considered when
# switch is on for at least 2 consecutive CAN samples
self.brake_pressed = cp.vl["POWERTRAIN_DATA"]['BRAKE_PRESSED'] or \
(self.brake_switch and self.brake_switch_prev and \
cp.ts["POWERTRAIN_DATA"]['BRAKE_SWITCH'] != self.brake_switch_ts)
self.brake_switch_prev = self.brake_switch
self.brake_switch_ts = cp.ts["POWERTRAIN_DATA"]['BRAKE_SWITCH']
self.v_cruise_pcm = int(
min(self.v_cruise_pcm,
interp(self.angle_steers, self.Angle, self.Angle_Speed)))
self.user_brake = cp.vl["VSA_STATUS"]['USER_BRAKE']
self.pcm_acc_status = cp.vl["POWERTRAIN_DATA"]['ACC_STATUS']
self.hud_lead = cp.vl["ACC_HUD"]['HUD_LEAD']
if self.cruise_setting == 3:
if cp.vl["SCM_BUTTONS"]["CRUISE_SETTING"] == 0:
self.trMode = (self.trMode + 1) % 3
kegman.save({'lastTrMode': int(self.trMode)
}) # write last distance bar setting to file
self.cstm_btns.btns[4].btn_label2 = self.trLabels[self.trMode]
self.read_distance_lines = self.trMode + 1
if self.read_distance_lines <> self.read_distance_lines_prev:
if self.read_distance_lines == 1:
self.UE.custom_alert_message(2,
"Following distance set to 0.9s",
200, 3)
if self.read_distance_lines == 2:
self.UE.custom_alert_message(2, "Dynamic Following distance",
200, 3)
if self.read_distance_lines == 3:
self.UE.custom_alert_message(2,
"Following distance set to 2.7s",
200, 3)
self.read_distance_lines_prev = self.read_distance_lines
# when user presses LKAS button on steering wheel
if self.cruise_setting == 1:
if cp.vl["SCM_BUTTONS"]["CRUISE_SETTING"] == 0:
if self.lkMode:
self.lkMode = False
else:
self.lkMode = True
self.prev_cruise_setting = self.cruise_setting
self.cruise_setting = cp.vl["SCM_BUTTONS"]['CRUISE_SETTING']
if self.cstm_btns.get_button_status("lka") == 0:
self.lane_departure_toggle_on = False
else:
if self.alcaMode == 3 and (self.left_blinker_on
or self.right_blinker_on):
self.lane_departure_toggle_on = False
else:
self.lane_departure_toggle_on = True
# Gets rid of Pedal Grinding noise when brake is pressed at slow speeds for some models
# TODO: this should be ok for all cars. Verify it.
if self.CP.carFingerprint in (CAR.PILOT, CAR.PILOT_2019,
CAR.RIDGELINE):
if self.user_brake > 0.05:
self.brake_pressed = 1
def __init__(self, CP):
self.lkMode = True
self.gasMode = int(kegman.conf['lastGasMode'])
self.gasLabels = ["dynamic", "sport", "eco"]
self.Angle = [0, 5, 10, 15, 20, 25, 30, 35, 60, 100, 180, 270, 500]
self.Angle_Speed = [
255, 160, 100, 80, 70, 60, 55, 50, 40, 30, 20, 10, 5
]
self.blind_spot_on = bool(0)
#labels for ALCA modes
self.alcaLabels = ["MadMax", "Normal", "Wifey", "off"]
steerRatio = CP.steerRatio
self.trLabels = ["0.9", "dyn", "2.7"]
self.alcaMode = int(kegman.conf['lastALCAMode']
) # default to last ALCA Mode on startup
if self.alcaMode > 3:
self.alcaMode = 3
kegman.save({'lastALCAMode': int(self.alcaMode)
}) # write last distance bar setting to file
self.trMode = int(kegman.conf['lastTrMode']
) # default to last distance interval on startup
if self.trMode > 2:
self.trMode = 2
kegman.save({'lastTrMode': int(self.trMode)
}) # write last distance bar setting to file
#if (CP.carFingerprint == CAR.MODELS):
# ALCA PARAMS
# max REAL delta angle for correction vs actuator
self.CL_MAX_ANGLE_DELTA_BP = [10., 32., 55.]
self.CL_MAX_ANGLE_DELTA = [
2.0 * 15.4 / steerRatio, 1. * 15.4 / steerRatio,
0.5 * 15.4 / steerRatio
]
# adjustment factor for merging steer angle to actuator; should be over 4; the higher the smoother
self.CL_ADJUST_FACTOR_BP = [10., 44.]
self.CL_ADJUST_FACTOR = [16., 8.]
# reenrey angle when to let go
self.CL_REENTRY_ANGLE_BP = [10., 44.]
self.CL_REENTRY_ANGLE = [5., 5.]
# a jump in angle above the CL_LANE_DETECT_FACTOR means we crossed the line
self.CL_LANE_DETECT_BP = [10., 44.]
self.CL_LANE_DETECT_FACTOR = [1.5, 2.5]
self.CL_LANE_PASS_BP = [10., 20., 44.]
self.CL_LANE_PASS_TIME = [40., 10., 4.]
# change lane delta angles and other params
self.CL_MAXD_BP = [10., 32., 44.]
self.CL_MAXD_A = [
.358, 0.084, 0.040
] #delta angle based on speed; needs fine tune, based on Tesla steer ratio of 16.75
self.CL_MIN_V = 8.9 # do not turn if speed less than x m/2; 20 mph = 8.9 m/s
# do not turn if actuator wants more than x deg for going straight; this should be interp based on speed
self.CL_MAX_A_BP = [10., 44.]
self.CL_MAX_A = [10., 10.]
# define limits for angle change every 0.1 s
# we need to force correction above 10 deg but less than 20
# anything more means we are going to steep or not enough in a turn
self.CL_MAX_ACTUATOR_DELTA = 2.
self.CL_MIN_ACTUATOR_DELTA = 0.
self.CL_CORRECTION_FACTOR = [1., 1.1, 1.2]
self.CL_CORRECTION_FACTOR_BP = [10., 32., 44.]
#duration after we cross the line until we release is a factor of speed
self.CL_TIMEA_BP = [10., 32., 44.]
self.CL_TIMEA_T = [0.7, 0.30, 0.30]
#duration to wait (in seconds) with blinkers on before starting to turn
self.CL_WAIT_BEFORE_START = 1
#END OF ALCA PARAMS
self.read_distance_lines_prev = 3
self.CP = CP
self.can_define = CANDefine(DBC[CP.carFingerprint]['pt'])
self.shifter_values = self.can_define.dv["GEARBOX"]["GEAR_SHIFTER"]
self.user_gas, self.user_gas_pressed = 0., 0
self.brake_switch_prev = 0
self.brake_switch_ts = 0
self.lead_distance = 255
self.hud_lead = 0
self.cruise_buttons = 0
self.cruise_setting = 0
self.v_cruise_pcm_prev = 0
self.blinker_on = 0
self.left_blinker_on = 0
self.right_blinker_on = 0
self.stopped = 0
#BB UIEvents
self.UE = UIEvents(self)
#BB variable for custom buttons
self.cstm_btns = UIButtons(self, "Honda", "honda")
#BB pid holder for ALCA
self.pid = None
#BB custom message counter
self.custom_alert_counter = -1 #set to 100 for 1 second display; carcontroller will take down to zero
# vEgo kalman filter
dt = 0.01
# 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], [0.0, 1.0]],
C=[[1.0, 0.0]],
K=[[0.12287673], [0.29666309]])
self.v_ego = 0.0