105 lines
2.3 KiB
Python
105 lines
2.3 KiB
Python
from selfdrive.car.mazda.values import CAR, Buttons
|
|
|
|
def create_steering_control(packer, car_fingerprint, frame, apply_steer, lkas):
|
|
|
|
tmp = apply_steer + 2048
|
|
|
|
lo = tmp & 0xFF
|
|
hi = tmp >> 8
|
|
|
|
b1 = int(lkas["BIT_1"])
|
|
ldw = int(lkas["LDW"])
|
|
er1 = int(lkas["ERR_BIT_1"])
|
|
lnv = 0
|
|
er2 = int(lkas["ERR_BIT_2"])
|
|
|
|
steering_angle = int(lkas["STEERING_ANGLE"])
|
|
b2 = int(lkas["ANGLE_ENABLED"])
|
|
|
|
tmp = steering_angle + 2048
|
|
ahi = tmp >> 10
|
|
amd = (tmp & 0x3FF) >> 2
|
|
amd = (amd >> 4) | (( amd & 0xF) << 4)
|
|
alo = (tmp & 0x3) << 2
|
|
|
|
ctr = frame % 16
|
|
# bytes: [ 1 ] [ 2 ] [ 3 ] [ 4 ]
|
|
csum = 249 - ctr - hi - lo - (lnv << 3) - er1 - (ldw << 7) - ( er2 << 4) - (b1 << 5)
|
|
|
|
#bytes [ 5 ] [ 6 ] [ 7 ]
|
|
csum = csum - ahi - amd - alo - b2
|
|
|
|
if ahi == 1:
|
|
csum = csum + 15
|
|
|
|
if csum < 0:
|
|
if csum < -256:
|
|
csum = csum + 512
|
|
else:
|
|
csum = csum + 256
|
|
|
|
csum = csum % 256
|
|
|
|
if car_fingerprint == CAR.CX5:
|
|
values = {
|
|
"LKAS_REQUEST" : apply_steer,
|
|
"CTR" : ctr,
|
|
"ERR_BIT_1" : er1,
|
|
"LINE_NOT_VISIBLE" : lnv,
|
|
"LDW" : ldw,
|
|
"BIT_1" : b1,
|
|
"ERR_BIT_2" : er2,
|
|
"STEERING_ANGLE" : steering_angle,
|
|
"ANGLE_ENABLED" : b2,
|
|
"CHKSUM" : csum
|
|
}
|
|
|
|
return packer.make_can_msg("CAM_LKAS", 0, values)
|
|
|
|
|
|
def create_button_cmd(packer, car_fingerprint, button):
|
|
|
|
if button == Buttons.CANCEL:
|
|
can = 1
|
|
res = 0
|
|
elif button == Buttons.RESUME:
|
|
can = 0
|
|
res = 1
|
|
else:
|
|
can = 0
|
|
res = 0
|
|
|
|
if car_fingerprint == CAR.CX5:
|
|
values = {
|
|
"CAN_OFF" : can,
|
|
"CAN_OFF_INV" : (can + 1) % 2,
|
|
|
|
"SET_P" : 0,
|
|
"SET_P_INV" : 1,
|
|
|
|
"RES" : res,
|
|
"RES_INV" : (res + 1) % 2,
|
|
|
|
"SET_M" : 0,
|
|
"SET_M_INV" : 1,
|
|
|
|
"DISTANCE_LESS" : 0,
|
|
"DISTANCE_LESS_INV" : 1,
|
|
|
|
"DISTANCE_MORE" : 0,
|
|
"DISTANCE_MORE_INV" : 1,
|
|
|
|
"MODE_X" : 0,
|
|
"MODE_X_INV" : 1,
|
|
|
|
"MODE_Y" : 0,
|
|
"MODE_Y_INV" : 1,
|
|
|
|
"BIT1" : 1,
|
|
"BIT2" : 1,
|
|
"BIT3" : 1,
|
|
"CTR" : 0
|
|
}
|
|
|
|
return packer.make_can_msg("CRZ_BTNS", 0, values)
|