hil2.commands

  1from typing import Optional
  2
  3import time
  4import logging
  5
  6import cantools.database.can.database as cantools_db
  7import serial
  8
  9from . import can_helper
 10from . import hil_errors
 11from . import serial_helper
 12
 13
 14# Command constants -------------------------------------------------------------------#
 15# fmt: off
 16READ_ID    = 0  # command                    -> SYNC_BYTES (4), READ_ID, id
 17WRITE_GPIO = 1  # command, pin, value        -> []
 18HIZ_GPIO   = 2  # command, pin               -> []
 19READ_GPIO  = 3  # command, pin               -> READ_GPIO, value
 20WRITE_DAC  = 4  # command, pin/offset, value -> []
 21HIZ_DAC    = 5  # command, pin/offset        -> []
 22READ_ADC   = 6  # command, pin               -> READ_ADC, value high, value low
 23WRITE_POT  = 7  # command, pin/offset, value -> []
 24SEND_CAN   = 8  # command, bus, signal bytes: 3-0, length, data (8 bytes) -> []
 25RECV_CAN   = 9  # <async>                    -> RECV_CAN, bus, signal bytes: 3-0,
 26                #                               length, data (length bytes)
 27ERROR      = 10 # <async/any>                -> ERROR, command
 28# fmt: on
 29
 30READ_ID_RESPONSE_LENGTH = 6  # SYNC_BYTES (4) + READ_ID (1) + ID (1)
 31SYNC_BYTES = [0xDE, 0xAD, 0xBE, 0xEF]
 32
 33SERIAL_RESPONSES = [READ_ID, READ_GPIO, READ_ADC, RECV_CAN, ERROR]
 34
 35
 36# Simple commands ---------------------------------------------------------------------#
 37def read_id(
 38    ser_raw: serial.Serial, response_wait: float
 39) -> Optional[tuple[int, list[int]]]:
 40    """
 41    Attempts to connect and read the HIL ID from a device.
 42    Sends a READ_ID command and waits for a response.
 43
 44    :param ser_raw: The raw serial connection to use (raw Serial object).
 45    :return: The HIL ID and any bytes received after the ID, or None if not a HIL device.
 46    """
 47    command = [READ_ID]
 48    logging.debug(f"Sending - READ_ID: {command}")
 49    ser_raw.reset_input_buffer()
 50    ser_raw.write(bytearray(command))
 51    read_buffer = []
 52
 53    try:
 54        start_time = time.time()
 55
 56        while time.time() - start_time < response_wait:
 57            to_read = max(ser_raw.in_waiting, 1)
 58            chunk = ser_raw.read(to_read)
 59            if not chunk:
 60                continue
 61            read_buffer.extend([int(b) for b in chunk])
 62            logging.debug(f"Read buffer: {read_buffer}")
 63
 64            if len(read_buffer) >= READ_ID_RESPONSE_LENGTH:
 65                # Look for SYNC_BYTES in the buffer
 66                for i in range(len(read_buffer) - 5):
 67                    if read_buffer[i : i + 4] == SYNC_BYTES:
 68                        logging.debug(f"SYNC_BYTES found at position {i}")
 69                        # Check if the next byte is READ_ID
 70                        if read_buffer[i + 4] == READ_ID:
 71                            read_hil_id = read_buffer[i + 5]
 72                            remaining_bytes = read_buffer[i + 6 :]
 73                            logging.debug(
 74                                f"Received - READ_ID: {read_hil_id},"
 75                                f"remaining bytes: {len(remaining_bytes)}"
 76                            )
 77                            return read_hil_id, remaining_bytes
 78                # If SYNC_BYTES not found, remove bytes before the last 3 bytes
 79                read_buffer = read_buffer[-3:]
 80    except serial.SerialException as e:
 81        logging.error(f"Serial exception occurred: {e}")
 82        return None
 83    
 84    return None
 85
 86
 87def write_gpio(ser: serial_helper.ThreadedSerial, pin: int, value: bool) -> None:
 88    """
 89    Writes a GPIO value to a device.
 90    Sends a WRITE_GPIO command with the specified pin and value.
 91
 92    :param ser: The serial connection to use.
 93    :param pin: The GPIO pin number.
 94    :param value: The value to write (low = false, high = true).
 95    """
 96    command = [WRITE_GPIO, pin, int(value)]
 97    logging.debug(f"Sending - WRITE_GPIO: {command}")
 98    ser.write(bytearray(command))
 99
100
101def hiZ_gpio(ser: serial_helper.ThreadedSerial, pin: int) -> None:
102    """
103    Set a GPIO pin to high impedance (HiZ).
104    (This is equivalent to setting the pin as an input.)
105    Sends a HIZ_GPIO command with the specified pin.
106
107    :param ser: The serial connection to use.
108    :param pin: The GPIO pin number.
109    """
110    command = [HIZ_GPIO, pin]
111    logging.debug(f"Sending - HIZ_GPIO: {command}")
112    ser.write(bytearray(command))
113
114
115def read_gpio(ser: serial_helper.ThreadedSerial, pin: int) -> bool:
116    """
117    Reads a GPIO value from a device.
118    Sends a READ_GPIO command with the specified pin and waits for a response.
119
120    :param ser: The serial connection to use.
121    :param pin: The GPIO pin number.
122    :return: The value read from the GPIO pin (low = false, high = true).
123    """
124
125    command = [READ_GPIO, pin]
126    logging.debug(f"Sending - READ_GPIO: {command}")
127    ser.write(bytearray(command))
128    match ser.get_readings_with_timeout(READ_GPIO):
129        case None:
130            raise hil_errors.SerialError("Failed to read GPIO value, no response")
131        case [read_value]:
132            logging.debug(f"Received - READ_GPIO: {read_value}")
133            return bool(read_value)
134        case x:
135            error_msg = f"Failed to read GPIO value, expected 1 byte: {x}"
136            raise hil_errors.EngineError(error_msg)
137
138
139def write_dac(ser: serial_helper.ThreadedSerial, pin: int, raw_value: int) -> None:
140    """
141    Writes a DAC value to a device.
142    Sends a WRITE_DAC command with the specified pin and raw value.
143
144    :param ser: The serial connection to use.
145    :param pin: The DAC pin number.
146    :param raw_value: The raw value to write (0-255).
147    """
148    command = [WRITE_DAC, pin, raw_value]
149    logging.debug(f"Sending - WRITE_DAC: {command}")
150    ser.write(bytearray(command))
151
152
153def hiZ_dac(ser: serial_helper.ThreadedSerial, pin: int) -> None:
154    """
155    Sets a DAC pin to high impedance mode.
156    Sends a HIZ_DAC command with the specified pin.
157
158    :param ser: The serial connection to use.
159    :param pin: The DAC pin number.
160    """
161    command = [HIZ_DAC, pin]
162    logging.debug(f"Sending - HIZ_DAC: {command}")
163    ser.write(bytearray(command))
164
165
166def read_adc(ser: serial_helper.ThreadedSerial, pin: int) -> int:
167    """
168    Reads an ADC value from a device.
169    Sends a READ_ADC command with the specified pin and waits for a response.
170
171    :param ser: The serial connection to use.
172    :param pin: The ADC pin number.
173    :return: The raw ADC value read from the specified pin.
174    """
175
176    command = [READ_ADC, pin]
177    logging.debug(f"Sending - READ_ADC: {command}")
178    ser.write(bytearray(command))
179    match ser.get_readings_with_timeout(READ_ADC):
180        case None:
181            raise hil_errors.SerialError("Failed to read ADC value, no response")
182        case [read_value_high, read_value_low]:
183            logging.debug(f"Received - READ_ADC: {read_value_high}, {read_value_low}")
184            return (read_value_high << 8) | read_value_low
185        case x:
186            error_msg = f"Failed to read ADC value, expected 2 bytes: {x}"
187            raise hil_errors.EngineError(error_msg)
188
189
190def write_pot(ser: serial_helper.ThreadedSerial, pin: int, raw_value: int) -> None:
191    """
192    Writes a potentiometer value to a device.
193    Sends a WRITE_POT command with the specified pin and raw value.
194
195    :param ser: The serial connection to use.
196    :param pin: The potentiometer pin number.
197    :param raw_value: The raw value to write (0-255).
198    """
199    command = [WRITE_POT, pin, raw_value]
200    logging.debug(f"Sending - WRITE_POT: {command}")
201    ser.write(bytearray(command))
202
203
204# CAN commands/parsing ----------------------------------------------------------------#
205def send_can(
206    ser: serial_helper.ThreadedSerial,
207    bus: int,
208    signal: int,
209    data: list[int],
210) -> None:
211    """
212    Sends a CAN message over the specified bus.
213
214    :param ser: The serial connection to use.
215    :param bus: The CAN bus number.
216    :param signal: The CAN signal ID.
217    :param data: The data to send (up to 8 bytes). When sent, will be padded with zeros.
218    """
219    signal_3 = (signal >> 24) & 0xFF
220    signal_2 = (signal >> 16) & 0xFF
221    signal_1 = (signal >> 8) & 0xFF
222    signal_0 = signal & 0xFF
223    length = len(data)
224    padding = [0] * (8 - len(data))
225    command = [
226        SEND_CAN,
227        bus,
228        signal_3,
229        signal_2,
230        signal_1,
231        signal_0,
232        length,
233        *data,
234        *padding,
235    ]
236    logging.debug(f"Sending - SEND_CAN: {command}")
237    print(f"Sending - SEND_CAN: {command}")
238    ser.write(bytearray(command))
239
240
241def parse_can_messages(
242    ser: serial_helper.ThreadedSerial, bus: int, can_dbc: cantools_db.Database
243) -> list[can_helper.CanMessage]:
244    """
245    Parses received CAN messages from the serial connection for the specified bus.
246
247    :param ser: The serial connection to use.
248    :param bus: The CAN bus number.
249    :param can_dbc: The DBC database to use for decoding messages.
250    :return: A list of parsed CAN messages.
251    """
252
253    def decode(values):
254        signal = (
255            (values[1] << 24) | (values[2] << 16) | (values[3] << 8) | values[4]
256        ) & 0x1FFFFFFF
257        data = bytes(values[6 : 6 + values[5]])
258        try:
259            decoded = can_dbc.decode_message(signal, data)
260            return can_helper.CanMessage(signal, decoded)
261        except Exception as e:
262            logging.error(f"Failed to decode CAN message with ID {signal} ({e})")
263            return None
264
265    return [
266        msg
267        for values in ser.get_parsed_can_messages(bus)
268        if (msg := decode(values)) is not None
269    ]
270
271
272# Serial parsing/spliting -------------------------------------------------------------#
273def parse_readings(
274    readings: list[int],
275    parsed_readings: dict[int, list[int]],
276    parsed_can_messages: dict[int, list[list[int]]],
277) -> tuple[bool, list[int]]:
278    """
279    Parse the first serial reading if possible.
280    Does not do conversion, just separates and saves the raw bytes for the corresponding
281    reading.
282
283    :param readings: The entire list of readings (bytes received from serial) to parse
284    :param parsed_readings: The dictionary to store parsed readings.
285    :param parsed_can_messages: The dictionary to store parsed CAN messages.
286    :return: A tuple:
287             - A boolean indicating if anything was parsed (and maybe this function
288               should be called again)
289             - The remaining unparsed readings.
290    """
291
292    logging.debug(f"Current readings to parse: {readings}")
293    match readings:
294        case []:
295            return False, []
296        case [cmd, value, *rest] if cmd == READ_ID:
297            logging.debug(f"Parsed - READ_ID: {value}")
298            parsed_readings[READ_ID] = [value]
299            return True, rest
300        case [cmd, value, *rest] if cmd == READ_GPIO:
301            logging.debug(f"Parsed - READ_GPIO: {value}")
302            parsed_readings[READ_GPIO] = [value]
303            return True, rest
304        case [cmd, value_high, value_low, *rest] if cmd == READ_ADC:
305            logging.debug(f"Parsed - READ_ADC: {value_high}, {value_low}")
306            parsed_readings[READ_ADC] = [value_high, value_low]
307            return True, rest
308        case [cmd, bus, signal_3, signal_2, signal_1, signal_0, length, *rest] if (
309            cmd == RECV_CAN and len(rest) >= length
310        ):
311            logging.debug(
312                f"Parsed - RECV_CAN: {bus}, {signal_3}, {signal_2}, {signal_1}, "
313                + f"{signal_0}, {length}, {rest[:length]}"
314            )
315            data, remaining = rest[:length], rest[length:]
316            if bus not in parsed_can_messages:
317                parsed_can_messages[bus] = []
318            parsed_can_messages[bus].append(
319                [bus, signal_3, signal_2, signal_1, signal_0, length, *data]
320            )
321            return True, remaining
322        case [cmd, command, *rest] if cmd == ERROR:
323            logging.critical(f"Parsed - ERROR for command: {command}. Rest={rest}")
324            raise hil_errors.SerialError(f"HIL reported error for command {command}")
325        case [first, *rest] if first not in SERIAL_RESPONSES:
326            logging.critical(f"Unexpected response: {first}. Rest={rest}")
327            raise hil_errors.SerialError(f"Unexpected response. Command error: {first}")
328        case _:
329            return False, readings