298 lines
9.7 KiB
Python
298 lines
9.7 KiB
Python
import math
|
|
|
|
from cereal import log
|
|
from openpilot.common.conversions import Conversions as CV
|
|
from openpilot.common.realtime import DT_CTRL
|
|
from openpilot.selfdrive.car import make_can_msg
|
|
from openpilot.selfdrive.car.gm.values import CAR, CruiseButtons, CanBus
|
|
|
|
|
|
def create_buttons(packer, bus, idx, button):
|
|
values = {
|
|
"ACCButtons": button,
|
|
"RollingCounter": idx,
|
|
"ACCAlwaysOne": 1,
|
|
"DistanceButton": 0,
|
|
"ACCByFive": 0,
|
|
}
|
|
|
|
checksum = 240 + int(values["ACCAlwaysOne"] * 0xf)
|
|
checksum += values["RollingCounter"] * (0x4ef if values["ACCAlwaysOne"] != 0 else 0x3f0)
|
|
checksum -= int(values["ACCButtons"] - 1) << 4 # not correct if value is 0
|
|
checksum -= 2 * values["DistanceButton"]
|
|
|
|
values["SteeringButtonChecksum"] = checksum
|
|
return packer.make_can_msg("ASCMSteeringButton", bus, values)
|
|
|
|
def create_buttons_five(packer, bus, idx, button, byfive):
|
|
if byfive and button == CruiseButtons.RES_ACCEL:
|
|
accbyfive = 2
|
|
ACCAlwaysOne = 1
|
|
checkoffset = -4
|
|
elif byfive and button == CruiseButtons.DECEL_SET:
|
|
accbyfive = 3
|
|
ACCAlwaysOne = 0
|
|
checkoffset = 255*idx+10
|
|
else:
|
|
accbyfive = 0
|
|
ACCAlwaysOne = 1
|
|
checkoffset = 0
|
|
|
|
values = {
|
|
"ACCButtons": button,
|
|
"RollingCounter": idx,
|
|
"ACCAlwaysOne": ACCAlwaysOne,
|
|
"DistanceButton": 0,
|
|
"ACCByFive": accbyfive,
|
|
}
|
|
|
|
checksum = 240 + int(values["ACCAlwaysOne"] * 0xf)
|
|
checksum += values["RollingCounter"] * (0x4ef if values["ACCAlwaysOne"] != 0 else 0x3f0)
|
|
checksum -= int(values["ACCButtons"] - 1) << 4 # not correct if value is 0
|
|
checksum -= 2 * values["DistanceButton"]
|
|
checksum += checkoffset
|
|
|
|
values["SteeringButtonChecksum"] = checksum
|
|
return packer.make_can_msg("ASCMSteeringButton", bus, values)
|
|
|
|
|
|
def create_pscm_status(packer, bus, pscm_status):
|
|
values = {s: pscm_status[s] for s in [
|
|
"HandsOffSWDetectionMode",
|
|
"HandsOffSWlDetectionStatus",
|
|
"LKATorqueDeliveredStatus",
|
|
"LKADriverAppldTrq",
|
|
"LKATorqueDelivered",
|
|
"LKATotalTorqueDelivered",
|
|
"RollingCounter",
|
|
"PSCMStatusChecksum",
|
|
]}
|
|
checksum_mod = int(1 - values["HandsOffSWlDetectionStatus"]) << 5
|
|
values["HandsOffSWlDetectionStatus"] = 1
|
|
values["PSCMStatusChecksum"] += checksum_mod
|
|
return packer.make_can_msg("PSCMStatus", bus, values)
|
|
|
|
|
|
def create_steering_control(packer, bus, apply_steer, idx, lkas_active):
|
|
values = {
|
|
"LKASteeringCmdActive": lkas_active,
|
|
"LKASteeringCmd": apply_steer,
|
|
"RollingCounter": idx,
|
|
"LKASteeringCmdChecksum": 0x1000 - (lkas_active << 11) - (apply_steer & 0x7ff) - idx
|
|
}
|
|
|
|
return packer.make_can_msg("ASCMLKASteeringCmd", bus, values)
|
|
|
|
|
|
def create_adas_keepalive(bus):
|
|
dat = b"\x00\x00\x00\x00\x00\x00\x00"
|
|
return [make_can_msg(0x409, dat, bus), make_can_msg(0x40a, dat, bus)]
|
|
|
|
|
|
def create_gas_regen_command(packer, bus, throttle, idx, enabled, at_full_stop):
|
|
values = {
|
|
"GasRegenCmdActive": enabled,
|
|
"RollingCounter": idx,
|
|
"GasRegenCmdActiveInv": 1 - enabled,
|
|
"GasRegenCmd": throttle,
|
|
"GasRegenFullStopActive": at_full_stop,
|
|
"GasRegenAlwaysOne": 1,
|
|
"GasRegenAlwaysOne2": 1,
|
|
"GasRegenAlwaysOne3": 1,
|
|
}
|
|
|
|
dat = packer.make_can_msg("ASCMGasRegenCmd", bus, values)[2]
|
|
values["GasRegenChecksum"] = (((0xff - dat[1]) & 0xff) << 16) | \
|
|
(((0xff - dat[2]) & 0xff) << 8) | \
|
|
((0x100 - dat[3] - idx) & 0xff)
|
|
|
|
return packer.make_can_msg("ASCMGasRegenCmd", bus, values)
|
|
|
|
|
|
def create_friction_brake_command(packer, bus, apply_brake, idx, enabled, near_stop, at_full_stop, CP):
|
|
mode = 0x1
|
|
|
|
# TODO: Understand this better. Volts and ICE Camera ACC cars are 0x1 when enabled with no brake
|
|
if enabled and CP.carFingerprint in (CAR.BOLT_EUV,):
|
|
mode = 0x9
|
|
|
|
if apply_brake > 0:
|
|
mode = 0xa
|
|
if at_full_stop:
|
|
mode = 0xd
|
|
|
|
# TODO: this is to have GM bringing the car to complete stop,
|
|
# but currently it conflicts with OP controls, so turned off. Not set by all cars
|
|
#elif near_stop:
|
|
# mode = 0xb
|
|
|
|
brake = (0x1000 - apply_brake) & 0xfff
|
|
checksum = (0x10000 - (mode << 12) - brake - idx) & 0xffff
|
|
|
|
values = {
|
|
"RollingCounter": idx,
|
|
"FrictionBrakeMode": mode,
|
|
"FrictionBrakeChecksum": checksum,
|
|
"FrictionBrakeCmd": -apply_brake
|
|
}
|
|
|
|
return packer.make_can_msg("EBCMFrictionBrakeCmd", bus, values)
|
|
|
|
|
|
def create_acc_dashboard_command(packer, bus, enabled, target_speed_kph, hud_control, fcw):
|
|
target_speed = min(target_speed_kph, 255)
|
|
|
|
values = {
|
|
"ACCAlwaysOne": 1,
|
|
"ACCResumeButton": 0,
|
|
"ACCSpeedSetpoint": target_speed,
|
|
"ACCGapLevel": hud_control.leadDistanceBars * enabled, # 3 "far", 0 "inactive"
|
|
"ACCCmdActive": enabled,
|
|
"ACCAlwaysOne2": 1,
|
|
"ACCLeadCar": hud_control.leadVisible,
|
|
"FCWAlert": 0x3 if fcw else 0
|
|
}
|
|
|
|
return packer.make_can_msg("ASCMActiveCruiseControlStatus", bus, values)
|
|
|
|
|
|
def create_adas_time_status(bus, tt, idx):
|
|
dat = [(tt >> 20) & 0xff, (tt >> 12) & 0xff, (tt >> 4) & 0xff,
|
|
((tt & 0xf) << 4) + (idx << 2)]
|
|
chksum = 0x1000 - dat[0] - dat[1] - dat[2] - dat[3]
|
|
chksum = chksum & 0xfff
|
|
dat += [0x40 + (chksum >> 8), chksum & 0xff, 0x12]
|
|
return make_can_msg(0xa1, bytes(dat), bus)
|
|
|
|
|
|
def create_adas_steering_status(bus, idx):
|
|
dat = [idx << 6, 0xf0, 0x20, 0, 0, 0]
|
|
chksum = 0x60 + sum(dat)
|
|
dat += [chksum >> 8, chksum & 0xff]
|
|
return make_can_msg(0x306, bytes(dat), bus)
|
|
|
|
|
|
def create_adas_accelerometer_speed_status(bus, speed_ms, idx):
|
|
spd = int(speed_ms * 16) & 0xfff
|
|
accel = 0 & 0xfff
|
|
# 0 if in park/neutral, 0x10 if in reverse, 0x08 for D/L
|
|
#stick = 0x08
|
|
near_range_cutoff = 0x27
|
|
near_range_mode = 1 if spd <= near_range_cutoff else 0
|
|
far_range_mode = 1 - near_range_mode
|
|
dat = [0x08, spd >> 4, ((spd & 0xf) << 4) | (accel >> 8), accel & 0xff, 0]
|
|
chksum = 0x62 + far_range_mode + (idx << 2) + dat[0] + dat[1] + dat[2] + dat[3] + dat[4]
|
|
dat += [(idx << 5) + (far_range_mode << 4) + (near_range_mode << 3) + (chksum >> 8), chksum & 0xff]
|
|
return make_can_msg(0x308, bytes(dat), bus)
|
|
|
|
|
|
def create_adas_headlights_status(packer, bus):
|
|
values = {
|
|
"Always42": 0x42,
|
|
"Always4": 0x4,
|
|
}
|
|
return packer.make_can_msg("ASCMHeadlight", bus, values)
|
|
|
|
|
|
def create_lka_icon_command(bus, active, critical, steer):
|
|
if active and steer == 1:
|
|
if critical:
|
|
dat = b"\x50\xc0\x14"
|
|
else:
|
|
dat = b"\x50\x40\x18"
|
|
elif active:
|
|
if critical:
|
|
dat = b"\x40\xc0\x14"
|
|
else:
|
|
dat = b"\x40\x40\x18"
|
|
else:
|
|
dat = b"\x00\x00\x00"
|
|
return make_can_msg(0x104c006c, dat, bus)
|
|
|
|
|
|
def create_gm_cc_spam_command(packer, controller, CS, actuators):
|
|
if controller.params_.get_bool("IsMetric"):
|
|
_CV = CV.MS_TO_KPH
|
|
RATE_UP_MAX = 0.04
|
|
RATE_DOWN_MAX = 0.04
|
|
else:
|
|
_CV = CV.MS_TO_MPH
|
|
RATE_UP_MAX = 0.2
|
|
RATE_DOWN_MAX = 0.2
|
|
|
|
accel = actuators.accel * _CV # m/s/s to mph/s
|
|
speedSetPoint = int(round(CS.out.cruiseState.speed * _CV))
|
|
|
|
cruiseBtn = CruiseButtons.INIT
|
|
if speedSetPoint == CS.CP.minEnableSpeed and accel < -1:
|
|
cruiseBtn = CruiseButtons.CANCEL
|
|
controller.apply_speed = 0
|
|
rate = 0.04
|
|
elif accel < 0:
|
|
cruiseBtn = CruiseButtons.DECEL_SET
|
|
if speedSetPoint > (CS.out.vEgo * _CV) + 3.0: # If accel is changing directions, bring set speed to current speed as fast as possible
|
|
rate = RATE_DOWN_MAX
|
|
else:
|
|
rate = max(-1 / accel, RATE_DOWN_MAX)
|
|
controller.apply_speed = speedSetPoint - 1
|
|
elif accel > 0:
|
|
cruiseBtn = CruiseButtons.RES_ACCEL
|
|
if speedSetPoint < (CS.out.vEgo * _CV) - 3.0:
|
|
rate = RATE_UP_MAX
|
|
else:
|
|
rate = max(1 / accel, RATE_UP_MAX)
|
|
controller.apply_speed = speedSetPoint + 1
|
|
else:
|
|
controller.apply_speed = speedSetPoint
|
|
rate = float('inf')
|
|
|
|
# Check rlogs closely - our message shouldn't show up on the pt bus for us
|
|
# Or bus 2, since we're forwarding... but I think it does
|
|
if (cruiseBtn != CruiseButtons.INIT) and ((controller.frame - controller.last_button_frame) * DT_CTRL > rate):
|
|
controller.last_button_frame = controller.frame
|
|
idx = (CS.buttons_counter + 1) % 4 # Need to predict the next idx for '22-23 EUV
|
|
return [create_buttons(packer, CanBus.POWERTRAIN, idx, cruiseBtn)]
|
|
else:
|
|
return []
|
|
|
|
def create_gm_acc_spam_command(packer, controller, CS, slcSet, bus, Vego, frogpilot_variables, accel, sdgm):
|
|
cruiseBtn = CruiseButtons.INIT
|
|
byfive = 0
|
|
|
|
MS_CONVERT = CV.MS_TO_KPH if frogpilot_variables.is_metric else CV.MS_TO_MPH
|
|
|
|
speedSetPoint = int(round(CS.out.cruiseState.speed * MS_CONVERT))
|
|
slcSet = int(round(slcSet * MS_CONVERT))
|
|
|
|
FRAMES_ON = 6
|
|
FRAMES_OFF = (30 if sdgm else 12) - FRAMES_ON
|
|
|
|
if not frogpilot_variables.experimentalMode:
|
|
if slcSet + 5 < Vego * MS_CONVERT:
|
|
slcSet = slcSet - 10 # 10 lower to increase deceleration until with 5
|
|
else:
|
|
slcSet = int(round((Vego * 1.01 + 4.6 * accel + 0.7 * accel ** 3 - 1 / 35 * accel ** 5) * MS_CONVERT)) # 1.01 factor to match cluster speed better
|
|
|
|
if slcSet <= int(math.floor((speedSetPoint - 1)/5.0)*5.0) and speedSetPoint > (25 if frogpilot_variables.is_metric else 20) and sdgm:
|
|
cruiseBtn = CruiseButtons.DECEL_SET
|
|
byfive = 1
|
|
elif slcSet >= int(math.ceil((speedSetPoint + 1)/5.0)*5.0) and sdgm:
|
|
cruiseBtn = CruiseButtons.RES_ACCEL
|
|
byfive = 1
|
|
elif slcSet < speedSetPoint and speedSetPoint > (25 if frogpilot_variables.is_metric else 16):
|
|
cruiseBtn = CruiseButtons.DECEL_SET
|
|
byfive = 0
|
|
elif slcSet > speedSetPoint:
|
|
cruiseBtn = CruiseButtons.RES_ACCEL
|
|
byfive = 0
|
|
else:
|
|
cruiseBtn = CruiseButtons.INIT
|
|
byfive = 0
|
|
|
|
if (cruiseBtn != CruiseButtons.INIT) and controller.frame % (FRAMES_ON + FRAMES_OFF) < FRAMES_ON:
|
|
controller.last_button_frame = controller.frame
|
|
idx = (CS.buttons_counter + (1 if sdgm else 2)) % 4
|
|
return [create_buttons_five(packer, bus, idx, cruiseBtn, byfive)]*(23 if sdgm else 3)
|
|
else:
|
|
return []
|