mawei/MoreTore
1
0
Fork 0
mirror of https://github.com/MoreTore/openpilot.git synced 2026-09-23 16:03:54 +08:00
Code Issues Packages Projects Releases Wiki Activity

no fsc fix

Browse Source
This commit is contained in:
MoreTore
2025-02-15 13:41:24 -06:00
parent b587098c57
commit 13efb81551
1 changed files with 52 additions and 54 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/car/mazda/mazdacan.py
+52 -54
View File
@@ -4,70 +4,68 @@ from openpilot.common.numpy_fast import clip
def create_steering_control(packer, CP, frame, apply_steer, lkas):
msgs = []
if CP.flags & MazdaFlags.GEN1:
tmp = apply_steer + 2048
if not CP.flags & MazdaFlags.NO_FSC:
tmp = apply_steer + 2048
lo = tmp & 0xFF
hi = tmp >> 8
lo = tmp & 0xFF
hi = tmp >> 8
# copy values from camera
b1 = int(lkas["BIT_1"])
er1 = int(lkas["ERR_BIT_1"])
lnv = 0
ldw = 0
er2 = int(lkas["ERR_BIT_2"])
# copy values from camera
b1 = int(lkas["BIT_1"])
er1 = int(lkas["ERR_BIT_1"])
lnv = 0
ldw = 0
er2 = int(lkas["ERR_BIT_2"])
# Some older models do have these, newer models don't.
# Either way, they all work just fine if set to zero.
steering_angle = 0
b2 = 0
# Some older models do have these, newer models don't.
# Either way, they all work just fine if set to zero.
steering_angle = 0
b2 = 0
tmp = steering_angle + 2048
ahi = tmp >> 10
amd = (tmp & 0x3FF) >> 2
amd = (amd >> 4) | (( amd & 0xF) << 4)
alo = (tmp & 0x3) << 2
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)
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
# bytes [ 5 ] [ 6 ] [ 7 ]
csum = csum - ahi - amd - alo - b2
if ahi == 1:
csum = csum + 15
if ahi == 1:
csum = csum + 15
if csum < 0:
if csum < -256:
csum = csum + 512
else:
csum = csum + 256
if csum < 0:
if csum < -256:
csum = csum + 512
else:
csum = csum + 256
csum = csum % 256
values = {}
if CP.flags & MazdaFlags.GEN1:
if not CP.flags & MazdaFlags.NO_FSC:
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
}
msgs.append(packer.make_can_msg("CAM_LKAS", 0, values))
csum = csum % 256
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
}
msgs.append(packer.make_can_msg("CAM_LKAS", 0, values))
if CP.flags & MazdaFlags.TORQUE_INTERCEPTOR:
values = {
"LKAS_REQUEST" : apply_steer,
"CHKSUM" : apply_steer,
"KEY" : 3294744160
}
msgs.append(packer.make_can_msg("CAM_LKAS2", 1, values))
if CP.flags & MazdaFlags.TORQUE_INTERCEPTOR:
values = {
"LKAS_REQUEST" : apply_steer,
"CHKSUM" : apply_steer,
"KEY" : 3294744160
}
msgs.append(packer.make_can_msg("CAM_LKAS2", 1, values))
elif CP.flags & MazdaFlags.GEN2:
bus = 1