diff --git a/.gitignore b/.gitignore index 7ea06c9..779b256 100644 --- a/.gitignore +++ b/.gitignore @@ -4,3 +4,5 @@ __pycache__/ result result/ .env +hytech.proto +dbc_to.proto diff --git a/README.md b/README.md index b9cad68..5f5a451 100644 --- a/README.md +++ b/README.md @@ -1,4 +1,11 @@ -usage: +# Base person usage: +1. Download foxglove studio at https://github.com/foxglove/studio/releases +2. Connect to the Pi's network +3. Open foxglove studio +4. Open a connection to the Pi's IP on port 8765 +5. Look at data + +# dev usage: to get into a dev environment locally (on linux): 1. install nix: @@ -117,4 +124,4 @@ flowchart TD kvaser u100 pinout: -![Alt text](image.png) \ No newline at end of file +![Alt text](image.png) diff --git a/dbc_proto_bin_gen.nix b/dbc_proto_bin_gen.nix index dc5bed4..264bc1d 100644 --- a/dbc_proto_bin_gen.nix +++ b/dbc_proto_bin_gen.nix @@ -1,16 +1,16 @@ -{pkgs, py_dbc_proto_gen_pkg, ht_can_pkg, protobuf}: +{pkgs, py_dbc_proto_gen_pkg, can_pkg, protobuf}: pkgs.stdenv.mkDerivation rec { name = "ht-proto-gen"; src = builtins.filterSource (path: type: false) ./.; - buildInputs = [ py_dbc_proto_gen_pkg ht_can_pkg protobuf ]; # Python as a build dependency + buildInputs = [ py_dbc_proto_gen_pkg can_pkg protobuf ]; # Python as a build dependency # Define the build phase to execute the scripts buildPhase = '' # Run the Python script - dbc_to_proto.py ${ht_can_pkg} + dbc_to_proto.py ${can_pkg} protoc --include_imports --descriptor_set_out=hytech.bin hytech.proto ''; @@ -22,4 +22,4 @@ pkgs.stdenv.mkDerivation rec { cp hytech.proto $out/proto cp hytech.bin $out/bin ''; -} \ No newline at end of file +} diff --git a/default.nix b/default.nix index 0dd5e4d..09f159d 100644 --- a/default.nix +++ b/default.nix @@ -18,6 +18,7 @@ python311Packages.buildPythonApplication { python311Packages.websockets python311Packages.pprintpp python311Packages.can + python311Packages.pyserial-asyncio asyncudp_pkg python311Packages.lz4 python311Packages.zstandard diff --git a/downv.sh b/downv.sh new file mode 100755 index 0000000..036c82f --- /dev/null +++ b/downv.sh @@ -0,0 +1,13 @@ +#!/bin/bash + +# Check if vcan0 is already down +if ! ip link show up | grep -q vcan0; then + echo "vcan0 is already down." +else + echo "Bringing down vcan0..." + + # Bring down can0 interface + sudo ip link set down vcan0 + + echo "vcan0 has been brought down." +fi diff --git a/flake.lock b/flake.lock index cea78ad..5e40584 100644 --- a/flake.lock +++ b/flake.lock @@ -19,41 +19,41 @@ "type": "github" } }, - "foxglove-websocket": { + "can_pkg_flake": { "inputs": { "nixpkgs": "nixpkgs_2", "utils": "utils_2" }, "locked": { - "lastModified": 1703087404, - "narHash": "sha256-PJB1pXZUrt8Za4PxXyIZtRffq5FuDS4do8lQPpY4iMY=", - "owner": "RCMast3r", - "repo": "py_foxglove_webserver_nix", - "rev": "bbab26c716248fad51447b4ab8f4b84b75e3677a", + "lastModified": 1712715991, + "narHash": "sha256-5eZWtJPBq/to3uVkmmhp6KcWcbaTMY4ihV8cFRJ7BfI=", + "owner": "KSU-MS", + "repo": "Nix_CAN", + "rev": "f7b31f081ba3a8ddff0aa0eee0fbd207a9860a7b", "type": "github" }, "original": { - "owner": "RCMast3r", - "repo": "py_foxglove_webserver_nix", + "owner": "KSU-MS", + "repo": "Nix_CAN", "type": "github" } }, - "ht_can_pkg_flake": { + "foxglove-websocket": { "inputs": { "nixpkgs": "nixpkgs_3", "utils": "utils_3" }, "locked": { - "lastModified": 1708295505, - "narHash": "sha256-djxRSAyr+9N3AO7uP4lYg1wW/X8a85D3XADFM3Rreeo=", - "owner": "hytech-racing", - "repo": "ht_can", - "rev": "a52db761d58f1caa163c44ca197ec19777a6b2f5", + "lastModified": 1711401255, + "narHash": "sha256-V9VX0LOHqXKoBuIY6gIsiU1WDDGZTGmOQULQ0YUur8s=", + "owner": "RCMast3r", + "repo": "py_foxglove_webserver_nix", + "rev": "61b43a707534b3ea41fed1ccc9369d893178a7f4", "type": "github" }, "original": { - "owner": "hytech-racing", - "repo": "ht_can", + "owner": "RCMast3r", + "repo": "py_foxglove_webserver_nix", "type": "github" } }, @@ -117,11 +117,11 @@ "nixpkgs": "nixpkgs_6" }, "locked": { - "lastModified": 1703652955, - "narHash": "sha256-9kCfd8qbkRwflWRuuhs8hJM/2b+ojmBTmULvCiCl0Hg=", + "lastModified": 1709706423, + "narHash": "sha256-uL/H0zh/Ckz1o5vWdd2DiKSu2QAPlYDNiOzpSymbPao=", "owner": "notalltim", "repo": "nix-proto", - "rev": "a067a064aee705e75f38163c869a6a77456278fe", + "rev": "2ccac0a3f70bb84a485db60f1a7ac7688c992d92", "type": "github" }, "original": { @@ -163,11 +163,11 @@ }, "nixpkgs_2": { "locked": { - "lastModified": 1702921762, - "narHash": "sha256-O/rP7gulApQAB47u6szEd8Pn8Biw0d84j5iuP2tcxzY=", + "lastModified": 1708294118, + "narHash": "sha256-evZzmLW7qoHXf76VCepvun1esZDxHfVRFUJtumD7L2M=", "owner": "NixOS", "repo": "nixpkgs", - "rev": "d02ffbbe834b5599fc5f134e644e49397eb07188", + "rev": "e0da498ad77ac8909a980f07eff060862417ccf7", "type": "github" }, "original": { @@ -179,11 +179,11 @@ }, "nixpkgs_3": { "locked": { - "lastModified": 1707650010, - "narHash": "sha256-dOhphIA4MGrH4ElNCy/OlwmN24MsnEqFjRR6+RY7jZw=", + "lastModified": 1702921762, + "narHash": "sha256-O/rP7gulApQAB47u6szEd8Pn8Biw0d84j5iuP2tcxzY=", "owner": "NixOS", "repo": "nixpkgs", - "rev": "809cca784b9f72a5ad4b991e0e7bcf8890f9c3a6", + "rev": "d02ffbbe834b5599fc5f134e644e49397eb07188", "type": "github" }, "original": { @@ -243,11 +243,11 @@ }, "nixpkgs_7": { "locked": { - "lastModified": 1702921762, - "narHash": "sha256-O/rP7gulApQAB47u6szEd8Pn8Biw0d84j5iuP2tcxzY=", + "lastModified": 1712588820, + "narHash": "sha256-y31s5idk3jMJMAVE4Ud9AdI7HT3CgTAeMTJ0StqKN7Y=", "owner": "NixOS", "repo": "nixpkgs", - "rev": "d02ffbbe834b5599fc5f134e644e49397eb07188", + "rev": "d272ca50d1f7424fbfcd1e6f1c9e01d92f6da167", "type": "github" }, "original": { @@ -260,8 +260,8 @@ "root": { "inputs": { "asyncudp": "asyncudp", + "can_pkg_flake": "can_pkg_flake", "foxglove-websocket": "foxglove-websocket", - "ht_can_pkg_flake": "ht_can_pkg_flake", "mcap": "mcap", "mcap-protobuf": "mcap-protobuf", "nix-proto": "nix-proto", @@ -382,11 +382,11 @@ "systems": "systems_2" }, "locked": { - "lastModified": 1701680307, - "narHash": "sha256-kAuep2h5ajznlPMD9rnQyffWG8EM/C73lejGofXvdM8=", + "lastModified": 1705309234, + "narHash": "sha256-uNRRNRKmJyCRC/8y1RqBkqWBLM034y4qN7EprSdmgyA=", "owner": "numtide", "repo": "flake-utils", - "rev": "4022d587cbbfd70fe950c1e2083a02621806a725", + "rev": "1ef2e671c3b0c19053962c07dbda38332dcebf26", "type": "github" }, "original": { @@ -400,11 +400,11 @@ "systems": "systems_3" }, "locked": { - "lastModified": 1705309234, - "narHash": "sha256-uNRRNRKmJyCRC/8y1RqBkqWBLM034y4qN7EprSdmgyA=", + "lastModified": 1701680307, + "narHash": "sha256-kAuep2h5ajznlPMD9rnQyffWG8EM/C73lejGofXvdM8=", "owner": "numtide", "repo": "flake-utils", - "rev": "1ef2e671c3b0c19053962c07dbda38332dcebf26", + "rev": "4022d587cbbfd70fe950c1e2083a02621806a725", "type": "github" }, "original": { @@ -454,11 +454,11 @@ "systems": "systems_6" }, "locked": { - "lastModified": 1701680307, - "narHash": "sha256-kAuep2h5ajznlPMD9rnQyffWG8EM/C73lejGofXvdM8=", + "lastModified": 1710146030, + "narHash": "sha256-SZ5L6eA7HJ/nmkzGG7/ISclqe6oZdOZTNoesiInkXPQ=", "owner": "numtide", "repo": "flake-utils", - "rev": "4022d587cbbfd70fe950c1e2083a02621806a725", + "rev": "b1d9ab70662946ef0850d488da1c9019f3a9752a", "type": "github" }, "original": { diff --git a/flake.nix b/flake.nix index 45c4d4b..57030c5 100644 --- a/flake.nix +++ b/flake.nix @@ -9,12 +9,12 @@ mcap.url = "github:RCMast3r/py_mcap_nix"; foxglove-websocket.url = "github:RCMast3r/py_foxglove_webserver_nix"; asyncudp.url = "github:RCMast3r/asyncudp_nix"; - ht_can_pkg_flake.url = "github:hytech-racing/ht_can"; - nix-proto = { url = "github:notalltim/nix-proto"; }; + can_pkg_flake.url = "github:KSU-MS/Nix_CAN"; + nix-proto.url = "github:notalltim/nix-proto"; }; outputs = { self, nixpkgs, utils, mcap-protobuf, mcap, foxglove-websocket - , asyncudp, nix-proto, ht_can_pkg_flake, ... }@inputs: + , asyncudp, nix-proto, can_pkg_flake, ... }@inputs: let makePackageSet = pkgs: { py_data_acq_pkg = pkgs.py_data_acq_pkg; @@ -48,7 +48,7 @@ py_dbc_proto_gen_overlay py_data_acq_overlay proto_gen_overlay - ht_can_pkg_flake.overlays.default + can_pkg_flake.overlays.default mcap-protobuf.overlays.default mcap.overlays.default asyncudp.overlays.default @@ -90,7 +90,7 @@ py_data_acq_pkg py_dbc_proto_gen_pkg proto_gen_pkg - ht_can_pkg + can_pkg cmake can-utils ]; @@ -100,7 +100,7 @@ in '' path=${x86_pkgs.proto_gen_pkg} bin_path=$path"/bin" - dbc_path=${x86_pkgs.ht_can_pkg} + dbc_path=${x86_pkgs.can_pkg} export BIN_PATH=$bin_path export DBC_PATH=$dbc_path @@ -115,14 +115,14 @@ # Development Tools py_dbc_proto_gen_pkg proto_gen_pkg - ht_can_pkg + can_pkg protobuf ]; shellHook = '' path=${x86_pkgs.proto_gen_pkg} bin_path=$path"/bin" - dbc_path=${x86_pkgs.ht_can_pkg} + dbc_path=${x86_pkgs.can_pkg} export BIN_PATH=$bin_path export DBC_PATH=$dbc_path ''; diff --git a/py_data_acq/broadcast-test.py b/py_data_acq/broadcast-test.py deleted file mode 100644 index f40fb8e..0000000 --- a/py_data_acq/broadcast-test.py +++ /dev/null @@ -1,42 +0,0 @@ -#!/usr/bin/env python -import socket -import time -import can -from can.interfaces.udp_multicast import UdpMulticastBus -import cantools -from pprint import pprint -import os - -from hytech_np_proto_py import hytech_pb2 - -# Define the IP and port for the UDP socket -# bus1 = can.interface.Bus('can0', bustype='virtual') -bus1 = can.Bus(channel="can0", interface='socketcan') -def main(): - path_to_dbc = os.environ.get('DBC_PATH') - full_path = os.path.join(path_to_dbc, "hytech.dbc") - # Serialize the message to bytes - db = cantools.database.load_file(full_path) - - msg = db.get_message_by_name("MC1_TORQUE_COMMAND") - rpm = db.get_message_by_name("MC4_SETPOINTS_COMMAND") - data = msg.encode({'torque_command': 100}) - - msg = can.Message(arbitration_id=msg.frame_id, is_extended_id=False, data=data) - - rpm_set = 100 - while(1): - try: - rpm_set= rpm_set+1 - bus1.send(msg) - rpm_data = rpm.encode({'negative_torque_limit': 1, 'positive_torque_limit': 1, 'speed_setpoint_rpm': rpm_set, 'remove_error': 1, 'driver_enable': 1, 'hv_enable': 1, 'inverter_enable': 1}) - rpm_msg = can.Message(arbitration_id=rpm.frame_id, is_extended_id=False, data=rpm_data) - bus1.send(rpm_msg) - - print("Message sent on {}".format(bus1.channel_info)) - except can.CanError: - print("Message NOT sent! Please verify can0 is working first") - time.sleep(0.1) - -if __name__ == "__main__": - main() diff --git a/py_data_acq/can-broadcast-test.py b/py_data_acq/can-broadcast-test.py new file mode 100644 index 0000000..8fc87cb --- /dev/null +++ b/py_data_acq/can-broadcast-test.py @@ -0,0 +1,53 @@ +#!/usr/bin/env python +import time +import can +import math +import os +from cantools import database + +# Define the bus to yap on +bus1 = can.Bus(channel="vcan0", interface="socketcan") + + +def generate_sine_wave(amplitude, frequency, phase_shift, time_variable): + return amplitude * math.sin(2 * math.pi * frequency * time_variable + phase_shift) + + +def main(): + # Load DBC into + path_to_dbc = os.environ.get("DBC_PATH") + full_path = os.path.join(path_to_dbc, "car.dbc") + db = database.load_file(full_path) + + # Setup fake message + rpm = db.get_message_by_name("M165_Motor_Position_Info") + + rpm_set = 100 + while 1: + try: + # Iterate example vals + rpm_set = rpm_set + 1 + + # Serialize the message to bytes + rpm_set = generate_sine_wave(3000, 1, 90, time.time()) + 3000 + rpm_data = rpm.encode( + { + "D4_Delta_Resolver_Filtered": int(1), + "D3_Electrical_Output_Frequency": int(1), + "D2_Motor_Speed": rpm_set, + "D1_Motor_Angle_Electrical": int(1), + } + ) + rpm_msg = can.Message( + arbitration_id=rpm.frame_id, is_extended_id=False, data=rpm_data + ) + bus1.send(rpm_msg) + + # print("Message sent on {}".format(bus1.channel_info)) + except can.CanError: + print("Message NOT sent! Please verify can0 is working first") + time.sleep(0.1) + + +if __name__ == "__main__": + main() diff --git a/py_data_acq/py_data_acq/common/protobuf_helpers.py b/py_data_acq/py_data_acq/common/protobuf_helpers.py index 6ebf5af..d6e4f29 100644 --- a/py_data_acq/py_data_acq/common/protobuf_helpers.py +++ b/py_data_acq/py_data_acq/common/protobuf_helpers.py @@ -24,8 +24,16 @@ def pack_protobuf_msg(cantools_dict: dict, msg_name: str, message_classes): if msg_name in message_classes: pb_msg = message_classes[msg_name]() for key in cantools_dict.keys(): - if(type(cantools_dict[key]) is namedsignalvalue.NamedSignalValue): - setattr(pb_msg, key, cantools_dict[key].value) - else: + try: setattr(pb_msg, key, cantools_dict[key]) + except TypeError as e: + print(f"Caught TypeError: {e}") + expected_type = type(getattr(pb_msg, key)) + + try: + converted_value = expected_type(cantools_dict[key]) + setattr(pb_msg, key, converted_value) + print(f"Successfully set {key} to {converted_value}") + except ValueError: + print(f"Unable to convert {cantools_dict[key]} to {expected_type.__name__}") return pb_msg diff --git a/py_data_acq/py_data_acq/io_handler/can_handle.py b/py_data_acq/py_data_acq/io_handler/can_handle.py index e69de29..6ec3ac1 100644 --- a/py_data_acq/py_data_acq/io_handler/can_handle.py +++ b/py_data_acq/py_data_acq/io_handler/can_handle.py @@ -0,0 +1,75 @@ +import os +import can +import asyncio +import cantools +from ..common import protobuf_helpers +from ..common.common_types import QueueData +from can.interfaces.udp_multicast import UdpMulticastBus + +can_methods = { + "debug": [UdpMulticastBus.DEFAULT_GROUP_IPv6, "udp_multicast"], + "local_can_usb_KV": [0, "kvaser"], + "local_debug": ["vcan0", "socketcan"], +} + + +def find_can_interface(): + """Find a CAN interface by checking /sys/class/net/.""" + for interface in os.listdir("/sys/class/net/"): + if interface.startswith("can"): + return interface + return None + + +def init_can(): + can_interface = find_can_interface() + + if can_interface: + print(f"Found CAN interface: {can_interface}") + try: + # Attempt to initialize the CAN bus + bus = can.Bus(channel=can_interface, bustype="socketcan") + print(f"Successfully initialized CAN bus on {can_interface}") + # Interface exists and bus is initialized, but this doesn't ensure the interface is 'up' + except can.CanError as e: + print(f"Failed to initialize CAN bus on {can_interface}: {e}") + print("defaulting to using virtual can interface vcan0") + bus = can.Bus( + interface="socketcan", channel="vcan0", receive_own_messages=True + ) + else: + print("defaulting to using virtual can interface vcan0") + bus = can.Bus(interface="socketcan", channel="vcan0", receive_own_messages=True) + + return bus + + +async def can_receiver(can_msg_decoder: cantools.db.Database, message_classes, q1, q2): + # Get bus + can_bus = init_can() + + # Set some asyncio vars + loop = asyncio.get_event_loop() + reader = can.AsyncBufferedReader() + notifier = can.Notifier(can_bus, [reader], loop=loop) + + while True: + # Wait for the next message from the buffer + msg = await reader.get_message() + try: + decoded_msg = can_msg_decoder.decode_message( + msg.arbitration_id, msg.data, decode_containers=True + ) + msg = can_msg_decoder.get_message_by_frame_id(msg.arbitration_id) + msg = protobuf_helpers.pack_protobuf_msg( + decoded_msg, msg.name.lower(), message_classes + ) + data = QueueData(msg.DESCRIPTOR.name, msg) + # await asyncio.sleep(1) + await q1.put(data) + await q2.put(data) + except: + pass + + # Don't forget to stop the notifier to clean up resources. + notifier.stop() diff --git a/py_data_acq/py_data_acq/io_handler/serial_handle.py b/py_data_acq/py_data_acq/io_handler/serial_handle.py new file mode 100644 index 0000000..ecd7569 --- /dev/null +++ b/py_data_acq/py_data_acq/io_handler/serial_handle.py @@ -0,0 +1,39 @@ +import serial_asyncio +import cantools +from ..common import protobuf_helpers +from ..common.common_types import QueueData + + +async def serial_reciever(can_db: cantools.db.Database, message_classes, q1, q2): + # Start asyncio on the port + reader, writer = await serial_asyncio.open_serial_connection( + url="/dev/xboi", baudrate=115200 + ) + + while True: + try: + # if check_sync_byte(reader): + # Wait for the next message from the buffer, then break it into parts using the byte value for "," + decoded_msg = None + sync_msg = await reader.readuntil(b'\n\xff\n') + frameid = int.from_bytes(sync_msg[0:2], byteorder="little") + msg = can_db.get_message_by_frame_id(frameid) + payload = sync_msg[2:-3] + # Break down message + decoded_msg = can_db.decode_message( + frameid, payload, decode_containers=True, decode_choices=True + ) + # Package as protobuf guy + msg = protobuf_helpers.pack_protobuf_msg( + decoded_msg, msg.name.lower(), message_classes + ) + data = QueueData(msg.DESCRIPTOR.name, msg) + # Throw data into queues and start again + await q1.put(data) + await q2.put(data) + + except (KeyError, TypeError, ValueError, IndexError) as e: + print(f"Error decoding frame, error : {e}") + if decoded_msg: + print(f"error msg: {decoded_msg}") + continue diff --git a/py_data_acq/runner.py b/py_data_acq/runner.py index 69539aa..e7a9b7e 100644 --- a/py_data_acq/runner.py +++ b/py_data_acq/runner.py @@ -3,15 +3,14 @@ from py_data_acq.foxglove_live.foxglove_ws import HTProtobufFoxgloveServer from py_data_acq.mcap_writer.writer import HTPBMcapWriter -from py_data_acq.common.common_types import QueueData import py_data_acq.common.protobuf_helpers as pb_helpers from py_data_acq.web_server.mcap_server import MCAPServer -from hytech_np_proto_py import hytech_pb2 -import concurrent.futures +from py_data_acq.io_handler.can_handle import can_receiver +from py_data_acq.io_handler.serial_handle import serial_reciever + +# from py_data_acq.io_handler.serial_handle import import sys import os -import can -from can.interfaces.udp_multicast import UdpMulticastBus import cantools import logging @@ -22,116 +21,76 @@ # - protobuf binary schema file location and file name # - config to inform io handler (say for different CAN baudrates) -can_methods = { - "debug": [UdpMulticastBus.DEFAULT_GROUP_IPv6, 'udp_multicast'], - "local_can_usb_KV": [0, 'kvaser'], - "local_debug": ["vcan0", 'socketcan'] -} -def find_can_interface(): - """Find a CAN interface by checking /sys/class/net/.""" - for interface in os.listdir('/sys/class/net/'): - if interface.startswith('can'): - return interface - return None - -async def continuous_can_receiver(can_msg_decoder: cantools.db.Database, message_classes, queue, q2, can_bus): - loop = asyncio.get_event_loop() - reader = can.AsyncBufferedReader() - notifier = can.Notifier(can_bus, [reader], loop=loop) - - while True: - # Wait for the next message from the buffer - msg = await reader.get_message() - try: - - decoded_msg = can_msg_decoder.decode_message(msg.arbitration_id, msg.data, decode_containers=True) - msg = can_msg_decoder.get_message_by_frame_id(msg.arbitration_id) - msg = pb_helpers.pack_protobuf_msg(decoded_msg, msg.name.lower(), message_classes) - data = QueueData(msg.DESCRIPTOR.name, msg) - # await asyncio.sleep(1) - await queue.put(data) - await q2.put(data) - except: - pass - - # Don't forget to stop the notifier to clean up resources. - notifier.stop() - async def write_data_to_mcap(queue, mcap_writer): async with mcap_writer as mcw: while True: await mcw.write_data(queue) + async def fxglv_websocket_consume_data(queue, foxglove_server): async with foxglove_server as fz: while True: await fz.send_msgs_from_queue(queue) -async def run(logger): - - # for example, we will have CAN as our only input as of right now but we may need to add in - # a sensor that inputs over UART or ethernet - - can_interface = find_can_interface() - - if can_interface: - print(f"Found CAN interface: {can_interface}") - try: - # Attempt to initialize the CAN bus - bus = can.interface.Bus(channel=can_interface, bustype='socketcan') - print(f"Successfully initialized CAN bus on {can_interface}") - # Interface exists and bus is initialized, but this doesn't ensure the interface is 'up' - except can.CanError as e: - print(f"Failed to initialize CAN bus on {can_interface}: {e}") - else: - - print("defaulting to using virtual can interface vcan0") - bus = can.Bus(interface='socketcan', channel='vcan0', receive_own_messages=True) - queue = asyncio.Queue() +async def run(logger): + # Init some bois + queue1 = asyncio.Queue() queue2 = asyncio.Queue() path_to_bin = "" path_to_dbc = "" - + + # Get paths if len(sys.argv) > 2: path_to_bin = sys.argv[1] path_to_dbc = sys.argv[2] else: - path_to_bin = os.environ.get('BIN_PATH') - path_to_dbc = os.environ.get('DBC_PATH') - - full_path = os.path.join(path_to_bin, "hytech.bin") - full_path_to_dbc = os.path.join(path_to_dbc, "hytech.dbc") - db = cantools.db.load_file(full_path_to_dbc) + path_to_bin = os.environ.get("BIN_PATH") + path_to_dbc = os.environ.get("DBC_PATH") + # Load everything + fp_proto = os.path.join(path_to_bin, "hytech.bin") + fp_dbc = os.path.join(path_to_dbc, "car.dbc") + db = cantools.db.load_file(fp_dbc) + # Start foxglove websocket and send message list list_of_msg_names, msg_pb_classes = pb_helpers.get_msg_names_and_classes() - fx_s = HTProtobufFoxgloveServer("0.0.0.0", 8765, "asdf", full_path, list_of_msg_names) + fx_s = HTProtobufFoxgloveServer( + "0.0.0.0", 8765, "asdf", fp_proto, list_of_msg_names + ) + + # Set output path of mcap files, and if on nixos save to a predefined path path_to_mcap = "." - if(os.path.exists('/etc/nixos')): + if os.path.exists("/etc/nixos"): logger.info("detected running on nixos") path_to_mcap = "/home/nixos/recordings" - mcap_writer = HTPBMcapWriter(path_to_mcap, list_of_msg_names, True) mcap_server = MCAPServer(mcap_writer=mcap_writer, path=path_to_mcap) - receiver_task = asyncio.create_task(continuous_can_receiver(db, msg_pb_classes, queue, queue2, bus)) - fx_task = asyncio.create_task(fxglv_websocket_consume_data(queue, fx_s)) + + # Get data source + os.environ["D_SOURCE"] = "SERIAL" + match os.environ.get("D_SOURCE"): + case "SERIAL": + receiver_task = asyncio.create_task( + serial_reciever(db, msg_pb_classes, queue1, queue2) + ) + case _: + receiver_task = asyncio.create_task( + can_receiver(db, msg_pb_classes, queue1, queue2) + ) + + # Setup other guys to respective asyncio tasks + fx_task = asyncio.create_task(fxglv_websocket_consume_data(queue1, fx_s)) mcap_task = asyncio.create_task(write_data_to_mcap(queue2, mcap_writer)) srv_task = asyncio.create_task(mcap_server.start_server()) logger.info("created tasks") - # in the mcap task I actually have to deserialize the any protobuf msg into the message ID and - # the encoded message for the message id. I will need to handle the same association of message id - # and schema in the foxglove websocket server. - + await asyncio.gather(receiver_task, fx_task, mcap_task, srv_task) - # await asyncio.gather(receiver_task, fx_task, mcap_task) - # await asyncio.gather(receiver_task, mcap_task, srv_task) - # await asyncio.gather(receiver_task) if __name__ == "__main__": logging.basicConfig() - logger = logging.getLogger('data_writer_service') + logger = logging.getLogger("data_writer_service") logger.setLevel(logging.INFO) - asyncio.run(run(logger)) \ No newline at end of file + asyncio.run(run(logger)) diff --git a/py_data_acq/serial-broadcast-test.py b/py_data_acq/serial-broadcast-test.py new file mode 100644 index 0000000..9d5a67c --- /dev/null +++ b/py_data_acq/serial-broadcast-test.py @@ -0,0 +1,56 @@ +#!/usr/bin/env python +import time +import cantools +import os +import serial, pty + + +# TODO: Make this real (currently does not actually send data(?)) +def main(): + # Open fake serial port + snd, rev = pty.openpty() + p_name = os.ttyname(snd) + print("Opening on {}".format(p_name)) + ser = serial.Serial(p_name) + + print("Opened for on {}".format(ser)) + + # Load DBC + path_to_dbc = os.environ.get("DBC_PATH") + full_path = os.path.join(path_to_dbc, "hytech.dbc") + db = cantools.database.load_file(full_path) + + # Setup fake messages + msg = db.get_message_by_name("MC1_TORQUE_COMMAND") + rpm = db.get_message_by_name("MC4_SETPOINTS_COMMAND") + + # Serialize the message to bytes + data = msg.encode({"torque_command": 100}) + rpm_set = 100 + + while 1: + # Iterate example vals + rpm_set = rpm_set + 1 + + # Send the guy + ser.write(bytearray(msg.frame_id) + b"," + data) + + # make new message with new data + rpm_data = rpm.encode( + { + "negative_torque_limit": 1, + "positive_torque_limit": 1, + "speed_setpoint_rpm": rpm_set, + "remove_error": 1, + "driver_enable": 1, + "hv_enable": 1, + "inverter_enable": 1, + } + ) + ser.write(bytearray(rpm.frame_id) + b"," + rpm_data) + + time.sleep(0.1) + + +if __name__ == "__main__": + main() diff --git a/py_data_acq/setup.py b/py_data_acq/setup.py index 0918035..e666066 100644 --- a/py_data_acq/setup.py +++ b/py_data_acq/setup.py @@ -5,5 +5,11 @@ name="py_data_acq", version="1.0", packages=find_packages(), - scripts=['runner.py', 'broadcast-test.py', 'data_acq_service.py', 'server_runner.py'] -) \ No newline at end of file + scripts=[ + "runner.py", + "can-broadcast-test.py", + "serial-broadcast-test.py", + "data_acq_service.py", + "server_runner.py", + ], +) diff --git a/py_dbc_proto_gen/dbc_to_proto.py b/py_dbc_proto_gen/dbc_to_proto.py index f2284e4..d623a86 100644 --- a/py_dbc_proto_gen/dbc_to_proto.py +++ b/py_dbc_proto_gen/dbc_to_proto.py @@ -1,40 +1,36 @@ #!/usr/bin/env python import cantools -from cantools.database import * -import pkg_resources +from cantools.database import can, conversion import sys +import os -class HyTechCANmsg: - def __init__(self): - self.can_id_name = "" - self.can_id_hex = 0x0 - self.sender_name = "" - self.packing_type = "" - self.signals = [can.signal.Signal] -# def create_field_name(name: str) -> str: replaced_text = name.replace(" ", "_") replaced_text = replaced_text.replace("(", "") replaced_text = replaced_text.replace(")", "") return replaced_text + def append_proto_message_from_CAN_message(file, can_msg: can.message.Message): # if the msg has a conversion, we know that the value with be a float - file_lines = [] msgname = can_msg.name # type and then name file.write("message " + msgname.lower() + " {\n") line_index = 0 for sig in can_msg.signals: line_index += 1 - - if sig.is_float or ((sig.scale is not None) and (sig.scale != 1.0)) or ( - type(sig.conversion) - is not type(conversion.IdentityConversion(is_float=False)) - and not type( - conversion.NamedSignalConversion( - choices={}, scale=0, offset=0, is_float=False + + if ( + sig.is_float + or ((sig.scale is not None) and (sig.scale != 1.0)) + or ( + type(sig.conversion) + is not type(conversion.IdentityConversion(is_float=False)) + and not type( + conversion.NamedSignalConversion( + choices={}, scale=0, offset=0, is_float=False + ) ) ) ): @@ -45,7 +41,7 @@ def append_proto_message_from_CAN_message(file, can_msg: can.message.Message): + str(line_index) + ";" ) - elif sig.choices is not None and sig.length is not 1: + elif sig.choices is not None: line = ( " string " + create_field_name(sig.name) @@ -61,7 +57,7 @@ def append_proto_message_from_CAN_message(file, can_msg: can.message.Message): + str(line_index) + ";" ) - elif sig.length > 1: + elif (sig.length > 1 and sig.length <=32): line = ( " int32 " + create_field_name(sig.name) @@ -69,22 +65,44 @@ def append_proto_message_from_CAN_message(file, can_msg: can.message.Message): + str(line_index) + ";" ) + elif (sig.length >= 32 and not sig.is_signed): + line = ( + " uint64 " + + create_field_name(sig.name) + + " = " + + str(line_index) + + ";" + ) + elif (sig.length >= 32 and not sig.is_signed): + line = ( + " uint64 " + + create_field_name(sig.name) + + " = " + + str(line_index) + + ";" + ) else: - print("ERROR") + line = ( + " int64 " + + create_field_name(sig.name) + + " = " + + str(line_index) + + ";" + ) file.write(line + "\n") file.write("}\n\n") return file + # load dbc file from the package location -if(len (sys.argv) > 1): +if len(sys.argv) > 1: path_to_dbc = sys.argv[1] else: - path_to_dbc = os.environ.get('DBC_PATH') -full_path = os.path.join(path_to_dbc, "hytech.dbc") + path_to_dbc = os.environ.get("DBC_PATH") +full_path = os.path.join(path_to_dbc, "car.dbc") db = cantools.database.load_file(full_path) with open("hytech.proto", "w+") as proto_file: proto_file.write('syntax = "proto3";\n\n') for msg in db.messages: proto_file = append_proto_message_from_CAN_message(proto_file, msg) - diff --git a/upv.sh b/upv.sh new file mode 100755 index 0000000..4161992 --- /dev/null +++ b/upv.sh @@ -0,0 +1,19 @@ +#!/bin/bash + +# Check if can0 is up +if ! ip link show up | grep -q can0; then + echo "Bringing up vcan0..." + + # Load CAN related modules + sudo modprobe vcan + + # Setup the faux can + sudo ip link add dev vcan0 type vcan + + # Bring up can0 interface + sudo ip link set up vcan0 + + echo "vcan0 setup complete." +else + echo "vcan0 is already up." +fi