Update checksum to account for steer angle signal used in some cars

Signed-off-by: Jafar Al-Gharaibeh <to.jafar@gmail.com>
pull/988/head
Jafar Al-Gharaibeh 5 years ago committed by Adeeb Shihadeh
parent 44c36ab275
commit 340907221b
  1. 4
      selfdrive/car/mazda/carcontroller.py
  2. 9
      selfdrive/car/mazda/carstate.py
  3. 36
      selfdrive/car/mazda/mazdacan.py

@ -17,11 +17,11 @@ class CarController():
### STEER ###
if enabled and not CS.steer_not_allowed:
if enabled:
# calculate steer and also set limits due to driver torque
new_steer = int(round(actuators.steer * SteerLimitParams.STEER_MAX))
apply_steer = apply_std_steer_torque_limits(new_steer, self.apply_steer_last,
CS.out.steer_torque_driver, SteerLimitParams)
CS.out.steeringTorque, SteerLimitParams)
self.steer_rate_limited = new_steer != apply_steer
else:
apply_steer = 0

@ -177,14 +177,15 @@ class CarState(CarStateBase):
signals = [
# sig_name, sig_address, default
("LKAS_REQUEST", "CAM_LKAS", 0),
("LKAS_REQUEST", "CAM_LKAS", 2048),
("CTR", "CAM_LKAS", 0),
("ERR_BIT_1", "CAM_LKAS", 0),
("LDW", "CAM_LKAS", 0),
("LINE_NOT_VISIBLE", "CAM_LKAS", 0),
("BIT_1", "CAM_LKAS", 0),
("LDW", "CAM_LKAS", 0),
("BIT_1", "CAM_LKAS", 1),
("ERR_BIT_2", "CAM_LKAS", 0),
("BIT_2", "CAM_LKAS", 0),
("LKAS_ANGLE", "CAM_LKAS", 2048),
("BIT2", "CAM_LKAS", 0),
("CHKSUM", "CAM_LKAS", 0),
]

@ -8,29 +8,49 @@ def create_steering_control(packer, car_fingerprint, frame, apply_steer, lkas):
hi = tmp >> 8
b1 = int(lkas["BIT_1"])
b2 = int(lkas[ "BIT_2"])
ldw = int(lkas["LDW"])
er1= int(lkas["ERR_BIT_1"])
lnv = 0 #int(lkas["LINE_NOT_VISIBLE"])
er2= int(lkas["ERR_BIT_2"])
lkas_angle = lkas["LKAS_ANGLE"]
b2 = int(lkas["BIT2"])
tmp = int((lkas_angle + 45.06 ) / 0.022)
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
csum = 241 - ctr - (hi - 8) - lo - (lnv << 3) - (b1 << 5) - (b2 << 1) - (ldw << 7)
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 = {
"CTR" : ctr,
"LKAS_REQUEST" : apply_steer,
"BIT_1" : b1,
"BIT_2" : b2,
"LDW" : ldw,
"CTR" : ctr,
"ERR_BIT_1" : er1,
"LINE_NOT_VISIBLE" : lnv,
"ERR_BIT_1" : 0,
"ERR_BIT_2" : 0,
"LDW" : ldw,
"BIT_1" : b1,
"ERR_BIT_2" : er2,
"LKAS_ANGLE" : lkas_angle,
"BIT2" : b2,
"CHKSUM" : csum
}

Loading…
Cancel
Save