You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
108 lines
3.6 KiB
108 lines
3.6 KiB
def create_steering_control(packer, bus, apply_steer, lkas_enabled):
|
|
values = {
|
|
"SET_ME_0X3": 0x3,
|
|
"Assist_Torque": abs(apply_steer),
|
|
"Assist_Requested": lkas_enabled,
|
|
"Assist_VZ": 1 if apply_steer < 0 else 0,
|
|
"HCA_Available": 1,
|
|
"HCA_Standby": not lkas_enabled,
|
|
"HCA_Active": lkas_enabled,
|
|
"SET_ME_0XFE": 0xFE,
|
|
"SET_ME_0X07": 0x07,
|
|
}
|
|
return packer.make_can_msg("HCA_01", bus, values)
|
|
|
|
|
|
def create_lka_hud_control(packer, bus, ldw_stock_values, enabled, steering_pressed, hud_alert, hud_control):
|
|
values = ldw_stock_values.copy()
|
|
|
|
values.update({
|
|
"LDW_Status_LED_gelb": 1 if enabled and steering_pressed else 0,
|
|
"LDW_Status_LED_gruen": 1 if enabled and not steering_pressed else 0,
|
|
"LDW_Lernmodus_links": 3 if hud_control.leftLaneDepart else 1 + hud_control.leftLaneVisible,
|
|
"LDW_Lernmodus_rechts": 3 if hud_control.rightLaneDepart else 1 + hud_control.rightLaneVisible,
|
|
"LDW_Texte": hud_alert,
|
|
})
|
|
return packer.make_can_msg("LDW_02", bus, values)
|
|
|
|
|
|
def create_acc_buttons_control(packer, bus, gra_stock_values, counter, cancel=False, resume=False):
|
|
values = gra_stock_values.copy()
|
|
|
|
values.update({
|
|
"COUNTER": counter,
|
|
"GRA_Abbrechen": cancel,
|
|
"GRA_Tip_Wiederaufnahme": resume,
|
|
})
|
|
|
|
return packer.make_can_msg("GRA_ACC_01", bus, values)
|
|
|
|
|
|
def acc_control_value(main_switch_on, acc_faulted, long_active):
|
|
if acc_faulted:
|
|
acc_control = 6
|
|
elif long_active:
|
|
acc_control = 3
|
|
elif main_switch_on:
|
|
acc_control = 2
|
|
else:
|
|
acc_control = 0
|
|
|
|
return acc_control
|
|
|
|
|
|
def acc_hud_status_value(main_switch_on, acc_faulted, long_active):
|
|
# TODO: happens to resemble the ACC control value for now, but extend this for init/gas override later
|
|
return acc_control_value(main_switch_on, acc_faulted, long_active)
|
|
|
|
|
|
def create_acc_accel_control(packer, bus, acc_type, enabled, accel, acc_control, stopping, starting, esp_hold):
|
|
commands = []
|
|
|
|
acc_06_values = {
|
|
"ACC_Typ": acc_type,
|
|
"ACC_Status_ACC": acc_control,
|
|
"ACC_StartStopp_Info": enabled,
|
|
"ACC_Sollbeschleunigung_02": accel if enabled else 3.01,
|
|
"ACC_zul_Regelabw_unten": 0.2, # TODO: dynamic adjustment of comfort-band
|
|
"ACC_zul_Regelabw_oben": 0.2, # TODO: dynamic adjustment of comfort-band
|
|
"ACC_neg_Sollbeschl_Grad_02": 4.0 if enabled else 0, # TODO: dynamic adjustment of jerk limits
|
|
"ACC_pos_Sollbeschl_Grad_02": 4.0 if enabled else 0, # TODO: dynamic adjustment of jerk limits
|
|
"ACC_Anfahren": starting,
|
|
"ACC_Anhalten": stopping,
|
|
}
|
|
commands.append(packer.make_can_msg("ACC_06", bus, acc_06_values))
|
|
|
|
if starting:
|
|
acc_hold_type = 4 # hold release / startup
|
|
elif esp_hold:
|
|
acc_hold_type = 3 # hold standby
|
|
elif stopping:
|
|
acc_hold_type = 1 # hold request
|
|
else:
|
|
acc_hold_type = 0
|
|
|
|
acc_07_values = {
|
|
"ACC_Anhalteweg": 0.75 if stopping else 20.46, # Distance to stop (stopping coordinator handles terminal roll-out)
|
|
"ACC_Freilauf_Info": 2 if enabled else 0,
|
|
"ACC_Folgebeschl": 3.02, # Not using secondary controller accel unless and until we understand its impact
|
|
"ACC_Sollbeschleunigung_02": accel if enabled else 3.01,
|
|
"ACC_Anforderung_HMS": acc_hold_type,
|
|
"ACC_Anfahren": starting,
|
|
"ACC_Anhalten": stopping,
|
|
}
|
|
commands.append(packer.make_can_msg("ACC_07", bus, acc_07_values))
|
|
|
|
return commands
|
|
|
|
|
|
def create_acc_hud_control(packer, bus, acc_hud_status, set_speed, lead_distance):
|
|
values = {
|
|
"ACC_Status_Anzeige": acc_hud_status,
|
|
"ACC_Wunschgeschw_02": set_speed if set_speed < 250 else 327.36,
|
|
"ACC_Gesetzte_Zeitluecke": 3,
|
|
"ACC_Display_Prio": 3,
|
|
"ACC_Abstandsindex": lead_distance,
|
|
}
|
|
|
|
return packer.make_can_msg("ACC_02", bus, values)
|
|
|