def test_steering_ipas(self):
self.longMessage = True
car_name = TOYOTA.RAV4
sendcan = messaging.pub_sock('sendcan')
params = CarInterface.get_params(car_name)
params.enableApgs = True
CI = CarInterface(params, CarController)
CI.CC.angle_control = True
# Get parser
parser_signals = [
('SET_ME_X10', 'STEERING_IPAS', 0),
('SET_ME_X40', 'STEERING_IPAS', 0),
('ANGLE', 'STEERING_IPAS', 0),
('STATE', 'STEERING_IPAS', 0),
('DIRECTION_CMD', 'STEERING_IPAS', 0),
]
parser = CANParser(CI.cp.dbc_name, parser_signals, [], 0, sendcan=True, tcp_addr="127.0.0.1")
time.sleep(0.2) # Slow joiner syndrome
for enabled in [True, False]:
for steer in np.linspace(-510., 510., 25):
control = car.CarControl.new_message()
actuators = car.CarControl.Actuators.new_message()
actuators.steerAngle = float(steer)
control.enabled = enabled
control.actuators = actuators
CI.update(control)
CI.CS.steer_not_allowed = False
for _ in range(1000 if steer < -505 else 25):
can_sends = CI.apply(control)
sendcan.send(can_list_to_can_capnp(can_sends, msgtype='sendcan'))
parser.update(int(sec_since_boot() * 1e9), False)
self.assertEqual(0x10, parser.vl['STEERING_IPAS']['SET_ME_X10'])
self.assertEqual(0x40, parser.vl['STEERING_IPAS']['SET_ME_X40'])
expected_state = 3 if enabled else 1
self.assertEqual(expected_state, parser.vl['STEERING_IPAS']['STATE'])
if steer < 0:
direction = 3
elif steer > 0:
direction = 1
else:
direction = 2
if not enabled:
direction = 2
self.assertEqual(direction, parser.vl['STEERING_IPAS']['DIRECTION_CMD'])
expected_steer = int(round(steer / 1.5)) * 1.5 if enabled else 0
self.assertAlmostEqual(expected_steer, parser.vl['STEERING_IPAS']['ANGLE'])
sendcan.close()
def test_steering(self):
self.longMessage = True
car_name = TOYOTA.RAV4
sendcan = messaging.pub_sock('sendcan')
params = CarInterface.get_params(car_name)
CI = CarInterface(params, CarController)
limit = 1500
# Get parser
parser_signals = [
('STEER_REQUEST', 'STEERING_LKA', 0),
('SET_ME_1', 'STEERING_LKA', 0),
('STEER_TORQUE_CMD', 'STEERING_LKA', -1),
('LKA_STATE', 'STEERING_LKA', -1),
]
parser = CANParser(CI.cp.dbc_name, parser_signals, [], 0, sendcan=True, tcp_addr="127.0.0.1")
time.sleep(0.2) # Slow joiner syndrome
for steer in np.linspace(-1., 1., 25):
control = car.CarControl.new_message()
actuators = car.CarControl.Actuators.new_message()
actuators.steer = float(steer)
control.enabled = True
control.actuators = actuators
CI.update(control)
CI.CS.steer_not_allowed = False
CI.CS.steer_torque_motor = limit * steer
# More control applies for the first one because of rate limits
for _ in range(1000 if steer < -0.99 else 25):
can_sends = CI.apply(control)
sendcan.send(can_list_to_can_capnp(can_sends, msgtype='sendcan'))
parser.update(int(sec_since_boot() * 1e9), False)
self.assertEqual(1, parser.vl['STEERING_LKA']['SET_ME_1'])
self.assertEqual(True, parser.vl['STEERING_LKA']['STEER_REQUEST'])
self.assertAlmostEqual(round(steer * limit), parser.vl['STEERING_LKA']['STEER_TORQUE_CMD'])
self.assertEqual(0, parser.vl['STEERING_LKA']['LKA_STATE'])
sendcan.close()
def test_accel(self):
self.longMessage = True
car_name = TOYOTA.RAV4
sendcan = messaging.pub_sock('sendcan')
params = CarInterface.get_params(car_name)
CI = CarInterface(params, CarController)
# Get parser
parser_signals = [
('ACCEL_CMD', 'ACC_CONTROL', 0),
]
parser = CANParser(CI.cp.dbc_name, parser_signals, [], 0, sendcan=True, tcp_addr="127.0.0.1")
time.sleep(0.2) # Slow joiner syndrome
for accel in np.linspace(-3., 1.5, 25):
control = car.CarControl.new_message()
actuators = car.CarControl.Actuators.new_message()
gas = accel / 3. if accel > 0. else 0.
brake = -accel / 3. if accel < 0. else 0.
actuators.gas = float(gas)
actuators.brake = float(brake)
control.enabled = True
control.actuators = actuators
CI.update(control)
# More control applies for the first one because of rate limits
for _ in range(25):
can_sends = CI.apply(control)
sendcan.send(can_list_to_can_capnp(can_sends, msgtype='sendcan'))
for _ in range(5):
parser.update(int(sec_since_boot() * 1e9), False)
time.sleep(0.01)
min_accel = accel - 0.061
max_accel = accel + 0.061
sent_accel = parser.vl['ACC_CONTROL']['ACCEL_CMD']
accel_ok = min_accel <= sent_accel <= max_accel
self.assertTrue(accel_ok, msg="%.2f <= %.2f <= %.2f" % (min_accel, sent_accel, max_accel))
sendcan.close()
def test_fcw(self):
# TODO: This message has a 0xc1 setme which is not yet checked or in the dbc file
self.longMessage = True
car_name = TOYOTA.RAV4
sendcan = messaging.pub_sock('sendcan')
params = CarInterface.get_params(car_name)
CI = CarInterface(params, CarController)
# Get parser
parser_signals = [
('FCW', 'ACC_HUD', 0),
('SET_ME_X20', 'ACC_HUD', 0),
('SET_ME_X10', 'ACC_HUD', 0),
('SET_ME_X80', 'ACC_HUD', 0),
]
parser = CANParser(CI.cp.dbc_name, parser_signals, [], 0, sendcan=True, tcp_addr="127.0.0.1")
time.sleep(0.2) # Slow joiner syndrome
VA = car.CarControl.HUDControl.VisualAlert
for fcw in [True, False]:
control = car.CarControl.new_message()
control.enabled = True
hud = car.CarControl.HUDControl.new_message()
if fcw:
hud.visualAlert = VA.fcw
control.hudControl = hud
CI.update(control)
for _ in range(200):
can_sends = CI.apply(control)
sendcan.send(can_list_to_can_capnp(can_sends, msgtype='sendcan'))
for _ in range(5):
parser.update(int(sec_since_boot() * 1e9), False)
time.sleep(0.01)
self.assertEqual(fcw, parser.vl['ACC_HUD']['FCW'])
self.assertEqual(0x20, parser.vl['ACC_HUD']['SET_ME_X20'])
self.assertEqual(0x10, parser.vl['ACC_HUD']['SET_ME_X10'])
self.assertEqual(0x80, parser.vl['ACC_HUD']['SET_ME_X80'])
def test_ui(self):
self.longMessage = True
car_name = TOYOTA.RAV4
sendcan = messaging.pub_sock('sendcan')
params = CarInterface.get_params(car_name)
CI = CarInterface(params, CarController)
# Get parser
parser_signals = [
('BARRIERS', 'LKAS_HUD', -1),
('RIGHT_LINE', 'LKAS_HUD', 0),
('LEFT_LINE', 'LKAS_HUD', 0),
('SET_ME_X01', 'LKAS_HUD', 0),
('SET_ME_X01_2', 'LKAS_HUD', 0),
('LDA_ALERT', 'LKAS_HUD', -1),
('SET_ME_X0C', 'LKAS_HUD', 0),
('SET_ME_X2C', 'LKAS_HUD', 0),
('SET_ME_X38', 'LKAS_HUD', 0),
('SET_ME_X02', 'LKAS_HUD', 0),
]
parser = CANParser(CI.cp.dbc_name, parser_signals, [], 0, sendcan=True, tcp_addr="127.0.0.1")
time.sleep(0.2) # Slow joiner syndrome
VA = car.CarControl.HUDControl.VisualAlert
for left_lane in [True, False]:
for right_lane in [True, False]:
for steer in [True, False]:
control = car.CarControl.new_message()
control.enabled = True
hud = car.CarControl.HUDControl.new_message()
if steer:
hud.visualAlert = VA.steerRequired
hud.leftLaneVisible = left_lane
hud.rightLaneVisible = right_lane
control.hudControl = hud
CI.update(control)
for _ in range(200): # UI is only sent at 1Hz
can_sends = CI.apply(control)
sendcan.send(can_list_to_can_capnp(can_sends, msgtype='sendcan'))
for _ in range(5):
parser.update(int(sec_since_boot() * 1e9), False)
time.sleep(0.01)
self.assertEqual(0x0c, parser.vl['LKAS_HUD']['SET_ME_X0C'])
self.assertEqual(0x2c, parser.vl['LKAS_HUD']['SET_ME_X2C'])
self.assertEqual(0x38, parser.vl['LKAS_HUD']['SET_ME_X38'])
self.assertEqual(0x02, parser.vl['LKAS_HUD']['SET_ME_X02'])
self.assertEqual(0, parser.vl['LKAS_HUD']['BARRIERS'])
self.assertEqual(1 if right_lane else 2, parser.vl['LKAS_HUD']['RIGHT_LINE'])
self.assertEqual(1 if left_lane else 2, parser.vl['LKAS_HUD']['LEFT_LINE'])
self.assertEqual(1, parser.vl['LKAS_HUD']['SET_ME_X01'])
self.assertEqual(1, parser.vl['LKAS_HUD']['SET_ME_X01_2'])
self.assertEqual(steer, parser.vl['LKAS_HUD']['LDA_ALERT'])
def test_standstill_and_cancel(self):
self.longMessage = True
car_name = TOYOTA.RAV4
sendcan = messaging.pub_sock('sendcan')
params = CarInterface.get_params(car_name)
CI = CarInterface(params, CarController)
# Get parser
parser_signals = [
('RELEASE_STANDSTILL', 'ACC_CONTROL', 0),
('CANCEL_REQ', 'ACC_CONTROL', 0),
('SET_ME_X3', 'ACC_CONTROL', 0),
('SET_ME_1', 'ACC_CONTROL', 0),
]
parser = CANParser(CI.cp.dbc_name, parser_signals, [], 0, sendcan=True, tcp_addr="127.0.0.1")
time.sleep(0.2) # Slow joiner syndrome
control = car.CarControl.new_message()
control.enabled = True
CI.update(control)
CI.CS.pcm_acc_status = 8 # Active
CI.CS.standstill = True
can_sends = CI.apply(control)
sendcan.send(can_list_to_can_capnp(can_sends, msgtype='sendcan'))
for _ in range(5):
parser.update(int(sec_since_boot() * 1e9), False)
time.sleep(0.01)
self.assertEqual(0x3, parser.vl['ACC_CONTROL']['SET_ME_X3'])
self.assertEqual(1, parser.vl['ACC_CONTROL']['SET_ME_1'])
self.assertFalse(parser.vl['ACC_CONTROL']['RELEASE_STANDSTILL'])
self.assertFalse(parser.vl['ACC_CONTROL']['CANCEL_REQ'])
CI.CS.pcm_acc_status = 7 # Standstill
for _ in range(10):
can_sends = CI.apply(control)
sendcan.send(can_list_to_can_capnp(can_sends, msgtype='sendcan'))
for _ in range(5):
parser.update(int(sec_since_boot() * 1e9), False)
time.sleep(0.01)
self.assertTrue(parser.vl['ACC_CONTROL']['RELEASE_STANDSTILL'])
cruise = car.CarControl.CruiseControl.new_message()
cruise.cancel = True
control.cruiseControl = cruise
for _ in range(10):
can_sends = CI.apply(control)
sendcan.send(can_list_to_can_capnp(can_sends, msgtype='sendcan'))
for _ in range(5):
parser.update(int(sec_since_boot() * 1e9), False)
time.sleep(0.01)
self.assertTrue(parser.vl['ACC_CONTROL']['CANCEL_REQ'])