VW ID.4 ICAS1 Vehicle Control Analysis

MEB_Kennungsfolge = {
    # LWI_01 Steering Angle
    0x86: [0x86, 0x86, 0x86, 0x86, 0x86, 0x86, 0x86, 0x86, 0x86, 0x86, 0x86, 0x86, 0x86, 0x86, 0x86, 0x86],
    # LH_EPS_03 Electric Power Steering
    0x9F: [0xF5, 0xF5, 0xF5, 0xF5, 0xF5, 0xF5, 0xF5, 0xF5, 0xF5, 0xF5, 0xF5, 0xF5, 0xF5, 0xF5, 0xF5, 0xF5],
    # HCA_01 Heading Control Assist
    0x126: [0xDA, 0xDA, 0xDA, 0xDA, 0xDA, 0xDA, 0xDA, 0xDA, 0xDA, 0xDA, 0xDA, 0xDA, 0xDA, 0xDA, 0xDA, 0xDA],
    # GRA_ACC_01 Steering wheel controls for ACC
    0x12B: [0x6A, 0x38, 0xB4, 0x27, 0x22, 0xEF, 0xE1, 0xBB, 0xF8, 0x80, 0x84, 0x49, 0xC7, 0x9E, 0x1E, 0x2B]
}

def gen_crc_lookup_table_8(poly):
    crc_lut = [0] * 256
    for i in range(256):
        crc = i
        for j in range(8):
            if crc & 0x80:
                crc = (crc << 1) ^ poly
            else:
                crc <<= 1
        crc_lut[i] = crc & 0xFF
    return crc_lut

def volkswagen_mqb_checksum(address, data):
    global crc8_lut_8h2f
    crc = 0xFF  # CRC8 8H2F/AUTOSAR

    for i in range(1, len(data)):
        crc = crc ^ data[i]
        # print(hex(crc), hex(i))
        crc = crc8_lut_8h2f[crc]

    counter = data[1] & 0x0F
    if address in MEB_Kennungsfolge:
        crc ^= MEB_Kennungsfolge[address][counter]
    else:
        # No validation required
        crc ^= [0x00] * 16  # Return all 0s by default

    crc = crc8_lut_8h2f[crc]  # Standard CRC8 8H2F/AUTOSAR final XOR
    return crc ^ 0xFF
    
can_id = 0x126
data = bytearray(bytes(8))
crc8_lut_8h2f = gen_crc_lookup_table_8(0x2f)
crc = volkswagen_mqb_checksum(can_id, data)
data[0] = crc

0x184
1. Lock Vehicle
   1. Byte 2: Mirror Open (0x20 Open)
   2. Byte 3: Mirror Close (0x40 Close)
2. Windows
   1. Byte 6: Window Control - 04 Front Left Down, 02 Front Left Up, 40 Rear Left Down, 08 Front Right Up, 10 Front Right Down
   2. Byte 7: 01 Rear Right Down

0x185
1. Window Control (10, 20 Up; 40, 80 Down)
   1. Byte 1: Front Row
   2. Byte 2: Rear Row

0x598
1. Sunroof Control
   1. Byte 3: 0x20 None, 0x21 Tap, 0x22 Slide, 0x25 Long Press
   2. Byte 7: 04 Close Once, 08 Close Cont., 0C Open Once, 10 Open Cont., 14/1C Others

def create_pla_control(sendestatus, positive, degree):
    data = bytearray(bytes(24))
    data[1] = 0x40
    # PLA_QFK_Spuerb
    data[2] = 0xFA
    # PLA_QFK_KruemmSoll
    data = set_value(data, degree, 32, 15)
    # PLA_QFK_KruemmSoll_VZ
    if positive:
        data = set_value(data, 1, 47, 1)
    else:
        data = set_value(data, 0, 47, 1)
    
    # PLA_05_Sendestatus
    if sendestatus:
        data = set_value(data, 1, 74, 1)
    else:
        data = set_value(data, 0, 74, 1)
    return data

hca_01_counter = 0
degree = 0
for i in range(20):
    # data = create_steering_control(10 * i, 1)
    # meb_can_send(0x126, 0.1, data)
    degree += 0x10
    data = create_pla_control(1, 1, degree)
    can_send(0x302, 0.1, data)

import os
import can
import time

can0 = can.interface.Bus(channel = 'can0', bustype = 'socketcan', fd=True, bitrate=500000)

# msg = can.Message(is_extended_id=False, arbitration_id=0x123, data=[0, 1, 2, 3, 4, 5, 6, 7])
# can0.send(msg)

MEB_Kennungsfolge = {
    # LWI_01 Steering Angle
    0x86: [0x86, 0x86, 0x86, 0x86, 0x86, 0x86, 0x86, 0x86, 0x86, 0x86, 0x86, 0x86, 0x86, 0x86, 0x86, 0x86],
    # LH_EPS_03 Electric Power Steering
    0x9F: [0xF5, 0xF5, 0xF5, 0xF5, 0xF5, 0xF5, 0xF5, 0xF5, 0xF5, 0xF5, 0xF5, 0xF5, 0xF5, 0xF5, 0xF5, 0xF5],
    # HCA_01 Heading Control Assist
    0x126: [0xDA, 0xDA, 0xDA, 0xDA, 0xDA, 0xDA, 0xDA, 0xDA, 0xDA, 0xDA, 0xDA, 0xDA, 0xDA, 0xDA, 0xDA, 0xDA],
    # GRA_ACC_01 Steering wheel controls for ACC
    0x12B: [0x6A, 0x38, 0xB4, 0x27, 0x22, 0xEF, 0xE1, 0xBB, 0xF8, 0x80, 0x84, 0x49, 0xC7, 0x9E, 0x1E, 0x2B]
}

def volkswagen_mqb_checksum(address, data):
    global crc8_lut_8h2f
    crc = 0xFF  # CRC8 8H2F/AUTOSAR

    for i in range(1, len(data)):
        crc = crc ^ data[i]
        # print(hex(crc), hex(i))
        crc = crc8_lut_8h2f[crc]

    counter = data[1] & 0x0F
    if address in MEB_Kennungsfolge:
        crc ^= MEB_Kennungsfolge[address][counter]
    else:
        print("Attempt to CRC check undefined Volkswagen message 0x%02X" % address)
        crc ^= [0x00] * 16  # Return all 0s by default

    crc = crc8_lut_8h2f[crc]  # Standard CRC8 8H2F/AUTOSAR final XOR
    return crc ^ 0xFF

def gen_crc_lookup_table_8(poly):
    crc_lut = [0] * 256
    for i in range(256):
        crc = i
        for j in range(8):
            if crc & 0x80:
                crc = (crc << 1) ^ poly
            else:
                crc <<= 1
        crc_lut[i] = crc & 0xFF
    return crc_lut

def meb_can_send(can_id, interval, data):
    global hca_01_counter
    data[1] = data[1] | hca_01_counter
    crc = volkswagen_mqb_checksum(can_id, data)
    data[0] = crc
    hca_01_counter += 1
    for j in data:
        print(hex(j), end=", ")
    time.sleep(interval)
    msg = can.Message(is_extended_id=False, arbitration_id=can_id, data=data, is_fd=True)
    can0.send(msg)
    
def can_send(can_id, interval, data):
    time.sleep(interval)
    msg = can.Message(is_extended_id=False, arbitration_id=can_id, data=data, is_fd=True)
    can0.send(msg)

crc8_lut_8h2f = gen_crc_lookup_table_8(0x2f)

def set_value(data, value, start_pos, value_length):
    end_pos = start_pos + value_length
    for bit_index in range(start_pos, end_pos):
        byte_index = bit_index // 8
        bit_offset = bit_index % 8
        bit_value = (value >> (bit_index - start_pos)) & 1
        # print("\n")
        # print("index", bit_index, start_pos, end_pos, "byte_index:", byte_index, "bit_offset:", bit_offset, "v:", bit_value)
        if bit_value:
            data[byte_index] |= 1 << bit_offset
        else:
            data[byte_index] &= ~(1 << bit_offset)
    # for i in data:
    #     print(bin(i), end=",")
    return data

def set_bit(b, bit, bit_index):
    if bit_index < 0 or bit_index > 7:
        raise ValueError("bit_index must be between 0 and 7")
    mask = 1 << bit_index
    flipped_byte = b ^ mask
    return flipped_byte

def create_steering_control(apply_steer, lkas_enabled):
    data = bytearray(bytes(8))
    if lkas_enabled:
        # HCA_01_Sendestatus
        data = set_value(data, 5, 30, 1)
        # HCA_01_Status_HCA
        data = set_value(data, 1, 32, 4)
    else:
        # HCA_01_Sendestatus
        data = set_value(data, 3, 30, 1)
        # HCA_01_Status_HCA
        data = set_value(data, 0, 32, 4)
    # HCA_01_LM_Offset
    print("create_steering_control", apply_steer)
    data = set_value(data, abs(apply_steer), 16, 9)
    # HCA_01_LM_OffSign
    v = 1 if apply_steer < 0 else 0
    data = set_value(data, v, 31, 1)
    # HCA_01_Vib_Freq
    data = set_value(data, 3, 12, 4)
    # HCA_01_Vib_Amp
    data = set_value(data, 10, 36, 4)
    return data

def create_pla_control(sendestatus, positive, degree):
    data = bytearray(bytes(24))
    data[1] = 0x40
    # PLA_QFK_Spuerb
    data[2] = 0xFA
    # PLA_QFK_KruemmSoll
    data = set_value(data, degree, 32, 15)
    # PLA_QFK_KruemmSoll_VZ
    if positive:
        data = set_value(data, 1, 47, 1)
    else:
        data = set_value(data, 0, 47, 1)
    
    # PLA_05_Sendestatus
    if sendestatus:
        data = set_value(data, 1, 74, 1)
    else:
        data = set_value(data, 0, 74, 1)
    return data

hca_01_counter = 0
degree = 0
for i in range(20):
    # data = create_steering_control(10 * i, 1)
    # meb_can_send(0x126, 0.1, data)
    degree += 0x10
    data = create_pla_control(1, 1, degree)
    can_send(0x302, 0.1, data)