From da9cc4702e529f3236aefd8333f27ed1ef68f05b Mon Sep 17 00:00:00 2001 From: Aryan Kashem <130705280+Akashem06@users.noreply.github.com> Date: Wed, 8 Jan 2025 04:15:56 -0500 Subject: [PATCH] Simulation infra (#15) * Simulation infra and can stuff * bootloader and simulation --- SConstruct | 6 +- autogen/can_autogen.py | 21 +- ....c.jinja => test_{{project_name}}.c.jinja} | 0 .../{can_codegen.h => can_codegen.h.jinja} | 4 + .../src/{{project_name}}_rx_all.c.jinja | 95 +- .../src/{{project_name}}_tx_all.c.jinja | 45 +- autogen/templates/simulation/.placeholder | 0 .../simulation_app/inc/can_scheduler.h.jinja | 145 +++ .../src/can_message_handler.cc.jinja | 99 ++ .../simulation_app/src/can_scheduler.cc.jinja | 165 ++++ .../templates/system_can/system_can.h.jinja | 2 +- build_system.md | 7 + can/boards/bms_carrier.yaml | 42 +- can/boards/can_communication.yaml | 44 + can/boards/centre_console.yaml | 4 +- can/inc/can.h | 66 +- can/inc/can_hw.h | 6 +- can/inc/can_msg.h | 3 + can/inc/can_watchdog.h | 42 + can/src/can.c | 132 ++- can/tools/system_dbc.dbc | 36 +- doxygen/Doxyfile | 1 + libraries/ms-common/inc/flash.h | 6 +- libraries/ms-common/inc/i2c.h | 4 +- libraries/ms-common/inc/interrupts.h | 16 +- libraries/ms-common/inc/notify.h | 4 +- libraries/ms-common/inc/spi.h | 4 +- libraries/ms-common/inc/tasks.h | 6 +- .../linker_scripts/bootloader/l4_memory.ld | 2 +- projects/bms_carrier/README.md | 15 + projects/bms_carrier/config.json | 8 + projects/bms_carrier/inc/bms_carrier.h | 24 + projects/bms_carrier/src/main.c | 50 + projects/bms_carrier/test/test_bms_carrier.c | 26 + projects/bootloader/README.md | 15 + projects/bootloader/config.json | 8 + projects/bootloader/inc/bootloader.h | 103 +++ projects/bootloader/inc/bootloader_can.h | 110 +++ .../bootloader/inc/bootloader_can_datagram.h | 112 +++ projects/bootloader/inc/bootloader_crc32.h | 52 ++ projects/bootloader/inc/bootloader_error.h | 52 ++ projects/bootloader/inc/bootloader_flash.h | 89 ++ projects/bootloader/inc/bootloader_mpu.h | 32 + projects/bootloader/src/bootloader.c | 281 ++++++ projects/bootloader/src/bootloader_can.c | 27 + .../bootloader/src/bootloader_can_datagram.c | 23 + projects/bootloader/src/bootloader_crc32.c | 25 + projects/bootloader/src/bootloader_flash.c | 31 + projects/bootloader/src/bootloader_mpu.c | 17 + projects/bootloader/src/main.c | 23 + projects/bootloader/test/test_bootloader.c | 26 + projects/can_communication/src/main.c | 3 +- scons/build.scons | 39 +- scons/common.py | 1 + scons/lint_format.scons | 16 +- simulation/.placeholder | 0 simulation/Simulation_JSON/README.md | 1 + simulation/client/app/inc/app.h | 38 + simulation/client/app/inc/app_callback.h | 43 + simulation/client/app/inc/gpio_manager.h | 115 +++ simulation/client/app/inc/i2c_manager.h | 43 + simulation/client/app/inc/spi_manager.h | 43 + simulation/client/app/src/app_callback.cc | 77 ++ simulation/client/app/src/gpio_manager.cc | 184 ++++ simulation/client/app/src/i2c_manager.cc | 15 + simulation/client/app/src/main.cc | 39 + simulation/client/app/src/spi_manager.cc | 15 + simulation/client/utils/inc/client.h | 127 +++ simulation/client/utils/inc/ntp_client.h | 73 ++ simulation/client/utils/src/client.cc | 158 ++++ simulation/client/utils/src/ntp_client.cc | 102 ++ simulation/command.md | 17 + simulation/common/inc/command_code.h | 68 ++ simulation/common/inc/gpio_datagram.h | 203 ++++ simulation/common/inc/i2c_datagram.h | 124 +++ simulation/common/inc/json_manager.h | 202 ++++ simulation/common/inc/metadata.h | 110 +++ simulation/common/inc/network_time_protocol.h | 132 +++ simulation/common/inc/serialization.h | 74 ++ simulation/common/inc/spi_datagram.h | 124 +++ simulation/common/inc/thread_helpers.h | 36 + simulation/common/src/command_code.cc | 40 + simulation/common/src/gpio_datagram.cc | 91 ++ simulation/common/src/i2c_datagram.cc | 73 ++ simulation/common/src/json_manager.cc | 116 +++ simulation/common/src/metadata.cc | 69 ++ .../common/src/network_time_protocol.cc | 93 ++ simulation/common/src/serialization.cc | 29 + simulation/common/src/spi_datagram.cc | 73 ++ simulation/common/src/thread_helpers.cc | 30 + simulation/config.json | 2 + simulation/scripts/main.py | 22 + simulation/server/app/inc/app.h | 39 + simulation/server/app/inc/app_callback.h | 47 + simulation/server/app/inc/app_terminal.h | 75 ++ simulation/server/app/inc/can_listener.h | 103 +++ simulation/server/app/inc/can_scheduler.h | 342 +++++++ simulation/server/app/inc/gpio_manager.h | 143 +++ simulation/server/app/inc/i2c_manager.h | 43 + simulation/server/app/inc/spi_manager.h | 43 + simulation/server/app/src/app_callback.cc | 76 ++ simulation/server/app/src/app_terminal.cc | 138 +++ simulation/server/app/src/can_listener.cc | 123 +++ .../server/app/src/can_message_handler.cc | 872 ++++++++++++++++++ simulation/server/app/src/can_scheduler.cc | 692 ++++++++++++++ simulation/server/app/src/gpio_manager.cc | 362 ++++++++ simulation/server/app/src/i2c_manager.cc | 18 + simulation/server/app/src/main.cc | 73 ++ simulation/server/app/src/spi_manager.cc | 18 + .../server/utils/inc/client_connection.h | 114 +++ simulation/server/utils/inc/ntp_server.h | 92 ++ simulation/server/utils/inc/server.h | 159 ++++ .../server/utils/src/client_connection.cc | 89 ++ simulation/server/utils/src/ntp_server.cc | 197 ++++ simulation/server/utils/src/server.cc | 241 +++++ 115 files changed, 8983 insertions(+), 132 deletions(-) rename autogen/templates/new_project/test/{test.c.jinja => test_{{project_name}}.c.jinja} (100%) rename autogen/templates/project_can/inc/{can_codegen.h => can_codegen.h.jinja} (87%) delete mode 100644 autogen/templates/simulation/.placeholder create mode 100644 autogen/templates/simulation_app/inc/can_scheduler.h.jinja create mode 100644 autogen/templates/simulation_app/src/can_message_handler.cc.jinja create mode 100644 autogen/templates/simulation_app/src/can_scheduler.cc.jinja create mode 100644 can/boards/can_communication.yaml create mode 100644 projects/bms_carrier/README.md create mode 100644 projects/bms_carrier/config.json create mode 100644 projects/bms_carrier/inc/bms_carrier.h create mode 100644 projects/bms_carrier/src/main.c create mode 100644 projects/bms_carrier/test/test_bms_carrier.c create mode 100644 projects/bootloader/README.md create mode 100644 projects/bootloader/config.json create mode 100644 projects/bootloader/inc/bootloader.h create mode 100644 projects/bootloader/inc/bootloader_can.h create mode 100644 projects/bootloader/inc/bootloader_can_datagram.h create mode 100644 projects/bootloader/inc/bootloader_crc32.h create mode 100644 projects/bootloader/inc/bootloader_error.h create mode 100644 projects/bootloader/inc/bootloader_flash.h create mode 100644 projects/bootloader/inc/bootloader_mpu.h create mode 100644 projects/bootloader/src/bootloader.c create mode 100644 projects/bootloader/src/bootloader_can.c create mode 100644 projects/bootloader/src/bootloader_can_datagram.c create mode 100644 projects/bootloader/src/bootloader_crc32.c create mode 100644 projects/bootloader/src/bootloader_flash.c create mode 100644 projects/bootloader/src/bootloader_mpu.c create mode 100644 projects/bootloader/src/main.c create mode 100644 projects/bootloader/test/test_bootloader.c delete mode 100644 simulation/.placeholder create mode 100644 simulation/Simulation_JSON/README.md create mode 100644 simulation/client/app/inc/app.h create mode 100644 simulation/client/app/inc/app_callback.h create mode 100644 simulation/client/app/inc/gpio_manager.h create mode 100644 simulation/client/app/inc/i2c_manager.h create mode 100644 simulation/client/app/inc/spi_manager.h create mode 100644 simulation/client/app/src/app_callback.cc create mode 100644 simulation/client/app/src/gpio_manager.cc create mode 100644 simulation/client/app/src/i2c_manager.cc create mode 100644 simulation/client/app/src/main.cc create mode 100644 simulation/client/app/src/spi_manager.cc create mode 100644 simulation/client/utils/inc/client.h create mode 100644 simulation/client/utils/inc/ntp_client.h create mode 100644 simulation/client/utils/src/client.cc create mode 100644 simulation/client/utils/src/ntp_client.cc create mode 100644 simulation/command.md create mode 100644 simulation/common/inc/command_code.h create mode 100644 simulation/common/inc/gpio_datagram.h create mode 100644 simulation/common/inc/i2c_datagram.h create mode 100644 simulation/common/inc/json_manager.h create mode 100644 simulation/common/inc/metadata.h create mode 100644 simulation/common/inc/network_time_protocol.h create mode 100644 simulation/common/inc/serialization.h create mode 100644 simulation/common/inc/spi_datagram.h create mode 100644 simulation/common/inc/thread_helpers.h create mode 100644 simulation/common/src/command_code.cc create mode 100644 simulation/common/src/gpio_datagram.cc create mode 100644 simulation/common/src/i2c_datagram.cc create mode 100644 simulation/common/src/json_manager.cc create mode 100644 simulation/common/src/metadata.cc create mode 100644 simulation/common/src/network_time_protocol.cc create mode 100644 simulation/common/src/serialization.cc create mode 100644 simulation/common/src/spi_datagram.cc create mode 100644 simulation/common/src/thread_helpers.cc create mode 100644 simulation/config.json create mode 100644 simulation/scripts/main.py create mode 100644 simulation/server/app/inc/app.h create mode 100644 simulation/server/app/inc/app_callback.h create mode 100644 simulation/server/app/inc/app_terminal.h create mode 100644 simulation/server/app/inc/can_listener.h create mode 100644 simulation/server/app/inc/can_scheduler.h create mode 100644 simulation/server/app/inc/gpio_manager.h create mode 100644 simulation/server/app/inc/i2c_manager.h create mode 100644 simulation/server/app/inc/spi_manager.h create mode 100644 simulation/server/app/src/app_callback.cc create mode 100644 simulation/server/app/src/app_terminal.cc create mode 100644 simulation/server/app/src/can_listener.cc create mode 100644 simulation/server/app/src/can_message_handler.cc create mode 100644 simulation/server/app/src/can_scheduler.cc create mode 100644 simulation/server/app/src/gpio_manager.cc create mode 100644 simulation/server/app/src/i2c_manager.cc create mode 100644 simulation/server/app/src/main.cc create mode 100644 simulation/server/app/src/spi_manager.cc create mode 100644 simulation/server/utils/inc/client_connection.h create mode 100644 simulation/server/utils/inc/ntp_server.h create mode 100644 simulation/server/utils/inc/server.h create mode 100644 simulation/server/utils/src/client_connection.cc create mode 100644 simulation/server/utils/src/ntp_server.cc create mode 100644 simulation/server/utils/src/server.cc diff --git a/SConstruct b/SConstruct index 044abec..e0283e5 100644 --- a/SConstruct +++ b/SConstruct @@ -163,17 +163,13 @@ elif COMMAND == "new": SConscript('scons/new_target.scons', exports='VARS') ########################################################### -# hil command +# HIL command ########################################################### elif COMMAND == "hil": - print(TEST_FILE) if not TEST_FILE: - #Error handling pass - SConscript('scons/pytest.scons', exports='VARS') - ########################################################### # Clean ########################################################### diff --git a/autogen/can_autogen.py b/autogen/can_autogen.py index 059ffff..2d80256 100644 --- a/autogen/can_autogen.py +++ b/autogen/can_autogen.py @@ -1,6 +1,6 @@ ## @file can_autogen.py # @date 2024-12-21 -# @author Midnight Sun Team #24 - MSXVI +# @author Aryan Kashem # @brief YAML parsing and validation module for CAN message configurations # # @details This module provides functionality to parse and validate CAN message @@ -94,6 +94,12 @@ def get_data(args): """ boards = [] messages = [] + message_count = { + "fast_cycle" : 0, + "medium_cycle" : 0, + "slow_cycle" : 0, + "total" : 0, + } for yaml_path in Path("can/boards").glob("*.yaml"): # read yaml @@ -134,9 +140,18 @@ def get_data(args): "receiver": message["target"], }) + # Update message counter + if message["cycle"] == "fast": + message_count["fast_cycle"] += 1 + elif message["cycle"] == "medium": + message_count["medium_cycle"] += 1 + elif message["cycle"] == "slow": + message_count["slow_cycle"] += 1 + + message_count["total"] +=1 + project_name = Path(args.output).parent.stem current_date = datetime.now() current_date = current_date.strftime("%Y-%m-%d") - - return {"boards": boards, "messages": messages, "project_name": project_name, "current_date": current_date} + return {"boards": boards, "messages": messages, "message_count": message_count, "project_name": project_name, "current_date": current_date} diff --git a/autogen/templates/new_project/test/test.c.jinja b/autogen/templates/new_project/test/test_{{project_name}}.c.jinja similarity index 100% rename from autogen/templates/new_project/test/test.c.jinja rename to autogen/templates/new_project/test/test_{{project_name}}.c.jinja diff --git a/autogen/templates/project_can/inc/can_codegen.h b/autogen/templates/project_can/inc/can_codegen.h.jinja similarity index 87% rename from autogen/templates/project_can/inc/can_codegen.h rename to autogen/templates/project_can/inc/can_codegen.h.jinja index da1205b..7864ceb 100644 --- a/autogen/templates/project_can/inc/can_codegen.h +++ b/autogen/templates/project_can/inc/can_codegen.h.jinja @@ -31,6 +31,10 @@ extern rx_struct g_rx_struct; extern tx_struct g_tx_struct; void can_tx_all(); +void can_tx_fast_cycle(); +void can_tx_medium_cycle(); +void can_tx_slow_cycle(); + void can_rx_all(); /** @} */ diff --git a/autogen/templates/project_can/src/{{project_name}}_rx_all.c.jinja b/autogen/templates/project_can/src/{{project_name}}_rx_all.c.jinja index 5d8959f..af8994e 100644 --- a/autogen/templates/project_can/src/{{project_name}}_rx_all.c.jinja +++ b/autogen/templates/project_can/src/{{project_name}}_rx_all.c.jinja @@ -53,13 +53,37 @@ void can_rx_all() { } } -void clear_rx_received() { +void clear_all_rx_received() { {%- for message in messages %} g_rx_struct.received_{{message.name}} = false; {%- endfor %} } -StatusCode check_can_watchdogs() { +void clear_fast_rx_received() { +{%- for message in messages %} +{%- if message.cycle == "fast" %} + g_rx_struct.received_{{message.name}} = false; +{%- endif %} +{%- endfor %} +} + +void clear_medium_rx_received() { +{%- for message in messages %} +{%- if message.cycle == "medium" %} + g_rx_struct.received_{{message.name}} = false; +{%- endif %} +{%- endfor %} +} + +void clear_slow_rx_received() { +{%- for message in messages %} +{%- if message.cycle == "slow" %} + g_rx_struct.received_{{message.name}} = false; +{%- endif %} +{%- endfor %} +} + +StatusCode check_all_can_watchdogs() { {%- for message in messages %} {%- if message.receiver[project_name].watchdog != 0 %} if (!g_rx_struct.received_{{message.name}}) { @@ -67,12 +91,73 @@ StatusCode check_can_watchdogs() { if (s_{{message.name}}_msg_watchdog.cycles_over >= s_{{message.name}}_msg_watchdog.max_cycles) { LOG_CRITICAL("DID NOT RECEIVE CAN MESSAGE: %u IN MAX CYCLES : %u\n", SYSTEM_CAN_MESSAGE_{{message.sender | upper}}_{{message.name | upper}}, s_{{message.name}}_msg_watchdog.max_cycles); - s_{{message.name}}_msg_watchdog.missed = 1; + s_{{message.name}}_msg_watchdog.missed = true; } else { - s_{{message.name}}_msg_watchdog.missed = 0; + s_{{message.name}}_msg_watchdog.missed = false; } } {%- endif %} {%- endfor %} return STATUS_CODE_OK; -} \ No newline at end of file +} + +StatusCode check_fast_can_watchdogs() { +{%- for message in messages %} +{%- if message.cycle == "fast" %} + {%- if message.receiver[project_name].watchdog != 0 %} + if (!g_rx_struct.received_{{message.name}}) { + ++s_{{message.name}}_msg_watchdog.cycles_over; + if (s_{{message.name}}_msg_watchdog.cycles_over >= s_{{message.name}}_msg_watchdog.max_cycles) { + LOG_CRITICAL("DID NOT RECEIVE FAST CYCLE CAN MESSAGE: %u IN MAX CYCLES : %u\n", SYSTEM_CAN_MESSAGE_{{message.sender | upper}}_{{message.name | upper}}, + s_{{message.name}}_msg_watchdog.max_cycles); + s_{{message.name}}_msg_watchdog.missed = true; + } else { + s_{{message.name}}_msg_watchdog.missed = false; + } + } + {%- endif %} +{%- endif %} +{%- endfor %} + return STATUS_CODE_OK; +} + +StatusCode check_medium_can_watchdogs() { +{%- for message in messages %} +{%- if message.cycle == "medium" %} + {%- if message.receiver[project_name].watchdog != 0 %} + if (!g_rx_struct.received_{{message.name}}) { + ++s_{{message.name}}_msg_watchdog.cycles_over; + if (s_{{message.name}}_msg_watchdog.cycles_over >= s_{{message.name}}_msg_watchdog.max_cycles) { + LOG_CRITICAL("DID NOT RECEIVE MEDIUM CYCLE CAN MESSAGE: %u IN MAX CYCLES : %u\n", SYSTEM_CAN_MESSAGE_{{message.sender | upper}}_{{message.name | upper}}, + s_{{message.name}}_msg_watchdog.max_cycles); + s_{{message.name}}_msg_watchdog.missed = true; + } else { + s_{{message.name}}_msg_watchdog.missed = false; + } + } + {%- endif %} +{%- endif %} +{%- endfor %} + return STATUS_CODE_OK; +} + +StatusCode check_slow_can_watchdogs() { +{%- for message in messages %} +{%- if message.cycle == "slow" %} + {%- if message.receiver[project_name].watchdog != 0 %} + if (!g_rx_struct.received_{{message.name}}) { + ++s_{{message.name}}_msg_watchdog.cycles_over; + if (s_{{message.name}}_msg_watchdog.cycles_over >= s_{{message.name}}_msg_watchdog.max_cycles) { + LOG_CRITICAL("DID NOT RECEIVE SLOW CAN MESSAGE: %u IN MAX CYCLES : %u\n", SYSTEM_CAN_MESSAGE_{{message.sender | upper}}_{{message.name | upper}}, + s_{{message.name}}_msg_watchdog.max_cycles); + s_{{message.name}}_msg_watchdog.missed = true; + } else { + s_{{message.name}}_msg_watchdog.missed = false; + } + } + {%- endif %} +{%- endif %} +{%- endfor %} + return STATUS_CODE_OK; +} + diff --git a/autogen/templates/project_can/src/{{project_name}}_tx_all.c.jinja b/autogen/templates/project_can/src/{{project_name}}_tx_all.c.jinja index 448617f..4c75c74 100644 --- a/autogen/templates/project_can/src/{{project_name}}_tx_all.c.jinja +++ b/autogen/templates/project_can/src/{{project_name}}_tx_all.c.jinja @@ -17,6 +17,7 @@ /* Intra-component Headers */ #include "can_codegen.h" {% set messages = messages | selectattr("sender", "eq", project_name) | list -%} + static CanMessage s_msg = { 0U }; static void prv_tx_can_message(CanMessageId id, uint8_t num_bytes, uint64_t data) { @@ -37,4 +38,46 @@ void can_tx_all() { {%- endfor -%} ); {%- endfor %} -} \ No newline at end of file +} + +void can_tx_fast_cycle() { +{%- for message in messages %} +{%- if message.cycle == "fast" %} + prv_tx_can_message( + SYSTEM_CAN_MESSAGE_{{message.sender | upper}}_{{message.name | upper}}, + {{- (message.signals | sum(attribute='length') / 8) | int }}, + {%- for signal in message.signals %} + (uint64_t) g_tx_struct.{{message.name}}_{{signal.name}} << {{signal.start_bit}}{{ " |" if not loop.last }} + {%- endfor -%} + ); +{%- endif %} +{%- endfor %} +} + +void can_tx_medium_cycle() { +{%- for message in messages %} +{%- if message.cycle == "medium" %} + prv_tx_can_message( + SYSTEM_CAN_MESSAGE_{{message.sender | upper}}_{{message.name | upper}}, + {{- (message.signals | sum(attribute='length') / 8) | int }}, + {%- for signal in message.signals %} + (uint64_t) g_tx_struct.{{message.name}}_{{signal.name}} << {{signal.start_bit}}{{ " |" if not loop.last }} + {%- endfor -%} + ); +{%- endif %} +{%- endfor %} +} + +void can_tx_slow_cycle() { +{%- for message in messages %} +{%- if message.cycle == "slow" %} + prv_tx_can_message( + SYSTEM_CAN_MESSAGE_{{message.sender | upper}}_{{message.name | upper}}, + {{- (message.signals | sum(attribute='length') / 8) | int }}, + {%- for signal in message.signals %} + (uint64_t) g_tx_struct.{{message.name}}_{{signal.name}} << {{signal.start_bit}}{{ " |" if not loop.last }} + {%- endfor -%} + ); +{%- endif %} +{%- endfor %} +} diff --git a/autogen/templates/simulation/.placeholder b/autogen/templates/simulation/.placeholder deleted file mode 100644 index e69de29..0000000 diff --git a/autogen/templates/simulation_app/inc/can_scheduler.h.jinja b/autogen/templates/simulation_app/inc/can_scheduler.h.jinja new file mode 100644 index 0000000..0e39553 --- /dev/null +++ b/autogen/templates/simulation_app/inc/can_scheduler.h.jinja @@ -0,0 +1,145 @@ +#pragma once + +/************************************************************************************************ + * @file can_scheduler.h + * + * @brief Header file defining the CanScheduler class + * + * @date 2025-01-04 + * @author Aryan Kashem + ************************************************************************************************/ + +/** @warning This file is autogenerated */ + +/* Standard library Headers */ +#include +#include + +/* Inter-component Headers */ +#include +#include +#include +#include +#include +#include +#include +#include +#include + +/* Intra-component Headers */ + +/** + * @defgroup CanScheduler + * @brief SocketCAN Broadcast Manager abstraction class + * @{ + */ +{% set messages = messages | list %} +/** + * @class CanScheduler + * @brief Class that handles message scheduling over a SocketCAN interface + * @details This class is responsible scheduling CAN messages based on their cycle speed + * Only 3 cycle speeds are supported, Fast (1kHz), medium (10Hz) and slow (1Hz) + * The class shall support message updating during run-time for further bus simulation + */ +class CanScheduler { + private: + const std::string CAN_INTERFACE_NAME = "vcan0"; /**< SocketCAN interface name */ + + static const constexpr unsigned int FAST_CYCLE_SPEED_MS = 1U; /**< CAN fast cycle period in milliseconds */ + static const constexpr unsigned int MEDIUM_CYCLE_SPEED_MS = 100U; /**< CAN medium cycle period in milliseconds */ + static const constexpr unsigned int SLOW_CYCLE_SPEED_MS = 1000U; /**< CAN slow cycle period in milliseconds */ + + static const constexpr unsigned int SLOW_CYCLE_BCM_ID = 0U; /**< Linux Broadcast Manager Id for tracking fast cycle messages */ + static const constexpr unsigned int MEDIUM_CYCLE_BCM_ID = 1U; /**< Linux Broadcast Manager Id for tracking medium cycle messages */ + static const constexpr unsigned int FAST_CYCLE_BCM_ID = 2U; /**< Linux Broadcast Manager Id for tracking slow cycle messages */ + + static const constexpr unsigned int NUM_FAST_CYCLE_MESSAGES = {{ message_count.fast_cycle }}U; /**< Number of fast cycle messages */ + static const constexpr unsigned int NUM_MEDIUM_CYCLE_MESSAGES = {{ message_count.medium_cycle }}U; /**< Number of medium cycle messages */ + static const constexpr unsigned int NUM_SLOW_CYCLE_MESSAGES = {{ message_count.slow_cycle }}U; /**< Number of slow cycle messages */ + static const constexpr unsigned int NUM_TOTAL_MESSAGES = {{ message_count.total }}U; /**< Total number of messages */ + static const constexpr unsigned int MAX_MESSAGE_LENGTH = 8U; /**< Max message length in bytes */ + + {% set fast_message = namespace(count = 0) %} + {%- for message in messages %} + {%- if message.cycle == "fast" %} + static const constexpr unsigned int FAST_{{ message.sender | upper }}_{{ message.name | upper }}_FRAME_INDEX = {{ fast_message.count }}U; /**< Broadcast Manager {{ message.name | lower }} to Frame index mapping */ + {%- set fast_message.count = fast_message.count + 1 %} + {%- endif %} + {%- endfor %} + + {% set medium_message = namespace(count = 0) %} + {%- for message in messages %} + {%- if message.cycle == "medium" %} + static const constexpr unsigned int MEDIUM_{{ message.sender | upper }}_{{ message.name | upper }}_FRAME_INDEX = {{ medium_message.count }}U; /**< Broadcast Manager {{ message.name | lower }} to Frame index mapping */ + {%- set medium_message.count = medium_message.count + 1 %} + {%- endif %} + {%- endfor %} + + {% set slow_message = namespace(count = 0) %} + {%- for message in messages %} + {%- if message.cycle == "slow" %} + static const constexpr unsigned int SLOW_{{ message.sender | upper }}_{{ message.name | upper }}_FRAME_INDEX = {{ slow_message.count }}U; /**< Broadcast Manager {{ message.name | lower }} to Frame index mapping */ + {%- set slow_message.count = slow_message.count + 1 %} + {%- endif %} + {%- endfor %} + + /** + * @brief Fast cycle Broadcast Manager message for the Linux Kernel + */ + struct { + struct bcm_msg_head msg_head; /**< Broadcast Manager message head containing metadata */ + struct can_frame frame[NUM_FAST_CYCLE_MESSAGES]; /**< CAN message frames that shall be scheduled for fast cycle */ + } canFastCycleBCM; + + /** + * @brief Medium cycle Broadcast Manager message for the Linux Kernel + */ + struct { + struct bcm_msg_head msg_head; /**< Broadcast Manager message head containing metadata */ + struct can_frame frame[NUM_MEDIUM_CYCLE_MESSAGES]; /**< CAN message frames that shall be scheduled for medium cycle */ + } canMediumCycleBCM; + + /** + * @brief Slow cycle Broadcast Manager message for the Linux Kernel + */ + struct { + struct bcm_msg_head msg_head; /**< Broadcast Manager message head containing metadata */ + struct can_frame frame[NUM_SLOW_CYCLE_MESSAGES]; /**< CAN message frames that shall be scheduled for slow cycle */ + } canSlowCycleBCM; + + int m_bcmCanSocket; /**< The CAN schedulers Broadcast Manager socket FD */ + std::atomic m_isConnected; /**< Boolean flag to track the CAN schedulers connection status */ + + /** + * @brief Schedules all CAN data by updating the Broacast Manager socket + * @details This function is called by startCanScheduler + * This function shall initialize all CAN message values to 0 + */ + void scheduleCanMessages(); + + public: + /** + * @brief Constructs a CanScheduler object + * @details Initializes the CanScheduler. The constructor sets up internal variables + */ + CanScheduler(); + + /** + * @brief Starts the CAN scheduler and sets all messages to 0. Must only be called once + * @details This function will connect to the Linux Broadcast Manager + * This function must only be called once, and it will set all messages to 0 + */ + void startCanScheduler(); + + {%- for message in messages %} + {%- for signal in message.signals %} + /** + * @brief Update the CAN value for {{ message.name | lower }} {{ signal.name | lower }} + * @param {{ signal.name | lower }}_value New value for the signal + */ + void update_{{ message.name | lower }}_{{ signal.name | lower }}(uint{{ signal.length }}_t {{ signal.name | lower }}_value); + {%- endfor %} + {%- endfor %} +}; + +/** @} */ diff --git a/autogen/templates/simulation_app/src/can_message_handler.cc.jinja b/autogen/templates/simulation_app/src/can_message_handler.cc.jinja new file mode 100644 index 0000000..f0a57c2 --- /dev/null +++ b/autogen/templates/simulation_app/src/can_message_handler.cc.jinja @@ -0,0 +1,99 @@ +/************************************************************************************************ + * @file can_message_handler.cc + * + * @brief Source file defining the Can Message Handler function + * + * @date {{ current_date }} + * @author Aryan Kashem + ************************************************************************************************/ + +/** @warning This file is autogenerated */ + +/* Standard library Headers */ +#include +#include +#include +#include + +/* Inter-component Headers */ +#include + +#include "system_can.h" + +/* Intra-component Headers */ +#include "can_listener.h" + +{% set messages = messages | list -%} +{%- for message in messages %} +/** + * @brief Storage class for {{ message.name | lower }} CAN message + */ +struct {{ message.name | lower }} { + {%- for signal in message.signals %} + uint{{ signal.length }}_t {{ signal.name }}; /**< CAN signal '{{ signal.name }}' defined in *.yaml */ + {%- endfor %} + + /** + * @brief Decode new CAN data and update the storage for {{ message.name }} + * @param data Pointer to the CAN message to be decoded + */ + void decode(const uint8_t *data) { + uint64_t raw_val = 0U; + uint8_t start_byte = 0U; + + {%- for signal in message.signals %} + { + raw_val = 0U; + start_byte = {{signal.start_bit // 8}}; + {% for i in range((signal.length + 7) // 8) %}raw_val |= static_cast(data[start_byte + {{ i }}]) << {{ i * 8 }}U; + {% endfor %} + {{ signal.name }} = raw_val; + } + {%- endfor %} + } + + /** + * @brief Create a JSON object for {{ message.name }} using the storage + */ + nlohmann::json to_json() const { + return { + {%- for signal in message.signals %} + {"{{ signal.name }}", {{ signal.name }}}{% if not loop.last %},{% endif %} + {%- endfor %} + }; + } + + /** + * @brief Get the message name: {{ message.name }} + * @return Returns the message name + */ + std::string get_message_name() const { + return "{{ message.name }}"; + } +}; + + +{%- endfor %} + +/** + * @brief Main CAN message Handler + * @details This function shall dynamically allocate a struct based on the CAN ID + * This function shall decode the data, then update the CAN Cache 'm_canInfo' with JSON data + * @param id Can message ID + * @param data Pointer to the CAN message data + */ +void CanListener::canMessageHandler(uint32_t id, const uint8_t* data) { + switch (id) { + {%- for message in messages %} + case SYSTEM_CAN_MESSAGE_{{message.sender | upper}}_{{message.name | upper}}: { + {{ message.name | lower }} *message = new {{ message.name | lower }}(); + message->decode(data); + m_canInfo[message->get_message_name()] = message->to_json(); + break; + } + {%- endfor %} + default: { + std::cout << "Unknown message ID: " << static_cast(id) << std::endl; + } + } +} diff --git a/autogen/templates/simulation_app/src/can_scheduler.cc.jinja b/autogen/templates/simulation_app/src/can_scheduler.cc.jinja new file mode 100644 index 0000000..10b4de7 --- /dev/null +++ b/autogen/templates/simulation_app/src/can_scheduler.cc.jinja @@ -0,0 +1,165 @@ +/************************************************************************************************ + * @file can_scheduler.cc + * + * @brief Source file defining the CanScheduler class + * + * @date {{ current_date }} + * @author Aryan Kashem + ************************************************************************************************/ + +/** @warning This file is autogenerated */ + +/* Standard library Headers */ +#include +#include + +/* Inter-component Headers */ +#include "system_can.h" + +/* Intra-component Headers */ +#include "can_scheduler.h" +{% set messages = messages | list %} + +CanScheduler::CanScheduler() { + m_isConnected = false; + m_bcmCanSocket = -1; +} + +void CanScheduler::scheduleCanMessages() { + canFastCycleBCM.msg_head.opcode = TX_SETUP; + canFastCycleBCM.msg_head.can_id = FAST_CYCLE_BCM_ID; + canFastCycleBCM.msg_head.flags = SETTIMER | STARTTIMER; + canFastCycleBCM.msg_head.nframes = NUM_FAST_CYCLE_MESSAGES; + canFastCycleBCM.msg_head.count = 0; + + canFastCycleBCM.msg_head.ival1.tv_sec = 0U; + canFastCycleBCM.msg_head.ival1.tv_usec = 0U; + canFastCycleBCM.msg_head.ival2.tv_sec = FAST_CYCLE_SPEED_MS / 1000U; + canFastCycleBCM.msg_head.ival2.tv_usec = (FAST_CYCLE_SPEED_MS % 1000U) * 1000U; + + {% set fast_message = namespace(count = 0) %} + {%- for message in messages %} + {%- if message.cycle == "fast" %} + canFastCycleBCM.frame[FAST_{{ message.sender | upper }}_{{ message.name | upper }}_FRAME_INDEX].can_id = SYSTEM_CAN_MESSAGE_{{ message.sender | upper }}_{{ message.name | upper }}; + canFastCycleBCM.frame[FAST_{{ message.sender | upper }}_{{ message.name | upper }}_FRAME_INDEX].can_dlc = {{ (message.signals | sum(attribute='length') / 8) | int }}U; + memset(canFastCycleBCM.frame[FAST_{{ message.sender | upper }}_{{ message.name | upper }}_FRAME_INDEX].data, 0U, MAX_MESSAGE_LENGTH); + {%- set fast_message.count = fast_message.count + 1 %} + {%- endif %} + {%- endfor %} + + if (write(m_bcmCanSocket, &canFastCycleBCM, sizeof(canFastCycleBCM)) < 0) { + throw std::runtime_error("Failed to schedule CAN BCM Fast cycle messages"); + } + + canMediumCycleBCM.msg_head.opcode = TX_SETUP; + canMediumCycleBCM.msg_head.can_id = MEDIUM_CYCLE_BCM_ID; + canMediumCycleBCM.msg_head.flags = SETTIMER | STARTTIMER; + canMediumCycleBCM.msg_head.nframes = NUM_MEDIUM_CYCLE_MESSAGES; + canMediumCycleBCM.msg_head.count = 0; + + canMediumCycleBCM.msg_head.ival1.tv_sec = 0U; + canMediumCycleBCM.msg_head.ival1.tv_usec = 0U; + canMediumCycleBCM.msg_head.ival2.tv_sec = MEDIUM_CYCLE_SPEED_MS / 1000U; + canMediumCycleBCM.msg_head.ival2.tv_usec = (MEDIUM_CYCLE_SPEED_MS % 1000U) * 1000U; + + {% set medium_message = namespace(count = 0) %} + {%- for message in messages %} + {%- if message.cycle == "medium" %} + canMediumCycleBCM.frame[MEDIUM_{{ message.sender | upper }}_{{ message.name | upper }}_FRAME_INDEX].can_id = SYSTEM_CAN_MESSAGE_{{ message.sender | upper }}_{{ message.name | upper }}; + canMediumCycleBCM.frame[MEDIUM_{{ message.sender | upper }}_{{ message.name | upper }}_FRAME_INDEX].can_dlc = {{ (message.signals | sum(attribute='length') / 8) | int }}U; + memset(canMediumCycleBCM.frame[MEDIUM_{{ message.sender | upper }}_{{ message.name | upper }}_FRAME_INDEX].data, 0U, MAX_MESSAGE_LENGTH); + {%- set medium_message.count = medium_message.count + 1 %} + {%- endif %} + {%- endfor %} + + if (write(m_bcmCanSocket, &canMediumCycleBCM, sizeof(canMediumCycleBCM)) < 0) { + throw std::runtime_error("Failed to schedule CAN BCM Medium cycle messages"); + } + + canSlowCycleBCM.msg_head.opcode = TX_SETUP; + canSlowCycleBCM.msg_head.can_id = SLOW_CYCLE_BCM_ID; + canSlowCycleBCM.msg_head.flags = SETTIMER | STARTTIMER; + canSlowCycleBCM.msg_head.nframes = NUM_SLOW_CYCLE_MESSAGES; + canSlowCycleBCM.msg_head.count = 0; + + canSlowCycleBCM.msg_head.ival1.tv_sec = 0U; + canSlowCycleBCM.msg_head.ival1.tv_usec = 0U; + canSlowCycleBCM.msg_head.ival2.tv_sec = SLOW_CYCLE_SPEED_MS / 1000U; + canSlowCycleBCM.msg_head.ival2.tv_usec = (SLOW_CYCLE_SPEED_MS % 1000U) * 1000U; + + {% set slow_message = namespace(count = 0) %} + {%- for message in messages %} + {%- if message.cycle == "slow" %} + canSlowCycleBCM.frame[SLOW_{{ message.sender | upper }}_{{ message.name | upper }}_FRAME_INDEX].can_id = SYSTEM_CAN_MESSAGE_{{ message.sender | upper }}_{{ message.name | upper }}; + canSlowCycleBCM.frame[SLOW_{{ message.sender | upper }}_{{ message.name | upper }}_FRAME_INDEX].can_dlc = {{ (message.signals | sum(attribute='length') / 8) | int }}U; + memset(canSlowCycleBCM.frame[SLOW_{{ message.sender | upper }}_{{ message.name | upper }}_FRAME_INDEX].data, 0U, MAX_MESSAGE_LENGTH); + {%- set slow_message.count = slow_message.count + 1 %} + {%- endif %} + {%- endfor %} + + if (write(m_bcmCanSocket, &canSlowCycleBCM, sizeof(canSlowCycleBCM)) < 0) { + throw std::runtime_error("Failed to schedule CAN BCM Slow cycle messages"); + } +} + +void CanScheduler::startCanScheduler() { + try { + m_bcmCanSocket = socket(PF_CAN, SOCK_DGRAM, CAN_BCM); + + if (m_bcmCanSocket < 0) { + throw std::runtime_error("Error creating socket for CAN Broadcast Manager"); + } + + struct ifreq ifr; + strcpy(ifr.ifr_name, CAN_INTERFACE_NAME.c_str()); + if (ioctl(m_bcmCanSocket, SIOCGIFINDEX, &ifr) < 0) { + throw std::runtime_error("Error writing interface name to socketCAN file descriptor. Check if vcan0 is enabled?"); + } + + struct sockaddr_can addr = {}; + addr.can_family = AF_CAN; + addr.can_ifindex = ifr.ifr_ifindex; + + if (connect(m_bcmCanSocket, (struct sockaddr *)&addr, sizeof(addr)) < 0) { + throw std::runtime_error("Error connecting to SocketCAN broadcast manager"); + } + + scheduleCanMessages(); + + } catch (std::exception &e) { + std::cerr << "Error running CAN Scheduler: " << e.what() << std::endl; + } +} + +{%- for message in messages %} +{%- for signal in message.signals %} +void CanScheduler::update_{{ message.name | lower }}_{{ signal.name | lower }}(uint{{ signal.length }}_t {{ signal.name | lower }}_value) { + try { + unsigned int start_byte = {{signal.start_bit // 8}}; + {%- if message.cycle == "fast" %} + {% for i in range((signal.length + 7) // 8) %} + canFastCycleBCM.frame[FAST_{{ message.sender | upper }}_{{ message.name | upper }}_FRAME_INDEX].data[start_byte + {{ i }}U] = ({{ signal.name | lower }}_value >> {{ i * 8 }}U) & 0xFFU;{% endfor %} + if (write(m_bcmCanSocket, &canFastCycleBCM, sizeof(canFastCycleBCM)) < 0) { + throw std::runtime_error("Failed to update {{ message.name | lower }} {{ signal.name | lower }}"); + } + {%- endif %} + {%- if message.cycle == "medium" %} + {% for i in range((signal.length + 7) // 8) %} + canMediumCycleBCM.frame[MEDIUM_{{ message.sender | upper }}_{{ message.name | upper }}_FRAME_INDEX].data[start_byte + {{ i }}U] = ({{ signal.name | lower }}_value >> {{ i * 8 }}U) & 0xFFU;{% endfor %} + if (write(m_bcmCanSocket, &canMediumCycleBCM, sizeof(canMediumCycleBCM)) < 0) { + throw std::runtime_error("Failed to update {{ message.name | lower }} {{ signal.name | lower }}}"); + } + {%- endif %} + {%- if message.cycle == "slow" %} + {% for i in range((signal.length + 7) // 8) %} + canSlowCycleBCM.frame[SLOW_{{ message.sender | upper }}_{{ message.name | upper }}_FRAME_INDEX].data[start_byte + {{ i }}U] = ({{ signal.name | lower }}_value >> {{ i * 8 }}U) & 0xFFU;{% endfor %} + if (write(m_bcmCanSocket, &canSlowCycleBCM, sizeof(canSlowCycleBCM)) < 0) { + throw std::runtime_error("Failed to update {{ message.name | lower }} {{ signal.name | lower }}"); + } + {%- endif %} + } catch (std::exception &e) { + std::cerr << e.what() << std::endl; + } +} +{%- endfor %} +{%- endfor %} diff --git a/autogen/templates/system_can/system_can.h.jinja b/autogen/templates/system_can/system_can.h.jinja index ae9442d..c3945b4 100644 --- a/autogen/templates/system_can/system_can.h.jinja +++ b/autogen/templates/system_can/system_can.h.jinja @@ -35,6 +35,6 @@ typedef enum { {% for message in messages %} #define SYSTEM_CAN_MESSAGE_{{ message.sender | upper }}_{{ message.name | upper }} \ - {% if not message.critical %} SYSTEM_CAN_MESSAGE_PRIORITY_BIT + {% endif %}({{ message.id }} << CAN_MESSAGE_ID_OFFSET) + SYSTEM_CAN_DEVICE_{{ message.sender | upper }} + {% if not message.critical %} SYSTEM_CAN_MESSAGE_PRIORITY_BIT + {% endif %}({{ message.id }} << SYSTEM_CAN_MESSAGE_ID_OFFSET) + SYSTEM_CAN_DEVICE_{{ message.sender | upper }} {% endfor %} /** @} */ diff --git a/build_system.md b/build_system.md index 715f0ae..7838624 100644 --- a/build_system.md +++ b/build_system.md @@ -63,6 +63,10 @@ Commands: (x86) Run the project's binary. - e.g. `scons sim --platform=x86 ` (`scons sim --platform=x86 --project=new_led`) + vehicle_sim + (x86) Runs a full vehicle simulation + - e.g. 'scons vehicle_sim --platform=x86' + gdb (x86) Run the project's binary with gdb. - e.g. `scons gdb ` (`scons gdb --project=new_led`) @@ -84,6 +88,9 @@ Commands: clean Delete the `build` directory. + hil + Runs a HIL test. This is not fully implemented yet + doxygen Build a local copy of the doxygen HTML diff --git a/can/boards/bms_carrier.yaml b/can/boards/bms_carrier.yaml index 69ed757..a8854bb 100644 --- a/can/boards/bms_carrier.yaml +++ b/can/boards/bms_carrier.yaml @@ -25,25 +25,8 @@ afe_status: length: 8 - battery_info: - id: 14 - critical: true - cycle: "slow" - target: - centre_console: - watchdog: 0 - signals: - fan1: - length: 8 - fan2: - length: 8 - max_cell_v: - length: 16 - min_cell_v: - length: 16 - battery_vt: - id: 15 + id: 2 critical: true cycle: "fast" target: @@ -58,11 +41,28 @@ length: 16 batt_perc: length: 16 + + battery_info: + id: 3 + critical: true + cycle: "slow" + target: + centre_console: + watchdog: 0 + signals: + fan1: + length: 8 + fan2: + length: 8 + max_cell_v: + length: 16 + min_cell_v: + length: 16 AFE1_status: id: 61 critical: false - cycle: "medium" + cycle: "slow" target: centre_console: watchdog: 0 @@ -83,7 +83,7 @@ AFE2_status: id: 62 critical: false - cycle: "medium" + cycle: "slow" target: centre_console: watchdog: 0 @@ -104,7 +104,7 @@ AFE3_status: id: 63 critical: false - cycle: "medium" + cycle: "slow" target: centre_console: watchdog: 0 diff --git a/can/boards/can_communication.yaml b/can/boards/can_communication.yaml new file mode 100644 index 0000000..6d29a5b --- /dev/null +++ b/can/boards/can_communication.yaml @@ -0,0 +1,44 @@ +# If you need to add a new message use a reasonable +# reserved ID. The higher ID the lower the priority. Generally +# - 0-13: Critical messages +# - 14-30: Actionable messages (trigger a change in another system) +# - 30-63: Data messages (usually not actionable by an onboard device) + +--- + Messages: + fast_one_shot_msg: + id: 58 + critical: false + cycle: "fast" + target: + can_communication: + watchdog: 5 + signals: + sig1: + length: 16 + sig2: + length: 16 + medium_one_shot_msg: + id: 59 + critical: false + cycle: "medium" + target: + can_communication: + watchdog: 5 + signals: + sig1: + length: 16 + sig2: + length: 16 + slow_one_shot_msg: + id: 60 + critical: false + cycle: "slow" + target: + can_communication: + watchdog: 5 + signals: + sig1: + length: 16 + sig2: + length: 16 diff --git a/can/boards/centre_console.yaml b/can/boards/centre_console.yaml index aa0a91f..5f4c570 100644 --- a/can/boards/centre_console.yaml +++ b/can/boards/centre_console.yaml @@ -7,7 +7,7 @@ --- Messages: cc_pedal: - id: 2 + id: 4 critical: true cycle: "fast" target: @@ -58,7 +58,7 @@ Messages: length: 8 cc_regen_percentage: - id: 8 + id: 7 critical: true cycle: "medium" target: diff --git a/can/inc/can.h b/can/inc/can.h index 4a61718..885eead 100644 --- a/can/inc/can.h +++ b/can/inc/can.h @@ -27,12 +27,15 @@ * @{ */ +/** @brief Maximum time permitted for a CAN transaction */ +#define CAN_TIMEOUT_MS 5U + /** * @brief Storage class for the device ID and RX data */ typedef struct CanStorage { - volatile CanQueue rx_queue; - uint16_t device_id; + volatile CanQueue rx_queue; /**< Global RX queue to store CAN messages */ + uint16_t device_id; /**< Device ID of the running device */ } CanStorage; /** @@ -48,62 +51,79 @@ StatusCode can_init(CanStorage *storage, const CanSettings *settings); /** * @brief Sets a filter on the CAN interface * @param msg_id Message ID of the message to filter - * @return STATUS_CODE_OK if adding the filter succeeded + * @return STATUS_CODE_OK if the filter is added successfully * STATUS_CODE_INVALID_ARGS if one of the parameters are incorrect * STATUS_CODE_INTERNAL_ERROR if HAL initialization fails */ StatusCode can_add_filter_in(CanMessageId msg_id); /** - * @brief Initialize the CAN interface + * @brief Transmits CAN data on the bus * @param msg Pointer to the message to transmit - * @return STATUS_CODE_OK if data is transmitted succesfuully + * @return STATUS_CODE_OK if data is transmitted successfully * STATUS_CODE_INVALID_ARGS if one of the parameters are incorrect - * STATUS_CODE_INTERNAL_ERROR if HAL initialization fails + * STATUS_CODE_UNINITIALIZED if the CAN bus is not initialized */ StatusCode can_transmit(const CanMessage *msg); /** - * @brief Initialize the CAN interface + * @brief Receives CAN data from the bus * @param msg Pointer to the message to update on receive - * @return STATUS_CODE_OK if data is retrieved succesfuully + * @return STATUS_CODE_OK if data is retrieved successfully * STATUS_CODE_INVALID_ARGS if one of the parameters are incorrect - * STATUS_CODE_INTERNAL_ERROR if HAL initialization fails + * STATUS_CODE_UNINITIALIZED if the CAN bus is not initialized */ StatusCode can_receive(const CanMessage *msg); /** - * @brief Run the CAN TX cache to transmit all messages - * @param rx_queue Pointer to the CAN RX queue - * @param settings Pointer to the CAN settings - * @return STATUS_CODE_OK if the cache is cleared succesfuully + * @brief Transmit all CAN data + * @return STATUS_CODE_OK if the cache is cleared successfully * STATUS_CODE_INTERNAL_ERROR if HAL initialization fails */ -StatusCode run_can_tx_cache(); +StatusCode run_can_tx_all(); /** - * @brief Run the CAN RX cache to receive all messages - * @param rx_queue Pointer to the CAN RX queue - * @param settings Pointer to the CAN settings - * @return STATUS_CODE_OK if the cache is cleared succesfuully + * @brief Transmit all fast-cycle CAN data + * @return STATUS_CODE_OK if the cache is cleared successfully + * STATUS_CODE_INTERNAL_ERROR if HAL initialization fails + */ +StatusCode run_can_tx_fast(); + +/** + * @brief Transmit all medium-cycle CAN data + * @return STATUS_CODE_OK if the cache is cleared successfully + * STATUS_CODE_INTERNAL_ERROR if HAL initialization fails + */ +StatusCode run_can_tx_medium(); + +/** + * @brief Transmit all slow-cycle CAN data + * @return STATUS_CODE_OK if the cache is cleared successfully + * STATUS_CODE_INTERNAL_ERROR if HAL initialization fails + */ +StatusCode run_can_tx_slow(); + +/** + * @brief Receive all CAN data + * @return STATUS_CODE_OK if the cache is cleared successfully * STATUS_CODE_INTERNAL_ERROR if HAL initialization fails */ -StatusCode run_can_rx_cache(); +StatusCode run_can_rx_all(); /** - * @brief Clear the RX cache + * @brief Clear the RX data struct * @return STATUS_CODE_OK if initialization succeeded * STATUS_CODE_INVALID_ARGS if one of the parameters are incorrect * STATUS_CODE_INTERNAL_ERROR if HAL initialization fails */ -StatusCode clear_rx_cache(); +StatusCode clear_rx_struct(); /** - * @brief Clear the TX cache + * @brief Clear the TX data struct * @return STATUS_CODE_OK if initialization succeeded * STATUS_CODE_INVALID_ARGS if one of the parameters are incorrect * STATUS_CODE_INTERNAL_ERROR if HAL initialization fails */ -StatusCode clear_tx_cache(); +StatusCode clear_tx_struct(); /** @} */ diff --git a/can/inc/can_hw.h b/can/inc/can_hw.h index 339e308..dbaa099 100644 --- a/can/inc/can_hw.h +++ b/can/inc/can_hw.h @@ -89,6 +89,8 @@ StatusCode can_hw_init(const CanQueue* rx_queue, const CanSettings *settings); /** * @brief Sets a filter on the CAN interface + * @details The filter works as such: + * if INCOMING_MSG_ID & mask == filter & mask, the message is handled * @param mask Determines which bits in the received ID are considered during filtering * @param filter Specifies the pattern the CAN ID must adhere to * @param extended Boolean to use CAN extended ID feature @@ -111,7 +113,7 @@ CanHwBusStatus can_hw_bus_status(void); * @param extended Boolean to use CAN extended ID feature * @param data Pointer to the data to transmit * @param len Size of the data to transfer - * @return STATUS_CODE_OK if data is transmitted succesfully + * @return STATUS_CODE_OK if data is transmitted successfully * STATUS_CODE_RESOURCE_EXHAUSTED if CAN mailbox is full * STATUS_CODE_INVALID_ARGS if one of the parameters are incorrect */ @@ -123,7 +125,7 @@ StatusCode can_hw_transmit(uint32_t id, bool extended, const uint8_t *data, size * @param extended Pointer to a flag to indicate CAN extended ID feature * @param data Pointer to a buffer to store data * @param len Pointer to the number of CAN messages received - * @return true if data is retrieved succesfully + * @return true if data is retrieved successfully * false if one of the parameters are incorrect or internal error occurred */ bool can_hw_receive(uint32_t *id, bool *extended, uint64_t *data, size_t *len); diff --git a/can/inc/can_msg.h b/can/inc/can_msg.h index b4fa661..b37bb5e 100644 --- a/can/inc/can_msg.h +++ b/can/inc/can_msg.h @@ -23,6 +23,9 @@ * @{ */ +/** @brief Maximum permitted CAN ID for 11-bit identifiers */ +#define CAN_MSG_MAX_STD_IDS (1 << 11) + /** * @brief CAN message ID * @details 11 Bits in standard mode diff --git a/can/inc/can_watchdog.h b/can/inc/can_watchdog.h index 5be5578..909caa2 100644 --- a/can/inc/can_watchdog.h +++ b/can/inc/can_watchdog.h @@ -23,4 +23,46 @@ * @{ */ +/** + * @brief Software defined CAN watchdog + */ +typedef struct CanWatchDog { + uint16_t cycles_over; /**< Tracks the number of cycles over max_cycles */ + uint16_t max_cycles; /**< Maximum number of cycles open for data to be received */ + bool missed; /**< Flag to track if the message has been missed */ +} CanWatchDog; + +/** + * @brief Checks all CAN watchdogs + * @details This will throw a LOG_WARN if a message has missed its watchdog + * @return STATUS_CODE_OK if all watchdogs are checked succesfully + */ +StatusCode check_all_can_watchdogs(); + +/** + * @brief Checks CAN watchdogs for messages in fast-cycle + * @details This will throw a LOG_WARN if a message has missed its watchdog + * @return STATUS_CODE_OK if all watchdogs are checked succesfully + */ +StatusCode check_fast_can_watchdogs(); + +/** + * @brief Checks CAN watchdogs for messages in medium-cycle + * @details This will throw a LOG_WARN if a message has missed its watchdog + * @return STATUS_CODE_OK if all watchdogs are checked succesfully + */ +StatusCode check_medium_can_watchdogs(); + +/** + * @brief Checks CAN watchdogs for messages in slow-cycle + * @details This will throw a LOG_WARN if a message has missed its watchdog + * @return STATUS_CODE_OK if all watchdogs are checked succesfully + */ +StatusCode check_slow_can_watchdogs(); + +void clear_all_rx_received(); +void clear_fast_rx_received(); +void clear_medium_rx_received(); +void clear_slow_rx_received(); + /** @} */ diff --git a/can/src/can.c b/can/src/can.c index 57d9b80..9c08be1 100644 --- a/can/src/can.c +++ b/can/src/can.c @@ -15,13 +15,16 @@ #include "FreeRTOS.h" #include "semphr.h" #include "log.h" +#include "system_can.h" +#include "can_codegen.h" /* Intra-component Headers */ #include "can_hw.h" #include "can.h" +#include "can_watchdog.h" -// rx_struct g_rx_struct; -// tx_struct g_tx_struct; +rx_struct g_rx_struct; +tx_struct g_tx_struct; static CanStorage *s_can_storage; @@ -33,17 +36,17 @@ static SemaphoreHandle_t s_can_rx_handle; StatusCode can_init(CanStorage *storage, const CanSettings *settings) { - // if (settings->device_id >= CAN_MSG_MAX_DEVICES) { - // return STATUS_CODE_INVALID_ARGS; - // } + if (settings->device_id >= NUM_SYSTEM_CAN_DEVICES) { + return STATUS_CODE_INVALID_ARGS; + } s_can_storage = storage; memset(s_can_storage, 0, sizeof(*s_can_storage)); s_can_storage->device_id = settings->device_id; - // memset(&g_tx_struct, 0, sizeof(g_tx_struct)); - // memset(&g_rx_struct, 0, sizeof(g_rx_struct)); + memset(&g_tx_struct, 0, sizeof(g_tx_struct)); + memset(&g_rx_struct, 0, sizeof(g_rx_struct)); status_ok_or_return(can_queue_init(&s_can_storage->rx_queue)); @@ -58,4 +61,117 @@ StatusCode can_init(CanStorage *storage, const CanSettings *settings) } return STATUS_CODE_OK; -} \ No newline at end of file +} + +StatusCode can_add_filter_in(CanMessageId msg_id) { + + if (s_can_storage == NULL) { + return STATUS_CODE_UNINITIALIZED; + } else if (msg_id >= CAN_MSG_MAX_STD_IDS) { + return STATUS_CODE_INVALID_ARGS; + } + + /* Set the filter as the message ID */ + CanId can_id = { .raw = msg_id }; + + /* Mask everything, to check all bits of the incoming ID */ + CanId mask = { 0U }; + mask.raw = (uint32_t)~mask.msg_id; + + return can_hw_add_filter_in(mask.raw, can_id.raw, false); +} + +StatusCode can_transmit(const CanMessage *msg) { + if (s_can_storage == NULL) { + return STATUS_CODE_UNINITIALIZED; + } + + return can_hw_transmit(msg->id.raw, msg->extended, msg->data_u8, msg->dlc); +} + +StatusCode can_receive(const CanMessage *msg) { + if (s_can_storage == NULL) { + return STATUS_CODE_UNINITIALIZED; + } + + StatusCode ret = can_queue_pop(&s_can_storage->rx_queue, msg); + + // if (ret == STATUS_CODE_OK) + // { + // LOG_DEBUG("Source Id: %d\n", msg->id); + // LOG_DEBUG("Data: %lx\n", msg->data); + // LOG_DEBUG("DLC: %ld\n", msg->dlc); + // LOG_DEBUG("ret: %d\n", ret); + // } + + return ret; +} + +StatusCode run_can_tx_all() { + xSemaphoreTake(s_can_tx_handle, pdMS_TO_TICKS(CAN_TIMEOUT_MS)); + + can_tx_all(); + + /* Check all cycles watchdogs, and update internal states */ + check_all_can_watchdogs(); + clear_all_rx_received(); + + xSemaphoreGive(s_can_tx_handle); + return STATUS_CODE_OK; +} + +StatusCode run_can_tx_fast() { + xSemaphoreTake(s_can_tx_handle, pdMS_TO_TICKS(CAN_TIMEOUT_MS)); + + can_tx_fast_cycle(); + + /* Check fast cycle watchdogs, and update internal states */ + check_fast_can_watchdogs(); + clear_fast_rx_received(); + + xSemaphoreGive(s_can_tx_handle); + return STATUS_CODE_OK; +} + +StatusCode run_can_tx_medium() { + xSemaphoreTake(s_can_tx_handle, pdMS_TO_TICKS(CAN_TIMEOUT_MS)); + + can_tx_medium_cycle(); + + /* Check medium cycle watchdogs, and update internal states */ + check_medium_can_watchdogs(); + clear_medium_rx_received(); + + xSemaphoreGive(s_can_tx_handle); + return STATUS_CODE_OK; +} + +StatusCode run_can_tx_slow() { + xSemaphoreTake(s_can_tx_handle, pdMS_TO_TICKS(CAN_TIMEOUT_MS)); + + can_tx_slow_cycle(); + + /* Check slow cycle watchdogs, and update internal states */ + check_slow_can_watchdogs(); + clear_slow_rx_received(); + + xSemaphoreGive(s_can_tx_handle); + return STATUS_CODE_OK; +} + +StatusCode run_can_rx_all() { + xSemaphoreTake(s_can_rx_handle, pdMS_TO_TICKS(CAN_TIMEOUT_MS)); + can_rx_all(); + xSemaphoreGive(s_can_rx_handle); + return STATUS_CODE_OK; +} + +StatusCode clear_rx_struct() { + memset(&g_rx_struct, 0, sizeof(g_rx_struct)); + return STATUS_CODE_OK; +} + +StatusCode clear_tx_struct() { + memset(&g_rx_struct, 0, sizeof(g_rx_struct)); + return STATUS_CODE_OK; +} diff --git a/can/tools/system_dbc.dbc b/can/tools/system_dbc.dbc index 24b9237..1376fdc 100644 --- a/can/tools/system_dbc.dbc +++ b/can/tools/system_dbc.dbc @@ -30,7 +30,7 @@ NS_ : BS_: -BU_: BMS_CARRIER CENTRE_CONSOLE +BU_: BMS_CARRIER CAN_COMMUNICATION CENTRE_CONSOLE BO_ 16 battery_status: 7 bms_carrier SG_ fault : 0|16@1+ (1,0) [0|65535] "" centre_console, power_distribution @@ -38,18 +38,18 @@ BO_ 16 battery_status: 7 bms_carrier SG_ aux_batt_v : 32|16@1+ (1,0) [0|65535] "" centre_console, power_distribution SG_ afe_status : 48|8@1+ (1,0) [0|255] "" centre_console, power_distribution -BO_ 224 battery_info: 6 bms_carrier - SG_ fan1 : 0|8@1+ (1,0) [0|255] "" centre_console - SG_ fan2 : 8|8@1+ (1,0) [0|255] "" centre_console - SG_ max_cell_v : 16|16@1+ (1,0) [0|65535] "" centre_console - SG_ min_cell_v : 32|16@1+ (1,0) [0|65535] "" centre_console - -BO_ 240 battery_vt: 8 bms_carrier +BO_ 32 battery_vt: 8 bms_carrier SG_ voltage : 0|16@1+ (1,0) [0|65535] "" centre_console SG_ current : 16|16@1+ (1,0) [0|65535] "" centre_console SG_ temperature : 32|16@1+ (1,0) [0|65535] "" centre_console SG_ batt_perc : 48|16@1+ (1,0) [0|65535] "" centre_console +BO_ 48 battery_info: 6 bms_carrier + SG_ fan1 : 0|8@1+ (1,0) [0|255] "" centre_console + SG_ fan2 : 8|8@1+ (1,0) [0|255] "" centre_console + SG_ max_cell_v : 16|16@1+ (1,0) [0|65535] "" centre_console + SG_ min_cell_v : 32|16@1+ (1,0) [0|65535] "" centre_console + BO_ 2000 AFE1_status: 8 bms_carrier SG_ id : 0|8@1+ (1,0) [0|255] "" centre_console, power_distribution SG_ temp : 8|8@1+ (1,0) [0|255] "" centre_console, power_distribution @@ -71,21 +71,33 @@ BO_ 2032 AFE3_status: 8 bms_carrier SG_ v2 : 32|16@1+ (1,0) [0|65535] "" centre_console, power_distribution SG_ v3 : 48|16@1+ (1,0) [0|65535] "" centre_console, power_distribution -BO_ 33 cc_pedal: 5 centre_console +BO_ 1953 fast_one_shot_msg: 4 can_communication + SG_ sig1 : 0|16@1+ (1,0) [0|65535] "" can_communication + SG_ sig2 : 16|16@1+ (1,0) [0|65535] "" can_communication + +BO_ 1969 medium_one_shot_msg: 4 can_communication + SG_ sig1 : 0|16@1+ (1,0) [0|65535] "" can_communication + SG_ sig2 : 16|16@1+ (1,0) [0|65535] "" can_communication + +BO_ 1985 slow_one_shot_msg: 4 can_communication + SG_ sig1 : 0|16@1+ (1,0) [0|65535] "" can_communication + SG_ sig2 : 16|16@1+ (1,0) [0|65535] "" can_communication + +BO_ 66 cc_pedal: 5 centre_console SG_ throttle_output : 0|32@1+ (1,0) [0|4294967295] "" motor_controller, power_distribution SG_ brake_output : 32|8@1+ (1,0) [0|255] "" motor_controller, power_distribution -BO_ 81 cc_info: 8 centre_console +BO_ 82 cc_info: 8 centre_console SG_ target_velocity : 0|32@1+ (1,0) [0|4294967295] "" motor_controller, power_distribution SG_ drive_state : 32|8@1+ (1,0) [0|255] "" motor_controller, power_distribution SG_ cruise_control : 40|8@1+ (1,0) [0|255] "" motor_controller, power_distribution SG_ regen_braking : 48|8@1+ (1,0) [0|255] "" motor_controller, power_distribution SG_ hazard_enabled : 56|8@1+ (1,0) [0|255] "" motor_controller, power_distribution -BO_ 97 cc_steering: 2 centre_console +BO_ 98 cc_steering: 2 centre_console SG_ input_cc : 0|8@1+ (1,0) [0|255] "" power_distribution, motor_controller SG_ input_lights : 8|8@1+ (1,0) [0|255] "" power_distribution, motor_controller -BO_ 129 cc_regen_percentage: 4 centre_console +BO_ 114 cc_regen_percentage: 4 centre_console SG_ percent : 0|32@1+ (1,0) [0|4294967295] "" motor_controller diff --git a/doxygen/Doxyfile b/doxygen/Doxyfile index a43c5bb..bbd9344 100644 --- a/doxygen/Doxyfile +++ b/doxygen/Doxyfile @@ -916,6 +916,7 @@ INPUT = projects \ libraries/master \ doxygen/main_page.md \ autogen \ + simulation \ build_system.md # This tag can be used to specify the character encoding of the source files diff --git a/libraries/ms-common/inc/flash.h b/libraries/ms-common/inc/flash.h index c34b306..eeadaf3 100644 --- a/libraries/ms-common/inc/flash.h +++ b/libraries/ms-common/inc/flash.h @@ -57,7 +57,7 @@ StatusCode flash_init(); * @param address Memory address to read from. This value MUST be 4-byte aligned * @param buffer Pointer to the buffer to store data * @param buffer_len Length of the buffer. This value MUST be 4-byte aligned - * @return STATUS_CODE_OK if flash memory was read succesfully + * @return STATUS_CODE_OK if flash memory was read successfully * STATUS_CODE_OUT_OF_RANGE if address is out of range * STATUS_CODE_INVALID_ARGS if address or read bytes is not aligned */ @@ -70,7 +70,7 @@ StatusCode flash_read(uintptr_t address, uint8_t *buffer, size_t buffer_len); * @param address Memory address to store the buffer. This value MUST be 4-byte aligned * @param buffer Pointer to the buffer of data to store * @param buffer_len Length of the buffer to write. This MUST also be 4-byte aligned - * @return STATUS_CODE_OK if flash memory was written succesfully + * @return STATUS_CODE_OK if flash memory was written successfully * STATUS_CODE_OUT_OF_RANGE if address is out of range * STATUS_CODE_INTERNAL_ERROR if flash write failed */ @@ -80,7 +80,7 @@ StatusCode flash_write(uintptr_t address, uint8_t *buffer, size_t buffer_len); * @brief Erase pages of flash memory * @param start_page First page to erase * @param num_pages Number of pages to erase - * @return STATUS_CODE_OK if flash memory was erased succesfully + * @return STATUS_CODE_OK if flash memory was erased successfully * STATUS_CODE_INVALID_ARGS if incorrect arguments were passed in * STATUS_CODE_INTERNAL_ERROR if flash erase failed */ diff --git a/libraries/ms-common/inc/i2c.h b/libraries/ms-common/inc/i2c.h index 601bc13..8ce735f 100644 --- a/libraries/ms-common/inc/i2c.h +++ b/libraries/ms-common/inc/i2c.h @@ -130,7 +130,7 @@ StatusCode i2c_write_reg(I2CPort i2c, I2CAddress addr, uint8_t reg, uint8_t *tx_ * @param i2c Specifies which I2C port to update * @param data Pointer to a buffer of data to set * @param len Length of the data to set - * @return STATUS_CODE_OK if data is set succesfully + * @return STATUS_CODE_OK if data is set successfully * STATUS_CODE_INVALID_ARGS if one of the parameters are incorrect */ StatusCode i2c_set_rx_data(I2CPort i2c, uint8_t *data, size_t len); @@ -140,7 +140,7 @@ StatusCode i2c_set_rx_data(I2CPort i2c, uint8_t *data, size_t len); * @param i2c Specifies which I2C port to read from * @param data Pointer to a buffer of data to fill * @param len Length of the data to retrieve - * @return STATUS_CODE_OK if data is retrieved succesfully + * @return STATUS_CODE_OK if data is retrieved successfully * STATUS_CODE_INVALID_ARGS if one of the parameters are incorrect */ StatusCode i2c_get_tx_data(I2CPort i2c, uint8_t *data, size_t len); diff --git a/libraries/ms-common/inc/interrupts.h b/libraries/ms-common/inc/interrupts.h index c5a47f5..c94413d 100644 --- a/libraries/ms-common/inc/interrupts.h +++ b/libraries/ms-common/inc/interrupts.h @@ -88,7 +88,7 @@ void interrupt_init(void); * @brief Enables the nested interrupt vector controller for a given channel * @param irq_channel Numeric ID of the interrupt channel from the NVIC * @param priority Priority level of the interrupt - * @return STATUS_CODE_OK if the channel is succesfully initialized + * @return STATUS_CODE_OK if the channel is successfully initialized * STATUS_CODE_INVALID_ARGS if one of the parameters are incorrect */ StatusCode interrupt_nvic_enable(uint8_t irq_channel, InterruptPriority priority); @@ -97,7 +97,7 @@ StatusCode interrupt_nvic_enable(uint8_t irq_channel, InterruptPriority priority * @brief Enables an external interrupt line with the given settings * @param address Pointer to the GPIO address * @param settings Pointer to the interrupt settings - * @return STATUS_CODE_OK if the channel is succesfully initialized + * @return STATUS_CODE_OK if the channel is successfully initialized * STATUS_CODE_INVALID_ARGS if one of the parameters are incorrect */ StatusCode interrupt_exti_enable(GpioAddress *address, const InterruptSettings *settings); @@ -105,7 +105,7 @@ StatusCode interrupt_exti_enable(GpioAddress *address, const InterruptSettings * /** * @brief Triggers a software interrupt on a given external interrupt channel * @param line Numeric ID of the EXTI line (GPIO Pin number) - * @return STATUS_CODE_OK if the channel is succesfully initialized + * @return STATUS_CODE_OK if the channel is successfully initialized * STATUS_CODE_INVALID_ARGS if one of the parameters are incorrect */ StatusCode interrupt_exti_trigger(uint8_t line); @@ -114,7 +114,7 @@ StatusCode interrupt_exti_trigger(uint8_t line); * @brief Get the pending flag for an external interrupt * @param line Numeric ID of the EXTI line (GPIO Pin number) * @param pending_bit Pointer to a variable that is updated with the pending bit - * @return STATUS_CODE_OK if the channel is succesfully initialized + * @return STATUS_CODE_OK if the channel is successfully initialized * STATUS_CODE_INVALID_ARGS if one of the parameters are incorrect */ StatusCode interrupt_exti_get_pending(uint8_t line, uint8_t *pending_bit); @@ -122,7 +122,7 @@ StatusCode interrupt_exti_get_pending(uint8_t line, uint8_t *pending_bit); /** * @brief Clears the pending flag for an external interrupt * @param line Numeric ID of the EXTI line (GPIO Pin number) - * @return STATUS_CODE_OK if the channel is succesfully initialized + * @return STATUS_CODE_OK if the channel is successfully initialized * STATUS_CODE_INVALID_ARGS if one of the parameters are incorrect */ StatusCode interrupt_exti_clear_pending(uint8_t line); @@ -131,7 +131,7 @@ StatusCode interrupt_exti_clear_pending(uint8_t line); * @brief Masks or clears the external interrupt on a given line * @param line Numeric ID of the EXTI line (GPIO Pin number) * @param masked 0: Clears the interrupt 1: Mask the lines interrupt - * @return STATUS_CODE_OK if the channel is succesfully initialized + * @return STATUS_CODE_OK if the channel is successfully initialized * STATUS_CODE_INVALID_ARGS if one of the parameters are incorrect */ StatusCode interrupt_exti_set_mask(uint8_t line, bool masked); @@ -147,7 +147,7 @@ typedef void (*x86InterruptHandler)(uint8_t interrupt_id); * @param irq_channel Numeric ID of the interrupt channel from the NVIC * @param handler Function pointer to the interrupt handler. Can be left as NULL * @param settings Pointer to the interrupt settings - * @return STATUS_CODE_OK if the interrupt handler is registered succesfully + * @return STATUS_CODE_OK if the interrupt handler is registered successfully * STATUS_CODE_INVALID_ARGS if one of the parameters are incorrect */ StatusCode interrupt_nvic_register_handler(uint8_t irq_channel, x86InterruptHandler handler, const InterruptSettings *settings); @@ -158,7 +158,7 @@ StatusCode interrupt_nvic_register_handler(uint8_t irq_channel, x86InterruptHand * @param line Numeric ID of the EXTI line (GPIO Pin number) * @param handler Function pointer to the interrupt handler. Can be left as NULL * @param settings Pointer to the interrupt settings - * @return STATUS_CODE_OK if the interrupt handler is registered succesfully + * @return STATUS_CODE_OK if the interrupt handler is registered successfully * STATUS_CODE_INVALID_ARGS if one of the parameters are incorrect */ StatusCode interrupt_exti_register_handler(uint8_t line, x86InterruptHandler handler, const InterruptSettings *settings); diff --git a/libraries/ms-common/inc/notify.h b/libraries/ms-common/inc/notify.h index 5cfa2c4..e3b0b10 100644 --- a/libraries/ms-common/inc/notify.h +++ b/libraries/ms-common/inc/notify.h @@ -53,7 +53,7 @@ bool notify_check_event(uint32_t *notification, Event event); /** * @brief Get the current notification value for the calilng task without a timeout * @param notification Pointer to a notification value that is updated upon success - * @return STATUS_CODE_OK if the value is retrieved succesfully + * @return STATUS_CODE_OK if the value is retrieved successfully * STATUS_CODE_TIMEOUT if the value cannot be retrieved */ StatusCode notify_get(uint32_t *notification); @@ -62,7 +62,7 @@ StatusCode notify_get(uint32_t *notification); * @brief Get the current notification value for the calilng task with a maximum timeout * @param notification Pointer to a notification value that is polled * @param ms_to_wait Time in milliseconds to wait for a notification before timing out - * @return STATUS_CODE_OK if notification is received succesfully + * @return STATUS_CODE_OK if notification is received successfully * STATUS_CODE_TIMEOUT if a timeout has occurred */ StatusCode notify_wait(uint32_t *notification, uint32_t ms_to_wait); diff --git a/libraries/ms-common/inc/spi.h b/libraries/ms-common/inc/spi.h index dc0512b..7f567dc 100644 --- a/libraries/ms-common/inc/spi.h +++ b/libraries/ms-common/inc/spi.h @@ -119,7 +119,7 @@ StatusCode spi_exchange(SpiPort spi, uint8_t *tx_data, size_t tx_len, uint8_t *r * @param i2c Specifies which SPI port to read from * @param data Pointer to a buffer of data to fill * @param len Length of the data to retrieve - * @return STATUS_CODE_OK if data is retrieved succesfully + * @return STATUS_CODE_OK if data is retrieved successfully * STATUS_CODE_INVALID_ARGS if one of the parameters are incorrect */ StatusCode spi_get_tx_data(SpiPort spi, uint8_t *data, uint8_t len); @@ -129,7 +129,7 @@ StatusCode spi_get_tx_data(SpiPort spi, uint8_t *data, uint8_t len); * @param spi Specifies which SPI port to update * @param data Pointer to a buffer of data to set * @param len Length of the data to set - * @return STATUS_CODE_OK if data is set succesfully + * @return STATUS_CODE_OK if data is set successfully * STATUS_CODE_INVALID_ARGS if one of the parameters are incorrect */ StatusCode spi_set_rx(SpiPort spi, uint8_t *data, uint8_t len); diff --git a/libraries/ms-common/inc/tasks.h b/libraries/ms-common/inc/tasks.h index 0d36ee1..229c040 100644 --- a/libraries/ms-common/inc/tasks.h +++ b/libraries/ms-common/inc/tasks.h @@ -96,7 +96,7 @@ typedef struct Task { * @param priority The task priority: higher number is higher priority, and the maximum * @param context Pointer to arguments that are passed to the task * is configNUM_TASK_PRIORITIES - 1. - * @return STATUS_CODE_OK if succesfully initialize task. + * @return STATUS_CODE_OK if successfully initialize task. */ StatusCode tasks_init_task(Task *task, TaskPriority priority, void *context); @@ -109,7 +109,7 @@ void tasks_start(void); /** * @brief Initialize the task module * @details Must be called before tasks are initialized or the scheduler is started. - * @return STATUS_CODE_OK if succesfully initialize the module. + * @return STATUS_CODE_OK if successfully initialize the module. */ StatusCode tasks_init(void); @@ -124,7 +124,7 @@ StatusCode wait_tasks(uint16_t num_tasks); /** * @brief A wrapper to give to the semaphore, to be called by tasks when they complete - * @return STATUS_CODE_OK if succesfully release sempahore + * @return STATUS_CODE_OK if successfully release sempahore */ StatusCode send_task_end(void); diff --git a/platform/linker_scripts/bootloader/l4_memory.ld b/platform/linker_scripts/bootloader/l4_memory.ld index a701dc1..3abb99a 100644 --- a/platform/linker_scripts/bootloader/l4_memory.ld +++ b/platform/linker_scripts/bootloader/l4_memory.ld @@ -24,7 +24,7 @@ _estack = _sram_start + _sram_size; MEMORY { - BOOTLOADER (rx) : ORIGIN = _flash_start, LENGTH = _bootloader_size + BOOTLOADER (rx) : ORIGIN = _bootloader_start, LENGTH = _bootloader_size APPLICATION (rx) : ORIGIN = _application_start, LENGTH = _application_size RAM (rwx) : ORIGIN = 0x20000000, LENGTH = 64K } diff --git a/projects/bms_carrier/README.md b/projects/bms_carrier/README.md new file mode 100644 index 0000000..d45daab --- /dev/null +++ b/projects/bms_carrier/README.md @@ -0,0 +1,15 @@ + +# bms_carrier \ No newline at end of file diff --git a/projects/bms_carrier/config.json b/projects/bms_carrier/config.json new file mode 100644 index 0000000..efab08f --- /dev/null +++ b/projects/bms_carrier/config.json @@ -0,0 +1,8 @@ +{ + "libs": [ + "FreeRTOS", + "ms-common", + "master" + ], + "can": true +} \ No newline at end of file diff --git a/projects/bms_carrier/inc/bms_carrier.h b/projects/bms_carrier/inc/bms_carrier.h new file mode 100644 index 0000000..9cfe3a3 --- /dev/null +++ b/projects/bms_carrier/inc/bms_carrier.h @@ -0,0 +1,24 @@ +#pragma once + +/************************************************************************************************ + * @file bms_carrier.h + * + * @brief Header file for bms_carrier + * + * @date 2024-12-25 + * @author Midnight Sun Team #24 - MSXVI + ************************************************************************************************/ + +/* Standard library Headers */ + +/* Inter-component Headers */ + +/* Intra-component Headers */ + +/** + * @defgroup bms_carrier + * @brief bms_carrier Firmware + * @{ + */ + +/** @} */ diff --git a/projects/bms_carrier/src/main.c b/projects/bms_carrier/src/main.c new file mode 100644 index 0000000..3dc6d5a --- /dev/null +++ b/projects/bms_carrier/src/main.c @@ -0,0 +1,50 @@ +/************************************************************************************************ + * @file main.c + * + * @brief Main file for bms_carrier + * + * @date 2024-12-25 + * @author Midnight Sun Team #24 - MSXVI + ************************************************************************************************/ + +/* Standard library Headers */ + +/* Inter-component Headers */ +#include "can.h" +#include "gpio.h" +#include "log.h" +#include "master_tasks.h" +#include "mcu.h" +#include "tasks.h" + +/* Intra-component Headers */ +#include "bms_carrier.h" + +void pre_loop_init() {} + +void run_1000hz_cycle() { + run_can_rx_all(); + + run_can_tx_fast(); +} + +void run_10hz_cycle() { + run_can_tx_medium(); +} + +void run_1hz_cycle() { + run_can_tx_slow(); +} + +int main() { + mcu_init(); + tasks_init(); + log_init(); + + init_master_tasks(); + + tasks_start(); + + LOG_DEBUG("exiting main?"); + return 0; +} diff --git a/projects/bms_carrier/test/test_bms_carrier.c b/projects/bms_carrier/test/test_bms_carrier.c new file mode 100644 index 0000000..a21738a --- /dev/null +++ b/projects/bms_carrier/test/test_bms_carrier.c @@ -0,0 +1,26 @@ +/************************************************************************************************ + * @file test_bms_carrier.c + * + * @brief Test file for bms_carrier + * + * @date 2024-12-25 + * @author Midnight Sun Team #24 - MSXVI + ************************************************************************************************/ + +/* Standard library Headers */ +#include +#include + +/* Inter-component Headers */ +#include "test_helpers.h" +#include "unity.h" + +/* Intra-component Headers */ + +void setup_test(void) {} + +void teardown_test(void) {} + +void test_example(void) { + TEST_ASSERT_TRUE(true); +} diff --git a/projects/bootloader/README.md b/projects/bootloader/README.md new file mode 100644 index 0000000..7c891bd --- /dev/null +++ b/projects/bootloader/README.md @@ -0,0 +1,15 @@ + +# bootloader \ No newline at end of file diff --git a/projects/bootloader/config.json b/projects/bootloader/config.json new file mode 100644 index 0000000..7467279 --- /dev/null +++ b/projects/bootloader/config.json @@ -0,0 +1,8 @@ +{ + "arm_libs": [ + "CMSIS", + "HAL" + ], + "can": false, + "arm_only": true +} \ No newline at end of file diff --git a/projects/bootloader/inc/bootloader.h b/projects/bootloader/inc/bootloader.h new file mode 100644 index 0000000..d632e41 --- /dev/null +++ b/projects/bootloader/inc/bootloader.h @@ -0,0 +1,103 @@ +#pragma once + +/************************************************************************************************ + * @file bootloader.h + * + * @brief Header file for bootloader + * + * @date 2025-01-08 + * @author Midnight Sun Team #24 - MSXVI + ************************************************************************************************/ + +/* Standard library Headers */ +#include + +/* Inter-component Headers */ + +/* Intra-component Headers */ +#include "bootloader_can.h" +#include "bootloader_error.h" + +/** + * @defgroup bootloader + * @brief bootloader Firmware + * @{ + */ + +extern uint32_t _application_start; +extern uint32_t _application_size; +extern uint32_t _bootloader_start; +extern uint32_t _bootloader_size; +extern uint32_t _sram_start; +extern uint32_t _sram_size; + +/** @brief Application start address as defined in the linkerscripts */ +#define APPLICATION_START_ADDRESS ((uint32_t)&_application_start) + +/** @brief Application size as defined in the linkerscripts */ +#define APPLICATION_SIZE ((size_t)&_application_size) + +/** @brief Bootloader start address as defined in the linkerscripts */ +#define BOOTLOADER_START_ADDRESS ((uint32_t)&_bootloader_start) + +/** @brief Bootloader size as defined in the linkerscripts */ +#define BOOTLOADER_SIZE ((size_t)&_bootloader_size) + +/** @brief SRAM start address as defined in the linkerscripts */ +#define SRAM_START_ADDRESS ((uint32_t)&_sram_start) + +/** @brief SRAM size as defined in the linkerscripts */ +#define SRAM_SIZE ((size_t)&_sram_size) + +/** + * @brief Bootloader State Machine + */ +typedef enum { + BOOTLOADER_UNINITIALIZED = 0, /**< Bootloader starts as unitialized */ + BOOTLOADER_IDLE, /**< Bootloader is initialized and waiting for messages */ + BOOTLOADER_START, /**< Bootloader is processing a start message */ + BOOTLOADER_WAIT_SEQUENCING, /**< Bootloader is waiting for the data sequencing packet */ + BOOTLOADER_DATA_RECEIVE, /**< Bootloader is receiving streamed data and flashing it in 2048 byte chunks */ + BOOTLOADER_JUMP_APP, /**< Bootloader is prompted to jump to application defined by APPLICATION_START_ADDRESS */ + BOOTLOADER_FAULT /**< Bootloader is in fault state */ +} BootloaderStates; + +/** + * @brief Private Bootloader State Storage + */ +typedef struct { + uintptr_t application_start; /**< Application start address */ + uintptr_t current_write_address; /**< Current write address */ + uint32_t bytes_written; /**< Number of bytes written to flash */ + uint32_t data_size; /**< Data size (ie: Binary application size) */ + uint32_t packet_crc32; /**< Packet CRC32 if available */ + uint16_t expected_sequence_number; /**< Next expected sequence number for validation */ + uint16_t buffer_index; /**< Data buffer index for correct reading/writing */ + + BootloaderStates state; /**< Internal state tracker */ + BootloaderError error; /**< Bootloader error tracker */ + uint16_t target_nodes; /**< Target MCU Ids */ + bool first_byte_received; /**< Boolean flag to track if the first byte was received */ +} BootloaderStateData; + +/** + * @brief Initialize the bootloader + * @return BOOTLOADER_ERROR_NONE if the bootloader is initialized succesfully + */ +BootloaderError bootloader_init(void); + +/** + * @brief Run the bootloader + * @param msg Pointer to incoming CAN data to feed the bootloader + * @return BootloaderError collected through the state machine + */ +BootloaderError bootloader_run(Boot_CanMessage *msg); + +/** + * @brief Jump application + * @details Exits bootloader and jumps to main application + * @return BOOTLOADER_INTERNAL_ERR. This function should never return + */ +BootloaderError bootloader_jump_app(void); + +/** @} */ diff --git a/projects/bootloader/inc/bootloader_can.h b/projects/bootloader/inc/bootloader_can.h new file mode 100644 index 0000000..fe1d8f0 --- /dev/null +++ b/projects/bootloader/inc/bootloader_can.h @@ -0,0 +1,110 @@ +#pragma once + +/************************************************************************************************ + * @file bootloader_can.h + * + * @brief Header file for CAN communication in the bootloader + * + * @date 2025-01-08 + * @author Midnight Sun Team #24 - MSXVI + ************************************************************************************************/ + +/* Standard library Headers */ +#include +#include +#include + +/* Inter-component Headers */ + +/* Intra-component Headers */ +#include "bootloader_error.h" + +/** + * @defgroup bootloader + * @brief bootloader Firmware + * @{ + */ + +/** + * @brief Selection for the supported CAN Bitrates + * @details CAN bit timing is composed of multiple time segments: + * - Synchronization Segment (Sync_Seg): Allows resynchronization + * - Propagation Segment (Prop_Seg): Compensates for physical delays + * - Phase Buffer Segments (Phase_Seg1 and Phase_Seg2): + * * Absorb time quanta variations + * * Provide sampling point adjustment + * @note Typical sampling occurs between 75-80% of bit time + * Higher bitrates require shorter time quantas + * Bitrate selection impacts signal reliability and bus length + */ +typedef enum { + BOOT_CAN_BITRATE_125KBPS, /**< 125 KBits per second */ + BOOT_CAN_BITRATE_250KBPS, /**< 250 KBits per second */ + BOOT_CAN_BITRATE_500KBPS, /**< 500 KBits per second */ + BOOT_CAN_BITRATE_1000KBPS, /**< 1000 KBits per second */ + NUM_BOOT_CAN_BITRATES /**< Number of supported bit rates */ +} Boot_CanBitrate; + +typedef enum { CAN_CONTINUOUS = 0, CAN_ONE_SHOT_MODE, NUM_CAN_MODES } Boot_CanMode; + +/** + * @brief CAN Settings + * @details TX Pin will transmit data. RX pin will receive data. + * The bitrate is the number of bits sent per second. + * The device ID is the CAN ID of the STM32 node. + * Loopback will internally connect the transmit and receive CAN lines for testing. + * Silent mode is used for only listening to the bus. + */ +typedef struct CanSettings { + uint16_t device_id; /**< Device CAN ID */ + Boot_CanBitrate bitrate; /**< Bits per second */ + bool loopback; /**< Enables self-listening for message debugging */ + bool silent; /**< Device can listen but not transmit messages */ +} Boot_CanSettings; + +/** + * @brief CAN message structure + * @details CAN ID is subjected to arbitration where lower ID's are given higher priority + * Extended ID flag indicates a 29-bit ID or an 11-bit ID + * Data length code ias 4 bit value indicating the number of bytes in the payload + * CAN Frames hold 8 bytes of data at most + */ +typedef struct { + uint32_t id; /**< CAN ID */ + uint8_t extended; /**< Extended ID Flag */ + size_t dlc; /**< Data length Code */ + union { + uint64_t data; /**< Raw data in the form of 8 bytes */ + uint32_t data_u32[2]; /**< Data split in 4 bytes partitions */ + uint16_t data_u16[4]; /**< Data split in 2 bytes partitions */ + uint8_t data_u8[8]; /**< Data split in 1 byte partitions */ + }; +} Boot_CanMessage; + +/** + * @brief Initialize the CAN interface for the bootloader + * @return BOOTLOADER_ERROR_NONE if the interface is initialized succesfully + * BOOTLOADER_CAN_INIT_ERR if initialization fails + */ +BootloaderError boot_can_init(const Boot_CanSettings *settings); + +/** + * @brief Transmit a CAN message in the bootloader + * @param id CAN Message arbitration Id + * @param extended Boolean flag to select 11-bit vs 29-bit CAN transmission + * @param data Pointer to the data buffer to transmit + * @param len Length of the data buffer + * @return BOOTLOADER_ERROR_NONE if the message is transmitted succesfully + * BOOLOADER_CAN_TRANSMISSION_ERROR if transmitting the message fails + */ +BootloaderError boot_can_transmit(uint32_t id, bool extended, const uint8_t *data, size_t len); + +/** + * @brief Receive a CAN message in the bootloader + * @param msg Pointer to the message that will be updated with incoming data + * @return BOOTLOADER_ERROR_NONE if the message is received succesfully + * BOOTLOADER_CAN_RECEIVE_ERROR if receiving the message fails + */ +BootloaderError boot_can_receive(Boot_CanMessage *msg); + +/** @} */ diff --git a/projects/bootloader/inc/bootloader_can_datagram.h b/projects/bootloader/inc/bootloader_can_datagram.h new file mode 100644 index 0000000..5fdc8d7 --- /dev/null +++ b/projects/bootloader/inc/bootloader_can_datagram.h @@ -0,0 +1,112 @@ +#pragma once + +/************************************************************************************************ + * @file bootloader_can_datagram.h + * + * @brief Header file for the CAN datagram in the bootloader + * + * @date 2025-01-08 + * @author Midnight Sun Team #24 - MSXVI + ************************************************************************************************/ + +/* Standard library Headers */ +#include +#include + +/* Inter-component Headers */ + +/* Intra-component Headers */ +#include "bootloader_can.h" +#include "bootloader_error.h" + +/** + * @defgroup bootloader + * @brief bootloader Firmware + * @{ + */ + +/** @brief ACK defined as 0 */ +#define ACK 0U + +/** @brief NACK defined as 1 */ +#define NACK 1U + +/** + * @brief Bootloader reserved CAN arbitration IDs + */ +typedef enum { + BOOTLOADER_CAN_SEQUENCING_ID = 30, /**< Data sequencing command (Highest priority) */ + BOOTLOADER_CAN_FLASH_ID, /**< Incoming flash data command */ + BOOTLOADER_CAN_JUMP_APPLICATION_ID, /**< Jump to application command */ + BOOTLOADER_CAN_ACK_ID, /**< Bootloader ACK/NACK message */ + BOOTLOADER_CAN_START_ID, /**< Start DFU command */ + BOOTLOADER_CAN_JUMP_BOOTLOADER /**< Jump to bootloader command */ +} BootloaderCanID; + +/** + * @brief Datagram message definition + */ +typedef struct { + uint8_t datagram_type_id; /**< Message Id (CAN arbitration Id) */ + + /** + * @brief CAN Payload union + */ + union { + /** + * @brief Start message definition + */ + struct { + uint16_t node_ids; /**< Target MCU Id */ + uint32_t data_len; /**< Incoming data length (ie: Binary size) */ + } start; + + /** + * @brief Sequencing message definition + */ + struct { + uint16_t sequence_num; /**< Sequence number */ + uint32_t crc32; /**< Flash chunk CRC32 checksum */ + } sequencing; + + /** + * @brief Data message definition + */ + struct { + uint8_t *data; /**< Pointer to buffer containing message data */ + } data; + + /** + * @brief Jump application message definition + */ + struct { + uint16_t node_ids; /**< Target MCU Id */ + } jump_app; + + /** + * @brief ACK/NACK message definition + */ + struct { + uint8_t ack_status; /**< 0: ACK 1: NACK */ + uint16_t bootloader_error; /**< Bootloader error as per BootloaderError definition */ + } ack; + } payload; +} BootloaderDatagram; + +/** + * @brief Deserialize an incoming CAN message + * @details This will deserialize a CAN message and produce a datagram + * @param msg Pointer to the incoming CAN message + * @param target_nodes Pointer that will be updated with the target MCU Id + * @return Updated datagram with new data + */ +BootloaderDatagram deserialize_datagram(Boot_CanMessage *msg, uint16_t *target_nodes); + +/** + * @brief Send Bootloader ACK/NACK message over CAN + * @param ack Boolean ACK status. 0: ACK, 1: NACK + * @param error Bootloader error status for further information + */ +void send_ack_datagram(bool ack, BootloaderError error); + +/** @} */ diff --git a/projects/bootloader/inc/bootloader_crc32.h b/projects/bootloader/inc/bootloader_crc32.h new file mode 100644 index 0000000..de29ea2 --- /dev/null +++ b/projects/bootloader/inc/bootloader_crc32.h @@ -0,0 +1,52 @@ +#pragma once + +/************************************************************************************************ + * @file bootloader_crc32.h + * + * @brief Header file for the CRC32 calculations in the bootloader + * + * @date 2025-01-08 + * @author Midnight Sun Team #24 - MSXVI + ************************************************************************************************/ + +/* Standard library Headers */ +#include +#include + +/* Inter-component Headers */ + +/* Intra-component Headers */ +#include "bootloader_error.h" + +/** + * @defgroup bootloader + * @brief bootloader Firmware + * @{ + */ + +/** @brief Convert bytes to number of words for ARM32 system */ +#define BYTES_TO_WORD(bytes) (bytes / 4) + +/** + * @brief Initialize the CRC hardware component + * @return BOOTLOADER_ERROR_NONE if initialized succesfully + */ +BootloaderError boot_crc32_init(void); + +/** + * @brief Calculate a CRC32 on a provided data buffer + * @param buffer Pointer to the data buffer to be checksummed + * @param buffer_len Length of the data buffer + * @return Calculated 32-bit CRC value + */ +uint32_t boot_crc32_calculate(const uint32_t *buffer, size_t buffer_len); + +/** + * @brief Align a provided data buffer to the 4-byte boundary + * @details This prevents hard faults when writing to flash memory, ensuring all our flash is 4-byte aligned + * @param buffer Pointer to the data buffer to be aligned + * @param buffer_len Length of the data buffer + */ +void boot_align_to_32bit_words(uint8_t *buffer, size_t *buffer_len); + +/** @} */ diff --git a/projects/bootloader/inc/bootloader_error.h b/projects/bootloader/inc/bootloader_error.h new file mode 100644 index 0000000..28657ef --- /dev/null +++ b/projects/bootloader/inc/bootloader_error.h @@ -0,0 +1,52 @@ +#pragma once + +/************************************************************************************************ + * @file bootloader_error.h + * + * @brief Header file for the Bootloader errors + * + * @date 2025-01-08 + * @author Midnight Sun Team #24 - MSXVI + ************************************************************************************************/ + +/* Standard library Headers */ +#include + +/* Inter-component Headers */ + +/* Intra-component Headers */ + +/** + * @defgroup bootloader + * @brief bootloader Firmware + * @{ + */ + +/** + * @brief Bootloader error definitions + */ +typedef enum { + BOOTLOADER_ERROR_NONE = 0, /**< No error is detected */ + BOOTLOADER_OVERSIZE, /**< More data is received than expected */ + BOOTLOADER_DATA_NOT_ALIGNED, /**< Data is not 4-byte aligned as per ARM32 spec */ + BOOTLOADER_INVALID_ID, /**< Invalid authenication ID */ + BOOTLOADER_INTERNAL_ERR, /**< Unidentified internal controller error*/ + BOOTLOADER_INVALID_ARGS, /**< Invalid arguments are passed into the bootloader */ + BOOTLOADER_ERROR_UNINITIALIZED, /**< Bootloader is uninitialized */ + BOOTLOADER_FLASH_ERR, /**< Flash write or erase failure */ + BOOTLOADER_FLASH_WRITE_NOT_ALIGNED, /**< Bootloader flash write is not aligned */ + BOOTLOADER_FLASH_WRITE_OUT_OF_BOUNDS, /**< Bootloader flash write adddress is out of bounds */ + BOOTLOADER_FLASH_READ_FAILED, /**< Bootloader flash read failed*/ + BOOTLOADER_BUFFER_OVERFLOW, /**< Bootloader flash buffer has overflowed (More than 2048 bytes) */ + BOOTLOADER_CAN_INIT_ERR, /**< Bootloader CAN initialization failed */ + BOOLOADER_CAN_TRANSMISSION_ERROR, /**< CAN Bus transmission failed */ + BOOTLOADER_CAN_RECEIVE_ERROR, /**< CAN Bus receive failed */ + BOOTLOADER_INVALID_ADDRESS, /**< Attempted to write/read to an incorrect address */ + BOOTLOADER_CRC_MISMATCH_BEFORE_WRITE, /**< Bootloader has detected a CRC32 mismatch before writing to flash */ + BOOTLOADER_CRC_MISMATCH_AFTER_WRITE, /**< Bootloader has detected a CRC32 mismatch after writing to flash */ + BOOTLOADER_SEQUENCE_ERROR, /**< Bootloader has received an incorrect sequence number */ + BOOTLOADER_TIMEOUT, /**< Bootloader has timed out */ + BOOTLOADER_FLASH_MEMORY_VERIFY_FAILED /**< Bootloader flash memory verification failed. Everything is still erased */ +} BootloaderError; + +/** @} */ diff --git a/projects/bootloader/inc/bootloader_flash.h b/projects/bootloader/inc/bootloader_flash.h new file mode 100644 index 0000000..3fd6c4c --- /dev/null +++ b/projects/bootloader/inc/bootloader_flash.h @@ -0,0 +1,89 @@ +#pragma once + +/************************************************************************************************ + * @file bootloader_can_flash.h + * + * @brief Header file for the flash API in the bootloader + * + * @date 2025-01-08 + * @author Midnight Sun Team #24 - MSXVI + ************************************************************************************************/ + +/* Standard library Headers */ +#include +#include + +/* Inter-component Headers */ + +/* Intra-component Headers */ +#include "bootloader.h" +#include "bootloader_error.h" + +/** + * @defgroup bootloader + * @brief bootloader Firmware + * @{ + */ + +/** @brief 256KB of memory / 2KB per page */ +#define NUM_FLASH_PAGES 128U + +/** @brief 2KB per flash page as per datasheet */ +#define BOOTLOADER_PAGE_BYTES ((size_t)0x800) + +/** @brief ARM32 Word size is 4 bytes */ +#define BOOTLOADER_FLASH_WORD_SIZE 4U + +/** + * @brief Convert memory address to page number + * @param addr Address to be converted to page number + * @return Page number + */ +#define BOOTLOADER_ADDR_TO_PAGE(addr) (((uintptr_t)(addr) - (uintptr_t)APPLICATION_START_ADDRESS) / BOOTLOADER_PAGE_BYTES) + +/** + * @brief Convert page number to memory address + * @param page Page number + * @return 32-bit address + */ +#define BOOTLOADER_PAGE_TO_ADDR(page) ((uintptr_t)(page) * (uintptr_t)BOOTLOADER_PAGE_BYTES + (uintptr_t)APPLICATION_START_ADDRESS) + +/** + * @brief Write to flash memory + * @param address Base memory address to write to + * @param buffer Pointer to the data buffer + * @param buffer_len Length of the data buffer + * @return BOOTLOADER_ERROR_NONE if data is written succesfully + * BOOTLOADER_FLASH_WRITE_OUT_OF_BOUNDS if address is out of bounds + * BOOTLOADER_FLASH_WRITE_NOT_ALIGNED if data is not aligned + */ +BootloaderError boot_flash_write(uintptr_t address, uint8_t *buffer, size_t buffer_len); + +/** + * @brief Erase some number of flash pages + * @param start_page Initial page number to erase + * @param num_pages Number of pages to erase + * @return BOOTLOADER_ERROR_NONE if data is erased succesfully + * BOOTLOADER_FLASH_ERR if page is out of bounds OR erasing failed + */ +BootloaderError boot_flash_erase(uint8_t start_page, uint8_t num_pages); + +/** + * @brief Read from flash memory + * @param address Base memory address to read from + * @param buffer Pointer to the data buffer + * @param buffer_len Length of the data buffer + * @return BOOTLOADER_ERROR_NONE if data is read succesfully + * BOOTLOADER_FLASH_READ_FAILED if data read failed + */ +BootloaderError boot_flash_read(uintptr_t address, uint8_t *buffer, size_t buffer_len); + +/** + * @brief Verify that the flash memory is not erased + * @details This checks the entire application flash memory to ensure it is not entirely erased + * @return BOOTLOADER_ERROR_NONE if the memomry is valid + * BOOTLOADER_FLASH_ERR if the memory is corrupt + */ +BootloaderError boot_verify_flash_memory(void); + +/** @} */ diff --git a/projects/bootloader/inc/bootloader_mpu.h b/projects/bootloader/inc/bootloader_mpu.h new file mode 100644 index 0000000..b37c1a1 --- /dev/null +++ b/projects/bootloader/inc/bootloader_mpu.h @@ -0,0 +1,32 @@ +#pragma once + +/************************************************************************************************ + * @file bootloader_mpu.h + * + * @brief Header file for the Memory protection unit for the bootloader + * + * @date 2025-01-08 + * @author Midnight Sun Team #24 - MSXVI + ************************************************************************************************/ + +/* Standard library Headers */ +#include + +/* Inter-component Headers */ + +/* Intra-component Headers */ + +/** + * @defgroup bootloader + * @brief bootloader Firmware + * @{ + */ + +/** + * @brief Protect the bootloader memory provided the bootloaders start address and size + * @param bootloader_start_address The start address defined in the linkerscript + * @param bootloader_size The bootloader size defined in the linkerscript + */ +void protect_bootloader_memory(uint32_t bootloader_start_address, uint32_t bootloader_size); + +/** @} */ diff --git a/projects/bootloader/src/bootloader.c b/projects/bootloader/src/bootloader.c new file mode 100644 index 0000000..27b6017 --- /dev/null +++ b/projects/bootloader/src/bootloader.c @@ -0,0 +1,281 @@ +/************************************************************************************************ + * @file bootloader.c + * + * @brief Source file for the bootloader + * + * @date 2025-01-08 + * @author Midnight Sun Team #24 - MSXVI + ************************************************************************************************/ + +/* Standard library Headers */ +#include + +/* Inter-component Headers */ + +/* Intra-component Headers */ +#include "bootloader.h" +#include "bootloader_can_datagram.h" +#include "bootloader_crc32.h" +#include "bootloader_flash.h" + +// Store CAN traffic in 2048 byte buffer to write to flash +static uint8_t flash_buffer[BOOTLOADER_PAGE_BYTES]; + +static BootloaderDatagram datagram; +static BootloaderStateData prv_bootloader = { .state = BOOTLOADER_UNINITIALIZED, .error = BOOTLOADER_ERROR_NONE, .first_byte_received = false }; + +BootloaderError bootloader_init() { + prv_bootloader.bytes_written = 0; + prv_bootloader.data_size = 0; + prv_bootloader.application_start = APPLICATION_START_ADDRESS; + prv_bootloader.current_write_address = APPLICATION_START_ADDRESS; + prv_bootloader.expected_sequence_number = 0; + prv_bootloader.packet_crc32 = 0; + prv_bootloader.state = BOOTLOADER_IDLE; + prv_bootloader.target_nodes = 0; + prv_bootloader.buffer_index = 0; + + boot_crc32_init(); + + return BOOTLOADER_ERROR_NONE; +} + +static BootloaderError bootloader_switch_states(const BootloaderStates new_state) { + BootloaderError return_err = BOOTLOADER_ERROR_NONE; + BootloaderStates current_state = prv_bootloader.state; + if (current_state == new_state) { + return return_err; + } + + switch (current_state) { + case BOOTLOADER_IDLE: + if (new_state == BOOTLOADER_JUMP_APP || new_state == BOOTLOADER_START || new_state == BOOTLOADER_FAULT) { + prv_bootloader.state = new_state; + } else { + return_err = BOOTLOADER_INVALID_ARGS; + prv_bootloader.state = BOOTLOADER_FAULT; + } + break; + + case BOOTLOADER_START: + if (new_state == BOOTLOADER_JUMP_APP || new_state == BOOTLOADER_WAIT_SEQUENCING || new_state == BOOTLOADER_FAULT) { + prv_bootloader.state = new_state; + } else { + return_err = BOOTLOADER_INVALID_ARGS; + prv_bootloader.state = BOOTLOADER_FAULT; + } + break; + + case BOOTLOADER_WAIT_SEQUENCING: + // Should be able to go to all states + prv_bootloader.state = new_state; + break; + + case BOOTLOADER_DATA_RECEIVE: + if (new_state == BOOTLOADER_START || new_state == BOOTLOADER_JUMP_APP || new_state == BOOTLOADER_WAIT_SEQUENCING || new_state == BOOTLOADER_FAULT) { + prv_bootloader.state = new_state; + } else { + return_err = BOOTLOADER_INVALID_ARGS; + prv_bootloader.state = BOOTLOADER_FAULT; + } + break; + + case BOOTLOADER_JUMP_APP: + if (new_state == BOOTLOADER_FAULT) { + prv_bootloader.state = new_state; + } else { + return_err = BOOTLOADER_INVALID_ARGS; + prv_bootloader.state = BOOTLOADER_FAULT; + } + break; + + case BOOTLOADER_UNINITIALIZED: + if (new_state == BOOTLOADER_IDLE) { + prv_bootloader.state = new_state; + } else { + return_err = BOOTLOADER_INVALID_ARGS; + prv_bootloader.state = BOOTLOADER_FAULT; + } + break; + + default: + return_err = BOOTLOADER_INVALID_ARGS; + prv_bootloader.state = BOOTLOADER_FAULT; + break; + } + + return return_err; +} + +static BootloaderError bootloader_handle_arbitration_id(Boot_CanMessage *msg) { + switch (msg->id) { + case BOOTLOADER_CAN_START_ID: + return bootloader_switch_states(BOOTLOADER_START); + case BOOTLOADER_CAN_SEQUENCING_ID: + return bootloader_switch_states(BOOTLOADER_WAIT_SEQUENCING); + case BOOTLOADER_CAN_FLASH_ID: + return bootloader_switch_states(BOOTLOADER_DATA_RECEIVE); + case BOOTLOADER_CAN_JUMP_APPLICATION_ID: + return bootloader_switch_states(BOOTLOADER_JUMP_APP); + default: + return BOOTLOADER_INVALID_ARGS; + } +} + +static BootloaderError bootloader_start() { + prv_bootloader.data_size = datagram.payload.start.data_len; + prv_bootloader.bytes_written = 0; + prv_bootloader.buffer_index = 0; + prv_bootloader.error = 0; + prv_bootloader.first_byte_received = false; + prv_bootloader.expected_sequence_number = 0; + + if (prv_bootloader.data_size % BOOTLOADER_FLASH_WORD_SIZE != 0) { + send_ack_datagram(NACK, BOOTLOADER_DATA_NOT_ALIGNED); + return BOOTLOADER_DATA_NOT_ALIGNED; + } + + send_ack_datagram(ACK, BOOTLOADER_ERROR_NONE); + return BOOTLOADER_ERROR_NONE; +} + +BootloaderError bootloader_jump_app() { + send_ack_datagram(ACK, BOOTLOADER_ERROR_NONE); + __asm volatile( + "LDR R0, =prv_bootloader \n" + "LDR R1, [R0] \n" + "LDR R2, [R1, #4] \n" + "BX R2 \n"); + + return BOOTLOADER_INTERNAL_ERR; +} + +static BootloaderError bootloader_wait_sequencing() { + if (prv_bootloader.expected_sequence_number == datagram.payload.sequencing.sequence_num) { + prv_bootloader.packet_crc32 = datagram.payload.sequencing.crc32; + prv_bootloader.buffer_index = 0; + if (datagram.payload.sequencing.sequence_num == 0) { + prv_bootloader.first_byte_received = true; + prv_bootloader.bytes_written = 0; + prv_bootloader.current_write_address = APPLICATION_START_ADDRESS; + } + prv_bootloader.expected_sequence_number++; + } else { + send_ack_datagram(NACK, BOOTLOADER_SEQUENCE_ERROR); + return BOOTLOADER_SEQUENCE_ERROR; + } + return BOOTLOADER_ERROR_NONE; +} + +static BootloaderError bootloader_data_receive() { + BootloaderError error = BOOTLOADER_ERROR_NONE; + if (!prv_bootloader.first_byte_received) { + send_ack_datagram(NACK, BOOTLOADER_INTERNAL_ERR); + return BOOTLOADER_INTERNAL_ERR; + } + if (prv_bootloader.buffer_index >= BOOTLOADER_PAGE_BYTES) { + send_ack_datagram(NACK, BOOTLOADER_BUFFER_OVERFLOW); + return BOOTLOADER_BUFFER_OVERFLOW; + } + + size_t bytes_to_copy = 8; + if (BOOTLOADER_PAGE_BYTES - prv_bootloader.buffer_index < 8) { + bytes_to_copy = BOOTLOADER_PAGE_BYTES - prv_bootloader.buffer_index; + } + if (prv_bootloader.data_size - (prv_bootloader.buffer_index + prv_bootloader.bytes_written) < 8) { + bytes_to_copy = prv_bootloader.data_size - (prv_bootloader.buffer_index + prv_bootloader.bytes_written); + } + + memcpy(flash_buffer + prv_bootloader.buffer_index, datagram.payload.data.data, bytes_to_copy); + prv_bootloader.buffer_index += bytes_to_copy; + + if (prv_bootloader.buffer_index == BOOTLOADER_PAGE_BYTES || (prv_bootloader.bytes_written + prv_bootloader.buffer_index) >= prv_bootloader.data_size) { + uint32_t calculated_crc32 = boot_crc32_calculate((const uint32_t *)flash_buffer, BYTES_TO_WORD(prv_bootloader.buffer_index)); + if (calculated_crc32 != prv_bootloader.packet_crc32) { + send_ack_datagram(NACK, BOOTLOADER_CRC_MISMATCH_BEFORE_WRITE); + return BOOTLOADER_CRC_MISMATCH_BEFORE_WRITE; + } + + error |= boot_flash_erase(BOOTLOADER_ADDR_TO_PAGE(prv_bootloader.current_write_address), 1U); + error |= boot_flash_write(prv_bootloader.current_write_address, flash_buffer, BOOTLOADER_PAGE_BYTES); + error |= boot_flash_read(prv_bootloader.current_write_address, flash_buffer, BOOTLOADER_PAGE_BYTES); + + if (error != BOOTLOADER_ERROR_NONE) { + send_ack_datagram(NACK, error); + return error; + } + + calculated_crc32 = boot_crc32_calculate((uint32_t *)flash_buffer, BYTES_TO_WORD(prv_bootloader.buffer_index)); + if (calculated_crc32 != prv_bootloader.packet_crc32) { + send_ack_datagram(NACK, BOOTLOADER_CRC_MISMATCH_AFTER_WRITE); + return BOOTLOADER_CRC_MISMATCH_AFTER_WRITE; + } + + prv_bootloader.bytes_written += prv_bootloader.buffer_index; + prv_bootloader.current_write_address += prv_bootloader.buffer_index; + prv_bootloader.buffer_index = 0; + memset(flash_buffer, 0, sizeof(flash_buffer)); + send_ack_datagram(ACK, BOOTLOADER_ERROR_NONE); + if (prv_bootloader.bytes_written >= prv_bootloader.data_size) { + if (boot_verify_flash_memory()) { + bootloader_switch_states(BOOTLOADER_FAULT); + send_ack_datagram(NACK, BOOTLOADER_FLASH_MEMORY_VERIFY_FAILED); + } + return bootloader_jump_app(); + } + } + return BOOTLOADER_ERROR_NONE; +} + +static BootloaderError bootloader_fault() { + /* Implement code to reset the board. */ + return BOOTLOADER_INTERNAL_ERR; +} + +static BootloaderError bootloader_run_state() { + switch (prv_bootloader.state) { + case BOOTLOADER_UNINITIALIZED: + return BOOTLOADER_INVALID_ARGS; + break; + case BOOTLOADER_IDLE: + return BOOTLOADER_ERROR_NONE; + break; + case BOOTLOADER_START: + return bootloader_start(); + break; + case BOOTLOADER_WAIT_SEQUENCING: + return bootloader_wait_sequencing(); + case BOOTLOADER_DATA_RECEIVE: + return bootloader_data_receive(); + case BOOTLOADER_JUMP_APP: + return bootloader_jump_app(); + case BOOTLOADER_FAULT: + return bootloader_fault(); + default: + return BOOTLOADER_INTERNAL_ERR; + } +} + +BootloaderError bootloader_run(Boot_CanMessage *msg) { + BootloaderError ret = BOOTLOADER_ERROR_NONE; + + if (prv_bootloader.state == BOOTLOADER_UNINITIALIZED) { + return BOOTLOADER_ERROR_UNINITIALIZED; + } + + datagram = deserialize_datagram(msg, &prv_bootloader.target_nodes); + + // if (!(prv_bootloader.target_nodes & (1 << _node_id))) { + // // Not the target node + // return BOOTLOADER_ERROR_NONE; + // } + + ret = bootloader_handle_arbitration_id(msg); + ret = bootloader_run_state(); + + if (ret != BOOTLOADER_ERROR_NONE) { + bootloader_switch_states(BOOTLOADER_FAULT); + } + + return ret; +} diff --git a/projects/bootloader/src/bootloader_can.c b/projects/bootloader/src/bootloader_can.c new file mode 100644 index 0000000..5286d0c --- /dev/null +++ b/projects/bootloader/src/bootloader_can.c @@ -0,0 +1,27 @@ +/************************************************************************************************ + * @file bootloader_can.c + * + * @brief Source file for CAN communication in the bootloader + * + * @date 2025-01-08 + * @author Midnight Sun Team #24 - MSXVI + ************************************************************************************************/ + +/* Standard library Headers */ + +/* Inter-component Headers */ + +/* Intra-component Headers */ +#include "bootloader_can.h" + +BootloaderError boot_can_init(const Boot_CanSettings *settings) { + return BOOTLOADER_ERROR_NONE; +} + +BootloaderError boot_can_transmit(uint32_t id, bool extended, const uint8_t *data, size_t len) { + return BOOTLOADER_ERROR_NONE; +} + +BootloaderError boot_can_receive(Boot_CanMessage *msg) { + return BOOTLOADER_ERROR_NONE; +} diff --git a/projects/bootloader/src/bootloader_can_datagram.c b/projects/bootloader/src/bootloader_can_datagram.c new file mode 100644 index 0000000..937872d --- /dev/null +++ b/projects/bootloader/src/bootloader_can_datagram.c @@ -0,0 +1,23 @@ +/************************************************************************************************ + * @file bootloader_can_datagram.c + * + * @brief Source file for the CAN datagram in the bootloader + * + * @date 2025-01-08 + * @author Midnight Sun Team #24 - MSXVI + ************************************************************************************************/ + +/* Standard library Headers */ + +/* Inter-component Headers */ + +/* Intra-component Headers */ +#include "bootloader_can_datagram.h" + +BootloaderDatagram deserialize_datagram(Boot_CanMessage *msg, uint16_t *target_nodes) { + BootloaderDatagram ret_datagram = { 0 }; + + return ret_datagram; +} + +void send_ack_datagram(bool ack, BootloaderError error) {} diff --git a/projects/bootloader/src/bootloader_crc32.c b/projects/bootloader/src/bootloader_crc32.c new file mode 100644 index 0000000..016a0c8 --- /dev/null +++ b/projects/bootloader/src/bootloader_crc32.c @@ -0,0 +1,25 @@ +/************************************************************************************************ + * @file bootloader_crc32.c + * + * @brief Source file for the CRC32 calculations in the bootloader + * + * @date 2025-01-08 + * @author Midnight Sun Team #24 - MSXVI + ************************************************************************************************/ + +/* Standard library Headers */ + +/* Inter-component Headers */ + +/* Intra-component Headers */ +#include "bootloader_crc32.h" + +BootloaderError boot_crc32_init(void) { + return BOOTLOADER_ERROR_NONE; +} + +uint32_t boot_crc32_calculate(const uint32_t *buffer, size_t buffer_len) { + return 0; +} + +void boot_align_to_32bit_words(uint8_t *buffer, size_t *buffer_len) {} diff --git a/projects/bootloader/src/bootloader_flash.c b/projects/bootloader/src/bootloader_flash.c new file mode 100644 index 0000000..6a2133c --- /dev/null +++ b/projects/bootloader/src/bootloader_flash.c @@ -0,0 +1,31 @@ +/************************************************************************************************ + * @file bootloader_flash.c + * + * @brief Source file for the flash API in the bootloader + * + * @date 2025-01-08 + * @author Midnight Sun Team #24 - MSXVI + ************************************************************************************************/ + +/* Standard library Headers */ + +/* Inter-component Headers */ + +/* Intra-component Headers */ +#include "bootloader_flash.h" + +BootloaderError boot_flash_write(uintptr_t address, uint8_t *buffer, size_t buffer_len) { + return BOOTLOADER_ERROR_NONE; +} + +BootloaderError boot_flash_erase(uint8_t start_page, uint8_t num_pages) { + return BOOTLOADER_ERROR_NONE; +} + +BootloaderError boot_flash_read(uintptr_t address, uint8_t *buffer, size_t buffer_len) { + return BOOTLOADER_ERROR_NONE; +} + +BootloaderError boot_verify_flash_memory(void) { + return BOOTLOADER_ERROR_NONE; +} diff --git a/projects/bootloader/src/bootloader_mpu.c b/projects/bootloader/src/bootloader_mpu.c new file mode 100644 index 0000000..c49aaf5 --- /dev/null +++ b/projects/bootloader/src/bootloader_mpu.c @@ -0,0 +1,17 @@ +/************************************************************************************************ + * @file bootloader_mpu.c + * + * @brief Source file for the Memory protection unit for the bootloader + * + * @date 2025-01-08 + * @author Midnight Sun Team #24 - MSXVI + ************************************************************************************************/ + +/* Standard library Headers */ + +/* Inter-component Headers */ + +/* Intra-component Headers */ +#include "bootloader_mpu.h" + +void protect_bootloader_memory(uint32_t bootloader_start_address, uint32_t bootloader_size) {} diff --git a/projects/bootloader/src/main.c b/projects/bootloader/src/main.c new file mode 100644 index 0000000..632ad92 --- /dev/null +++ b/projects/bootloader/src/main.c @@ -0,0 +1,23 @@ +/************************************************************************************************ + * @file main.c + * + * @brief Main file for bootloader + * + * @date 2025-01-08 + * @author Midnight Sun Team #24 - MSXVI + ************************************************************************************************/ + +/* Standard library Headers */ +#include + +/* Inter-component Headers */ + +/* Intra-component Headers */ +#include "bootloader.h" + +int main() { + while (true) { + } + + return 0; +} diff --git a/projects/bootloader/test/test_bootloader.c b/projects/bootloader/test/test_bootloader.c new file mode 100644 index 0000000..e475380 --- /dev/null +++ b/projects/bootloader/test/test_bootloader.c @@ -0,0 +1,26 @@ +/************************************************************************************************ + * @file test_bootloader.c + * + * @brief Test file for bootloader + * + * @date 2025-01-08 + * @author Midnight Sun Team #24 - MSXVI + ************************************************************************************************/ + +/* Standard library Headers */ +#include +#include + +/* Inter-component Headers */ +#include "test_helpers.h" +#include "unity.h" + +/* Intra-component Headers */ + +void setup_test(void) {} + +void teardown_test(void) {} + +void test_example(void) { + TEST_ASSERT_TRUE(true); +} diff --git a/projects/can_communication/src/main.c b/projects/can_communication/src/main.c index c834b61..1680c51 100644 --- a/projects/can_communication/src/main.c +++ b/projects/can_communication/src/main.c @@ -14,6 +14,7 @@ #include "gpio.h" #include "log.h" #include "mcu.h" +#include "system_can.h" #include "tasks.h" /* Intra-component Headers */ @@ -21,7 +22,7 @@ static CanStorage s_can_storage = { 0 }; const CanSettings can_settings = { - .device_id = 0U, + .device_id = SYSTEM_CAN_DEVICE_CAN_COMMUNICATION, .bitrate = CAN_HW_BITRATE_500KBPS, .tx = { GPIO_PORT_A, 12 }, .rx = { GPIO_PORT_A, 11 }, diff --git a/scons/build.scons b/scons/build.scons index 6d58f7f..838b0c5 100644 --- a/scons/build.scons +++ b/scons/build.scons @@ -22,7 +22,7 @@ SMOKE_DIR = ROOT.Dir('smoke') LIB_BIN_DIR = BIN_DIR.Dir('libraries') LIBRARIES_INC_DIR = LIB_DIR.Dir("ms-common").Dir("inc") -AUTOGEN_DIR = LIB_DIR.Dir("autogen") +AUTOGEN_DIR = ROOT.Dir("autogen") def src(path): # return all source files within a path @@ -55,12 +55,6 @@ def get_lib_deps(entry): autogen_sources = list(Path(AUTOGEN_DIR.abspath).rglob("*")) autogen_sources += list(Path(ROOT.abspath).glob("can/boards/*")) -env.Command( - PY_DIR.File("can/message.py"), - autogen_sources, - f"python3 -m autogen python_can -o py/can" -) - env.Command( LIBRARIES_INC_DIR.File("system_can.h"), autogen_sources, @@ -73,18 +67,17 @@ def generate_can_files(env, project): output_files = [] project_name = Path(project).stem - can_template_dir = Path(str(AUTOGEN_DIR), "templates/can") + can_template_dir = Path(str(AUTOGEN_DIR), "templates/project_can") for template in can_template_dir.glob('*/*.jinja'): template_path = template.relative_to(can_template_dir) output_name = str(template_path) \ - .replace(r"{{board}}", project_name) \ + .replace(r"{{project_name}}", project_name) \ .replace(".jinja", "") output_files.append(project_can_dir.File(output_name)) - # print([f.path for f in output_files]) env.Command(output_files, autogen_sources, - f"python3 -m autogen can -o {project_can_dir.path}") + f"python3 -m autogen project_can -o {project_can_dir.path}") # Add a VariantDir that point to can folder. Create the can target specific for the project VariantDir(project_can_dir, ROOT.Dir('can'), duplicate=0) @@ -132,16 +125,20 @@ for entry in PROJ_DIR.glob('*') + SMOKE_DIR.glob('*'): lib_deps = get_lib_deps(entry) # SCons automagically handles object creation and linking - target = env.Program( - target=BIN_DIR.File(entry.path), - source=srcs, - CPPPATH=env['CPPPATH'] + incs + lib_incs, - # link each library twice so that dependency cycles are resolved - # See: https://stackoverflow.com/questions/45135 - LIBS=env['LIBS'] + lib_deps * 2, - LIBPATH=[LIB_BIN_DIR], - CCFLAGS=env['CCFLAGS'] + config['cflags'], - ) + + if PLATFORM == 'x86' and config['arm_only']: + print(f'Project: {entry} is only for ARM devices. Cannot build x86 version.') + else: + target = env.Program( + target=BIN_DIR.File(entry.path), + source=srcs, + CPPPATH=env['CPPPATH'] + incs + lib_incs, + # link each library twice so that dependency cycles are resolved + # See: https://stackoverflow.com/questions/45135 + LIBS=env['LIBS'] + lib_deps * 2, + LIBPATH=[LIB_BIN_DIR], + CCFLAGS=env['CCFLAGS'] + config['cflags'], + ) # .bin file only required for arm, not x86 if PLATFORM == 'arm': diff --git a/scons/common.py b/scons/common.py index d53f821..5d5b1a0 100644 --- a/scons/common.py +++ b/scons/common.py @@ -14,6 +14,7 @@ def parse_config(entry): 'mocks': {}, 'no_lint': False, "can": False, + "arm_only": False } config_file = entry.File('config.json') if not config_file.exists(): diff --git a/scons/lint_format.scons b/scons/lint_format.scons index 3642ebe..3083961 100644 --- a/scons/lint_format.scons +++ b/scons/lint_format.scons @@ -16,7 +16,7 @@ PROJ_DIRS = ROOT.glob('projects/*') LIB_DIRS = ROOT.glob('libraries/*') SMOKE_DIRS = ROOT.glob('smoke/*') PY_DIRS = ROOT.glob('py/*') - +SIMULATION_DIRS = ROOT.glob('simulation/*') ########################################################### # Linting and Formatting @@ -26,10 +26,10 @@ def run_lint(target, source, env): PY_LINT_CMD = 'pylint --rcfile={}/.pylintrc'.format(ROOT.abspath) errors = 0 - # Lint C source files - if len(c_files) > 0: + # Lint C and C++ source files + if len(c_files) > 0 or len(cpp_files > 0): print(f'\nLinting *.[ch] files ...') - errors += subprocess.run(f'{C_LINT_CMD} {c_files_str}', + errors += subprocess.run(f'{C_LINT_CMD} {c_files_str} {cpp_files_str}', shell=True).returncode # Lint Python files @@ -54,9 +54,9 @@ def run_format(target, source, env): PY_FORMAT_CMD = 'autopep8 {} -i'.format(AUTOPEP8_CONFIG) # Format C source files - if len(c_files) > 0: + if len(c_files) > 0 or len(cpp_files > 0): print(f'\nFormatting *.[ch] files ...') - subprocess.run(f'{C_FORMAT_CMD} {c_files_str}', shell=True) + subprocess.run(f'{C_FORMAT_CMD} {c_files_str} {cpp_files_str}', shell=True) # Format Python source files if len(py_files) > 0: @@ -67,6 +67,7 @@ def run_format(target, source, env): # Retrieve files to lint - returns a tuple (c_lint_files, py_lint_files) +cpp_files = [] c_files = [] py_files = [] # Get directories to lint based on PROJECT/LIBRARY args. @@ -75,6 +76,7 @@ py_files = [] if TARGET: target_dirs = [ROOT.Dir(TARGET)] else: + # TODO: add this to the end of this '+ SIMULATION_DIRS' target_dirs = PROJ_DIRS + LIB_DIRS + SMOKE_DIRS + PY_DIRS for dir in target_dirs: @@ -82,9 +84,11 @@ for dir in target_dirs: if config.get('no_lint'): # Avoid linting/formatting external libraries continue + cpp_files += glob.glob(f'{dir.abspath}/**/*.[ch]pp', recursive=True) + glob.glob(f'{dir.abspath}/**/*.cc', recursive=True) c_files += glob.glob(f'{dir.abspath}/**/*.[ch]', recursive=True) py_files += glob.glob(f'{dir.abspath}/**/*.py', recursive=True) +cpp_files_str = ' '.join(cpp_files) c_files_str = ' '.join(c_files) py_files_str = ' '.join(py_files) diff --git a/simulation/.placeholder b/simulation/.placeholder deleted file mode 100644 index e69de29..0000000 diff --git a/simulation/Simulation_JSON/README.md b/simulation/Simulation_JSON/README.md new file mode 100644 index 0000000..243846e --- /dev/null +++ b/simulation/Simulation_JSON/README.md @@ -0,0 +1 @@ +# This folder contains autogenerated JSON files \ No newline at end of file diff --git a/simulation/client/app/inc/app.h b/simulation/client/app/inc/app.h new file mode 100644 index 0000000..f2eaa0a --- /dev/null +++ b/simulation/client/app/inc/app.h @@ -0,0 +1,38 @@ +#pragma once + +/************************************************************************************************ + * @file app.h + * + * @brief Header file defining the Application for the client + * + * @date 2025-01-04 + * @author Aryan Kashem + ************************************************************************************************/ + +/* Standard library Headers */ + +/* Inter-component Headers */ + +/* Intra-component Headers */ +#include "gpio_manager.h" + +/** + * @defgroup ClientAppMain + * @brief Client Application Main Interface + * @{ + */ + +#ifndef USE_NETWORK_TIME_PROTOCOL +/** @brief Define to use network-time-protocol synchronization. 0: Disabled 1: Enabled */ +#define USE_NETWORK_TIME_PROTOCOL 0U +#endif + +/** @brief Default project name to be used for the Metadata */ +#define DEFAULT_PROJECT_NAME "Default Project" + +/** @brief Default hardware model to be used for the Metadata */ +#define DEFAULT_HARDWARE_MODEL "MS16.0.0" + +extern GpioManager clientGpioManager; /**< Global GPIO Manager */ + +/** @} */ diff --git a/simulation/client/app/inc/app_callback.h b/simulation/client/app/inc/app_callback.h new file mode 100644 index 0000000..8350098 --- /dev/null +++ b/simulation/client/app/inc/app_callback.h @@ -0,0 +1,43 @@ +#pragma once + +/************************************************************************************************ + * @file app_callback.h + * + * @brief Header file defining the Application Callback functions for the client + * + * @date 2025-01-04 + * @author Aryan Kashem + ************************************************************************************************/ + +/* Standard library Headers */ +#include + +/* Inter-component Headers */ +#include "client.h" + +/* Intra-component Headers */ + +/** + * @defgroup ClientAppCallback + * @brief Application Callback functions for the Client + * @{ + */ + +/** + * @brief Handle receiving new client messages + * @details This shall handle all CommandCodes received + * This shall branch out to the GpioManager, I2CManager, SPIManager or InterruptManager + * based on the CommandCode + * @param client Pointer to the connected client instance + * @param message String message value that has been received + */ +void applicationMessageCallback(Client *client, std::string &message); + +/** + * @brief Handle connecting to the server + * @details This shall notify the user about a new server connection + * @param client Pointer to the connected client instance + */ +void applicationConnectCallback(Client *client); + +/** @} */ diff --git a/simulation/client/app/inc/gpio_manager.h b/simulation/client/app/inc/gpio_manager.h new file mode 100644 index 0000000..c6a586e --- /dev/null +++ b/simulation/client/app/inc/gpio_manager.h @@ -0,0 +1,115 @@ +#pragma once + +/************************************************************************************************ + * @file gpio_manager.h + * + * @brief Header file defining the Client GpioManager class + * + * @date 2025-01-04 + * @author Aryan Kashem + ************************************************************************************************/ + +/* Standard library Headers */ +#include +#include + +#include + +/* Inter-component Headers */ +#include "gpio_datagram.h" + +/* Intra-component Headers */ + +/** + * @defgroup ClientGpioManager + * @brief GpioManager for the Client + * @{ + */ + +/** + * @class GpioManager + * @brief Class that manages receiving and transmitting Gpio commands and JSON logging + * @details This class is responsible for handling and transmitting serialized messages for reading pin modes, + * alternate functions and states. It shall support reading all pins or individual pins + * The class shall perform procedures as soon as commands are queued by the client + */ +class GpioManager { + private: + Datagram::Gpio m_gpioDatagram; /**< Datagram class to serialize/deserialize commands */ + + public: + /** + * @brief Constructs a GpioManager object + * @details Default constructor + */ + GpioManager() = default; + + /** + * @brief Sets a Gpio Pin state given the data payload + * @details This function shall be called upon receiving a pin-specific payload + * This function is not responsible for handling update errors + * @param payload Message data payload to be parsed + */ + void setGpioPinState(std::string &payload); + + /** + * @brief Sets all Gpio Pin state given the data payload + * @details This function shall be called upon receiving a payload containing all pin data + * This function is not responsible for handling update errors + * @param payload Message data payload to be parsed + */ + void setGpioAllStates(std::string &payload); + + /** + * @brief Process a get Gpio Pin state command given the data payload + * @details This function shall be called upon receiving a pin-specific payload + * This function is not responsible for handling update errors + * @param payload Message data payload to be parsed + * @return Fully serialized data payload to be transmitted in response to the server + */ + std::string processGpioPinState(std::string &payload); + + /** + * @brief Process a get all Gpio Pin states command + * @details This function shall be called upon receiving a payload containing all pin data + * This function is not responsible for handling update errors + * @return Fully serialized data payload to be transmitted in response to the server + */ + std::string processGpioAllStates(); + + /** + * @brief Process a get Gpio Pin mode command given the data payload + * @details This function shall be called upon receiving a pin-specific payload + * This function is not responsible for handling update errors + * @param payload Message data payload to be parsed + * @return Fully serialized data payload to be transmitted in response to the server + */ + std::string processGpioPinMode(std::string &payload); + + /** + * @brief Process a get all Gpio Pin modes command + * @details This function shall be called upon receiving a payload containing all pin data + * This function is not responsible for handling update errors + * @return Fully serialized data payload to be transmitted in response to the server + */ + std::string processGpioAllModes(); + + /** + * @brief Process a get Gpio Pin alternate function command given the data payload + * @details This function shall be called upon receiving a pin-specific payload + * This function is not responsible for handling update errors + * @param payload Message data payload to be parsed + * @return Fully serialized data payload to be transmitted in response to the server + */ + std::string processGpioPinAltFunction(std::string &payload); + + /** + * @brief Process a get all Gpio Pin alternate functions command + * @details This function shall be called upon receiving a payload containing all pin data + * This function is not responsible for handling update errors + * @return Fully serialized data payload to be transmitted in response to the server + */ + std::string processGpioAllAltFunctions(); +}; + +/** @} */ diff --git a/simulation/client/app/inc/i2c_manager.h b/simulation/client/app/inc/i2c_manager.h new file mode 100644 index 0000000..ea6a7e2 --- /dev/null +++ b/simulation/client/app/inc/i2c_manager.h @@ -0,0 +1,43 @@ +#pragma once + +/************************************************************************************************ + * @file i2c_manager.h + * + * @brief Header file defining the Client I2CManager class + * + * @date 2025-01-04 + * @author Aryan Kashem + ************************************************************************************************/ + +/* Standard library Headers */ +#include +#include + +/* Inter-component Headers */ +#include "i2c_datagram.h" + +/* Intra-component Headers */ + +/** + * @defgroup ClientI2CManager + * @brief I2CManager for the Client + * @{ + */ + +/** + * @class ClientI2CManager + * @brief Class that manages receiving and transmitting I2C commands and JSON logging + * @details This class is responsible for handling and transmitting serialized messages for setting/retrieving RX/TX data buffers + * The class shall provide an interface to control I2C transactions at the driver-level + */ +class I2CManager { + private: + public: + /** + * @brief Constructs a I2CManager object + * @details Default constructor + */ + I2CManager() = default; +}; + +/** @} */ diff --git a/simulation/client/app/inc/spi_manager.h b/simulation/client/app/inc/spi_manager.h new file mode 100644 index 0000000..7f95530 --- /dev/null +++ b/simulation/client/app/inc/spi_manager.h @@ -0,0 +1,43 @@ +#pragma once + +/************************************************************************************************ + * @file spi_manager.h + * + * @brief Header file defining the Client SPIManager class + * + * @date 2025-01-04 + * @author Aryan Kashem + ************************************************************************************************/ + +/* Standard library Headers */ +#include +#include + +/* Inter-component Headers */ +#include "spi_datagram.h" + +/* Intra-component Headers */ + +/** + * @defgroup ClientSPIManager + * @brief SPIManager for the Client + * @{ + */ + +/** + * @class ClientSPIManager + * @brief Class that manages receiving and transmitting SPI commands and JSON logging + * @details This class is responsible for handling and transmitting serialized messages for setting/retrieving RX/TX data buffers + * The class shall provide an interface to control SPI transactions at the driver-level + */ +class SPIManager { + private: + public: + /** + * @brief Constructs a SPIManager object + * @details Default constructor + */ + SPIManager() = default; +}; + +/** @} */ diff --git a/simulation/client/app/src/app_callback.cc b/simulation/client/app/src/app_callback.cc new file mode 100644 index 0000000..5a13730 --- /dev/null +++ b/simulation/client/app/src/app_callback.cc @@ -0,0 +1,77 @@ +/************************************************************************************************ + * @file app_callback.cc + * + * @brief Source file defining the Application Callbacks for the client + * + * @date 2025-01-04 + * @author Aryan Kashem + ************************************************************************************************/ + +/* Standard library Headers */ + +/* Inter-component Headers */ +#include "command_code.h" +#include "json_manager.h" +#include "metadata.h" + +/* Intra-component Headers */ +#include "app.h" +#include "app_callback.h" + +std::string projectName = DEFAULT_PROJECT_NAME; +std::string hardwareModel = DEFAULT_HARDWARE_MODEL; + +void applicationMessageCallback(Client *client, std::string &message) { + auto [commandCode, payload] = decodeCommand(message); + + switch (commandCode) { + case CommandCode::METADATA: { + /* Future expansion if the server needs to send the client some metadata? */ + break; + } + case CommandCode::GPIO_SET_PIN_STATE: { + clientGpioManager.setGpioPinState(payload); + break; + } + case CommandCode::GPIO_SET_ALL_STATES: { + clientGpioManager.setGpioAllStates(payload); + break; + } + case CommandCode::GPIO_GET_PIN_STATE: { + client->sendMessage(clientGpioManager.processGpioPinState(payload)); + break; + } + case CommandCode::GPIO_GET_ALL_STATES: { + client->sendMessage(clientGpioManager.processGpioAllStates()); + break; + } + case CommandCode::GPIO_GET_PIN_MODE: { + client->sendMessage(clientGpioManager.processGpioPinMode(payload)); + break; + } + case CommandCode::GPIO_GET_ALL_MODES: { + client->sendMessage(clientGpioManager.processGpioAllModes()); + break; + } + case CommandCode::GPIO_GET_PIN_ALT_FUNCTION: { + client->sendMessage(clientGpioManager.processGpioPinAltFunction(payload)); + break; + } + case CommandCode::GPIO_GET_ALL_ALT_FUNCTIONS: { + client->sendMessage(clientGpioManager.processGpioAllAltFunctions()); + break; + } + default: { + break; + } + } +} + +void applicationConnectCallback(Client *client) { + std::cout << "Connected :-)" << std::endl; + + Datagram::Metadata::Payload initialData = { .projectName = projectName, .projectStatus = "RUNNING", .hardwareModel = hardwareModel }; + Datagram::Metadata projectMetadata(initialData); + + client->sendMessage(projectMetadata.serialize()); +} diff --git a/simulation/client/app/src/gpio_manager.cc b/simulation/client/app/src/gpio_manager.cc new file mode 100644 index 0000000..8ee9a9f --- /dev/null +++ b/simulation/client/app/src/gpio_manager.cc @@ -0,0 +1,184 @@ +/************************************************************************************************ + * @file gpio_manager.cc + * + * @brief Source file defining the GpioManager class for the client + * + * @date 2025-01-04 + * @author Aryan Kashem + ************************************************************************************************/ + +/* Standard library Headers */ +#include +#include +#include + +/* Inter-component Headers */ +extern "C" { +#include "gpio.h" +} + +#include "command_code.h" + +/* Intra-component Headers */ +#include "app.h" +#include "gpio_manager.h" + +void GpioManager::setGpioPinState(std::string &payload) { + m_gpioDatagram.deserialize(payload); + + const uint8_t *receivedData = m_gpioDatagram.getBuffer(); + GpioAddress pinAddress = { .port = static_cast(m_gpioDatagram.getGpioPort()), .pin = m_gpioDatagram.getGpioPin() }; + + gpio_set_state(&pinAddress, static_cast(receivedData[0U])); +} + +void GpioManager::setGpioAllStates(std::string &payload) { + m_gpioDatagram.deserialize(payload); + + const uint8_t *receivedData = m_gpioDatagram.getBuffer(); + + for (uint8_t i = 0U; i < static_cast(Datagram::Gpio::Port::NUM_GPIO_PORTS) * Datagram::Gpio::PINS_PER_PORT; i++) { + GpioAddress pinAddress = { .port = static_cast(i / 16U), .pin = i % 16U }; + gpio_set_state(&pinAddress, static_cast(receivedData[0U])); + } +} + +std::string GpioManager::processGpioPinState(std::string &payload) { + m_gpioDatagram.deserialize(payload); + + GpioAddress pinAddress = { .port = static_cast(m_gpioDatagram.getGpioPort()), .pin = m_gpioDatagram.getGpioPin() }; + uint8_t pinState = static_cast(gpio_peek_state(&pinAddress)); + + m_gpioDatagram.clearBuffer(); + m_gpioDatagram.setBuffer(&pinState, sizeof(pinState)); + + return m_gpioDatagram.serialize(CommandCode::GPIO_GET_PIN_STATE); +} + +std::string GpioManager::processGpioAllStates() { + std::vector gpioStateBitsetArray; + + /* Round up the number of blocks by adding the divisor - 1 */ + constexpr uint8_t numBlocks = ((Datagram::Gpio::PINS_PER_PORT * static_cast(Datagram::Gpio::Port::NUM_GPIO_PORTS)) + 32U - 1U) / 32U; + gpioStateBitsetArray.resize(numBlocks, 0U); + + /* Simulation only supports reading port A and B */ + for (uint8_t i = 0U; i < static_cast(Datagram::Gpio::Port::NUM_GPIO_PORTS) * Datagram::Gpio::PINS_PER_PORT; i++) { + size_t blockIndex = i / 32U; + size_t bitPosition = i % 32U; + GpioAddress pinAddress = { .port = static_cast(i / 16U), .pin = i % 16U }; + GpioState pinState = gpio_peek_state(&pinAddress); + gpioStateBitsetArray[blockIndex] |= (static_cast(pinState << bitPosition)); + } + m_gpioDatagram.setGpioPort(Datagram::Gpio::Port::NUM_GPIO_PORTS); + m_gpioDatagram.setGpioPin(Datagram::Gpio::PINS_PER_PORT); + + std::vector byteArray; + for (uint32_t value : gpioStateBitsetArray) { + byteArray.push_back(static_cast(value & 0xFF)); + byteArray.push_back(static_cast((value >> 8) & 0xFF)); + byteArray.push_back(static_cast((value >> 16) & 0xFF)); + byteArray.push_back(static_cast((value >> 24) & 0xFF)); + } + + m_gpioDatagram.clearBuffer(); + m_gpioDatagram.setBuffer(byteArray.data(), byteArray.size()); + + return m_gpioDatagram.serialize(CommandCode::GPIO_GET_ALL_STATES); +} + +std::string GpioManager::processGpioPinMode(std::string &payload) { + m_gpioDatagram.deserialize(payload); + + GpioAddress pinAddress = { .port = static_cast(m_gpioDatagram.getGpioPort()), .pin = m_gpioDatagram.getGpioPin() }; + uint8_t pinMode = static_cast(gpio_peek_mode(&pinAddress)); + + m_gpioDatagram.clearBuffer(); + m_gpioDatagram.setBuffer(&pinMode, sizeof(pinMode)); + + return m_gpioDatagram.serialize(CommandCode::GPIO_GET_PIN_MODE); +} + +std::string GpioManager::processGpioAllModes() { + std::vector gpioStateBitsetArray; + + /* Calculate number of 32-bit blocks needed (8 pins per 32-bit block with 4 bits per pin) */ + constexpr uint8_t numBlocks = (Datagram::Gpio::PINS_PER_PORT * static_cast(Datagram::Gpio::Port::NUM_GPIO_PORTS) + 8U - 1U) / 8U; + gpioStateBitsetArray.resize(numBlocks, 0U); + + /* Simulation only supports reading port A and B */ + for (uint8_t i = 0U; i < static_cast(Datagram::Gpio::Port::NUM_GPIO_PORTS) * Datagram::Gpio::PINS_PER_PORT; i++) { + size_t blockIndex = (i / 8U); /* 4 bits per pin so there is only 8 pins per block */ + size_t bitPosition = (i % 8U) * 4U; /* Multiply by 4 to account for 4 bits per pin */ + + GpioAddress pinAddress = { .port = static_cast(i / 16U), .pin = i % 16U }; + GpioMode pinMode = gpio_peek_mode(&pinAddress); + + gpioStateBitsetArray[blockIndex] &= ~(0xFU << bitPosition); + gpioStateBitsetArray[blockIndex] |= (static_cast(pinMode) << bitPosition); + } + + m_gpioDatagram.setGpioPort(Datagram::Gpio::Port::NUM_GPIO_PORTS); + m_gpioDatagram.setGpioPin(Datagram::Gpio::PINS_PER_PORT); + + std::vector byteArray; + for (uint32_t value : gpioStateBitsetArray) { + byteArray.push_back(static_cast(value & 0xFF)); + byteArray.push_back(static_cast((value >> 8) & 0xFF)); + byteArray.push_back(static_cast((value >> 16) & 0xFF)); + byteArray.push_back(static_cast((value >> 24) & 0xFF)); + } + + m_gpioDatagram.clearBuffer(); + m_gpioDatagram.setBuffer(byteArray.data(), byteArray.size()); + + return m_gpioDatagram.serialize(CommandCode::GPIO_GET_ALL_MODES); +} + +std::string GpioManager::processGpioPinAltFunction(std::string &payload) { + m_gpioDatagram.deserialize(payload); + + GpioAddress pinAddress = { .port = static_cast(m_gpioDatagram.getGpioPort()), .pin = m_gpioDatagram.getGpioPin() }; + uint8_t pinAltFunction = static_cast(gpio_peek_alt_function(&pinAddress)); + + m_gpioDatagram.clearBuffer(); + m_gpioDatagram.setBuffer(&pinAltFunction, sizeof(pinAltFunction)); + + return m_gpioDatagram.serialize(CommandCode::GPIO_GET_PIN_ALT_FUNCTION); +} + +std::string GpioManager::processGpioAllAltFunctions() { + std::vector gpioAltFunctionBitsetArray; + + /* Calculate number of 32-bit blocks needed (8 pins per 32-bit block with 4 bits per pin) */ + constexpr uint8_t numBlocks = (Datagram::Gpio::PINS_PER_PORT * static_cast(Datagram::Gpio::Port::NUM_GPIO_PORTS) + 8U - 1U) / 8U; + gpioAltFunctionBitsetArray.resize(numBlocks, 0U); + + /* Simulation only supports reading port A and B */ + for (uint8_t i = 0U; i < static_cast(Datagram::Gpio::Port::NUM_GPIO_PORTS) * Datagram::Gpio::PINS_PER_PORT; i++) { + size_t blockIndex = (i / 8U); /* 4 bits per pin so there is only 8 pins per block */ + size_t bitPosition = (i % 8U) * 4U; /* Multiply by 4 to account for 4 bits per pin */ + + GpioAddress pinAddress = { .port = static_cast(i / 16U), .pin = i % 16U }; + GpioAlternateFunctions pinFunction = gpio_peek_alt_function(&pinAddress); + + gpioAltFunctionBitsetArray[blockIndex] &= ~(0xFU << bitPosition); + gpioAltFunctionBitsetArray[blockIndex] |= (static_cast(pinFunction) << bitPosition); + } + + m_gpioDatagram.setGpioPort(Datagram::Gpio::Port::NUM_GPIO_PORTS); + m_gpioDatagram.setGpioPin(Datagram::Gpio::PINS_PER_PORT); + + std::vector byteArray; + for (uint32_t value : gpioAltFunctionBitsetArray) { + byteArray.push_back(static_cast(value & 0xFF)); + byteArray.push_back(static_cast((value >> 8) & 0xFF)); + byteArray.push_back(static_cast((value >> 16) & 0xFF)); + byteArray.push_back(static_cast((value >> 24) & 0xFF)); + } + + m_gpioDatagram.clearBuffer(); + m_gpioDatagram.setBuffer(byteArray.data(), byteArray.size()); + + return m_gpioDatagram.serialize(CommandCode::GPIO_GET_ALL_ALT_FUNCTIONS); +} diff --git a/simulation/client/app/src/i2c_manager.cc b/simulation/client/app/src/i2c_manager.cc new file mode 100644 index 0000000..3cb625c --- /dev/null +++ b/simulation/client/app/src/i2c_manager.cc @@ -0,0 +1,15 @@ +/************************************************************************************************ + * @file i2c_manager.cc + * + * @brief Source file defining the I2CManager class for the client + * + * @date 2025-01-04 + * @author Aryan Kashem + ************************************************************************************************/ + +/* Standard library Headers */ + +/* Inter-component Headers */ + +/* Intra-component Headers */ +#include "i2c_manager.h" diff --git a/simulation/client/app/src/main.cc b/simulation/client/app/src/main.cc new file mode 100644 index 0000000..76d19c6 --- /dev/null +++ b/simulation/client/app/src/main.cc @@ -0,0 +1,39 @@ +/************************************************************************************************ + * @file main.cc + * + * @brief Main source file + * + * @date 2025-01-04 + * @author Aryan Kashem + ************************************************************************************************/ + +/* Standard library Headers */ +#include + +/* Inter-component Headers */ +#include "client.h" +#include "ntp_client.h" + +/* Intra-component Headers */ +#include "app.h" +#include "app_callback.h" +#include "gpio_manager.h" + +GpioManager clientGpioManager; + +int main(int argc, char **argv) { + std::cout << "Running Client" << std::endl; + + Client serverClient("127.0.0.1", 1024, applicationMessageCallback, applicationConnectCallback); + serverClient.connectServer(); + +#if USE_NETWORK_TIME_PROTOCOL == 1U + NTPClient ntpClient; + ntpClient.startSynchronization("127.0.0.1"); +#endif + + int n; + std::cin >> n; + + return 0; +} diff --git a/simulation/client/app/src/spi_manager.cc b/simulation/client/app/src/spi_manager.cc new file mode 100644 index 0000000..cbbc9ec --- /dev/null +++ b/simulation/client/app/src/spi_manager.cc @@ -0,0 +1,15 @@ +/************************************************************************************************ + * @file spi_manager.cc + * + * @brief Source file defining the SPIManager class for the client + * + * @date 2025-01-04 + * @author Aryan Kashem + ************************************************************************************************/ + +/* Standard library Headers */ + +/* Inter-component Headers */ + +/* Intra-component Headers */ +#include "spi_manager.h" diff --git a/simulation/client/utils/inc/client.h b/simulation/client/utils/inc/client.h new file mode 100644 index 0000000..55a46e0 --- /dev/null +++ b/simulation/client/utils/inc/client.h @@ -0,0 +1,127 @@ +#pragma once + +/************************************************************************************************ + * @file client.h + * + * @brief Header file defining the Client class + * + * @date 2025-01-04 + * @author Aryan Kashem + ************************************************************************************************/ + +/* Standard library Headers */ +#include +#include +#include +#include + +/* Inter-component Headers */ +#include +#include +#include +#include +#include +#include + +/* Intra-component Headers */ + +/** + * @defgroup Client_Utils + * @brief Client Utilities and Infrastructure + * @{ + */ + +/** + * @class Client + * @brief Class that represents a client that connects to a main server + * @details This class is responsible for managing the client connection, binding to the server, + * monitoring server activity, and sending messages between the server and the client + */ +class Client { + private: + /** @brief The message callback function definition */ + using messageCallback = std::function; + /** @brief The connection callback function definition */ + using connectCallback = std::function; + + static constexpr size_t MAX_BUFFER_SIZE = 256; /**< Maximum permitted read size for all clients */ + + pthread_t m_receiverThreadId; /**< Thread Id for reading incoming server data */ + pthread_t m_processMessageThreadId; /**< Thread Id for processing cached server data */ + pthread_mutex_t m_mutex; /**< Mutex to protect m_messageQueue */ + sem_t m_messageSemaphore; /**< Semaphore to signal the processMessageThread when new data is received */ + + messageCallback m_messageCallback; /**< Function pointer to store the message callback */ + connectCallback m_connectCallback; /**< Function pointer to store the connection callback */ + + std::atomic m_isConnected{ false }; /**< Boolean flag to indicate the servers status */ + + std::queue m_messageQueue; /**< Queue to cache and input-buffer received server data */ + + int m_clientSocket; /**< The clients socket FD */ + std::string m_host; /**< The servers host address (ie: 127.0.0.1) */ + int m_port; /**< The clients port to connect on */ + struct sockaddr_in m_serverAddress; /**< The servers address */ + + public: + /** + * @brief Constructs a Client object + * @details Initializes the client. The constructor sets up internal + * variables and prepares the client to accept and communicate + * This shall set the host address, listening port and callback functions + * @param host Server address to connect to + * @param port Port for the client to listen on + * @param messageCallback Function pointer to a message callback + * @param connectCallback Function pointer to a connection callback + */ + Client(const std::string &host, int port, messageCallback messageCallback, connectCallback connectCallback); + + /** + * @brief Destructs a Client object + * @details If using TCP this closes the existing socket connection + * This shall also delete the mutex, and terminate both running threads + */ + ~Client(); + + /** + * @brief Thread procedure for caching/receiving incoming server data + * @details This thread shall be blocked while no new server data is available + */ + void receiverProcedure(); + + /** + * @brief Thread procedure for processing cached/stored server data + * @details This thread shall be blocked while no new server data is available in storage + * Upon receiving a message, the message callback shall be called + */ + void processMessagesProcedure(); + + /** + * @brief Connect to the server + * @details This shall throw an exception if the server does not exist or is not accepting clients + * Upon connection, this shall spawn the receiverProcedure and processMessages threads + */ + void connectServer(); + + /** + * @brief Disconnect from the server + * @details If the server is connected, this shall safely terminate both receiverProcedure and processMessages threads + * This shall close the existing socket connection + */ + void disconnectServer(); + + /** + * @brief Get the connection status of the clienet + * @return TRUE if the client is connected + * FALSE if the client is disconnected + */ + bool isConnected() const; + + /** + * @brief Function wrapper to transmit a message + * @param message String message value to be sent + */ + void sendMessage(const std::string &message); +}; + +/** @} */ diff --git a/simulation/client/utils/inc/ntp_client.h b/simulation/client/utils/inc/ntp_client.h new file mode 100644 index 0000000..2160fb1 --- /dev/null +++ b/simulation/client/utils/inc/ntp_client.h @@ -0,0 +1,73 @@ +#pragma once + +/************************************************************************************************ + * @file ntp_client.h + * + * @brief Header file defining the NTPClient class + * + * @date 2025-01-04 + * @author Aryan Kashem + ************************************************************************************************/ + +/* Standard library Headers */ +#include +#include +#include +#include + +/* Inter-component Headers */ +#include +#include +#include +#include +#include + +#include "network_time_protocol.h" + +/* Intra-component Headers */ + +/** + * @defgroup Client_Utils + * @brief Client Utilities and Infrastructure + * @{ + */ + +/** + * @class NTPClient + * @brief Class that represents the netowrk-time-protocol client that synchronizes with a server + * @details This class is responsible for fetching stratum 2 time and synchronizing itself + * The client shall account for timing errors/differences every NTP_SYNC_PERIOD_S + */ +class NTPClient { + private: + static constexpr int NTP_PORT = 123; /**< Universal port for network-time-protocol */ + static constexpr int NTP_SYNC_PERIOD_S = 60; /**< Client synchronization period */ + + int m_NTPSocket; /**< The NTP servers socket FD */ + std::string m_serverAddress; /**< The NTP servers address for fetching stratum 2 data (ie: 127.0.0.1) */ + NTPPacket m_serverResponse; /**< NTP Packet to store the servers timing data */ + pthread_t m_NTPClientThreadId; /**< Thread Id for synchronizing the client */ + std::atomic m_isSynchronizing; /**< Boolean flag to indicate the synchronization status of the client */ + + public: + /** + * @brief Constructs a NTPClient object + * @details Initializes the NTP client. The constructor sets up internal variables + */ + NTPClient(); + + /** + * @brief Thread procedure for handling NTP synchronization + * @details This thread shall run at the NTP_SYNC_PERIOD_S frequency + */ + void NTPClientProcedure(); + + /** + * @brief Starts NTP synchronization of the client + * @details This function will spawn the NTPClientProcedure thread + * @param serverAddress The address of the NTP server to bind to + */ + void startSynchronization(const std::string &serverAddress); +}; + +/** @} */ diff --git a/simulation/client/utils/src/client.cc b/simulation/client/utils/src/client.cc new file mode 100644 index 0000000..e69b308 --- /dev/null +++ b/simulation/client/utils/src/client.cc @@ -0,0 +1,158 @@ +/************************************************************************************************ + * @file client.cc + * + * @brief Source file defining the Client class + * + * @date 2025-01-04 + * @author Aryan Kashem + ************************************************************************************************/ + +/* Standard library Headers */ +#include +#include + +/* Inter-component Headers */ + +/* Intra-component Headers */ +#include "client.h" + +void Client::processMessagesProcedure() { + while (m_isConnected) { + /* Wait for new data */ + sem_wait(&m_messageSemaphore); + + pthread_mutex_lock(&m_mutex); + + if (!m_messageQueue.empty()) { + auto message = m_messageQueue.front(); + m_messageQueue.pop(); + + if (m_messageCallback) { + m_messageCallback(this, message); + } + pthread_mutex_unlock(&m_mutex); + } + } +} + +void Client::receiverProcedure() { + while (m_isConnected) { + std::string message(MAX_BUFFER_SIZE, '\0'); + + size_t bytesRead = read(m_clientSocket, &message[0], sizeof(message)); + if (bytesRead <= 0) { + disconnectServer(); + throw std::runtime_error("Connection lost"); + } + + message[bytesRead] = '\0'; + + pthread_mutex_lock(&m_mutex); + m_messageQueue.push(message); + pthread_mutex_unlock(&m_mutex); + + /* Signal that there is new data */ + sem_post(&m_messageSemaphore); + } +} + +void *processMessagesProcedureWrapper(void *param) { + Client *client = static_cast(param); + + try { + client->processMessagesProcedure(); + } catch (...) { + std::cerr << "Process Messages Thread Error" << std::endl; + } + + return nullptr; +} + +void *receiverProcedureWrapper(void *param) { + Client *client = static_cast(param); + + try { + client->receiverProcedure(); + } catch (std::exception &e) { + std::cerr << "Receiver Thread Error " << e.what() << std::endl; + } + + return nullptr; +} + +void Client::connectServer() { + try { + m_clientSocket = socket(AF_INET, SOCK_STREAM, 0); + + if (m_clientSocket < 0) throw std::runtime_error("Error created socket"); + + m_serverAddress.sin_family = AF_INET; + m_serverAddress.sin_port = htons(m_port); + + if (inet_pton(AF_INET, m_host.c_str(), &m_serverAddress.sin_addr) <= 0) { + close(m_clientSocket); + throw std::runtime_error("Error converting IPv4 host address to binary form"); + } + + if (connect(m_clientSocket, (struct sockaddr *)&m_serverAddress, sizeof(m_serverAddress)) < 0) { + close(m_clientSocket); + throw std::runtime_error("Error connecting socket"); + } + + m_isConnected = true; + + if (m_connectCallback) { + m_connectCallback(this); + } + + if (pthread_create(&m_receiverThreadId, NULL, receiverProcedureWrapper, this)) { + close(m_clientSocket); + throw std::runtime_error("Failed to create receiver thread"); + } + + if (pthread_create(&m_processMessageThreadId, NULL, processMessagesProcedureWrapper, this)) { + close(m_clientSocket); + throw std::runtime_error("Failed to create process messages thread"); + } + + } catch (std::exception &e) { + std::cerr << "Error connecting to the server: " << e.what() << std::endl; + } +} + +void Client::disconnectServer() { + m_isConnected = false; + close(m_clientSocket); + m_clientSocket = 0; +} + +void Client::sendMessage(const std::string &message) { + int n = send(m_clientSocket, message.c_str(), message.size(), 0); + if (n < 0) throw std::runtime_error("Error sending message"); +} + +bool Client::isConnected() const { + return m_isConnected; +} + +Client::Client(const std::string &host, int port, messageCallback messageCallback, connectCallback connectCallback) { + this->m_host = host; + this->m_port = port; + this->m_clientSocket = -1; + this->m_messageCallback = messageCallback; + this->m_connectCallback = connectCallback; + + if (pthread_mutex_init(&m_mutex, NULL) != 0) { + throw std::runtime_error("Error initializing mutex"); + } + + if (sem_init(&m_messageSemaphore, 0, 0) != 0) { + throw std::runtime_error("Error initializing semaphore"); + } +} + +Client::~Client() { + disconnectServer(); + pthread_mutex_destroy(&m_mutex); + sem_destroy(&m_messageSemaphore); +} diff --git a/simulation/client/utils/src/ntp_client.cc b/simulation/client/utils/src/ntp_client.cc new file mode 100644 index 0000000..38c2a3e --- /dev/null +++ b/simulation/client/utils/src/ntp_client.cc @@ -0,0 +1,102 @@ +/************************************************************************************************ + * @file ntp_client.cc + * + * @brief Source file defining the NTPClient class + * + * @date 2025-01-04 + * @author Aryan Kashem + ************************************************************************************************/ + +/* Standard library Headers */ +#include +#include + +/* Inter-component Headers */ +#include "thread_helpers.h" + +/* Intra-component Headers */ +#include "ntp_client.h" + +void NTPClient::NTPClientProcedure() { + int ntpSocket = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP); + if (ntpSocket < 0) { + close(ntpSocket); + throw std::runtime_error("Error creating socket"); + } + + struct sockaddr_in serverAddr; + memset((char *)&serverAddr, 0U, sizeof(serverAddr)); + serverAddr.sin_family = AF_INET; + serverAddr.sin_port = htons(NTP_PORT); + + if (inet_pton(AF_INET, m_serverAddress.c_str(), &serverAddr.sin_addr) <= 0) { + close(ntpSocket); + throw std::runtime_error("Invalid server address"); + } + + m_isSynchronizing = true; + std::cerr << "Connected NTP Server" << std::endl; + + while (m_isSynchronizing) { + NTPPacket request = {}; + request.flags = (NTP_VERSION << NTP_VERSION_OFFSET) | (NTPLeapIndicator::NTP_LI_NOSYNC << NTP_LEAP_INDICATOR_OFFSET) | (NTPMode::NTP_CLIENT_MODE << NTP_MODE_OFFSET); + + /* Convert UNIX time in seconds to NTP time */ + request.transmitTime.seconds = htonl(unixToNTPTime(std::chrono::system_clock::to_time_t(std::chrono::system_clock::now()))); + + /* Obtain the fractional time */ + request.transmitTime.fraction = htonl(static_cast(std::chrono::duration_cast(std::chrono::system_clock::now().time_since_epoch()).count() % 1000000)); + + if (sendto(ntpSocket, &request, sizeof(request), 0, (struct sockaddr *)&serverAddr, sizeof(serverAddr)) <= 0) { + close(ntpSocket); + throw std::runtime_error("Error sending NTP request"); + } + + NTPPacket serverResponse = {}; + if (recvfrom(ntpSocket, &serverResponse, sizeof(serverResponse), 0, nullptr, nullptr) <= 0) { + close(ntpSocket); + throw std::runtime_error("Error receiving NTP response"); + } + + convertNTPTimestamp(serverResponse.referenceTime); + convertNTPTimestamp(serverResponse.originTime); + convertNTPTimestamp(serverResponse.receiveTime); + convertNTPTimestamp(serverResponse.transmitTime); + + serverResponse.rootDelay = ntohl(serverResponse.rootDelay); + serverResponse.rootDispersion = ntohl(serverResponse.rootDispersion); + + m_serverResponse = serverResponse; + + dumpNTPPacketData(m_serverResponse); + thread_sleep_s(1); + } + + m_isSynchronizing = false; + close(ntpSocket); +} + +void *NTPClientWrapper(void *param) { + NTPClient *client = static_cast(param); + + try { + client->NTPClientProcedure(); + } catch (std::exception &e) { + std::cerr << "NTP Client Thread Error: " << e.what() << std::endl; + } + + return nullptr; +} + +void NTPClient::startSynchronization(const std::string &serverAddress) { + m_serverAddress = serverAddress; + + if (pthread_create(&m_NTPClientThreadId, nullptr, NTPClientWrapper, this)) { + throw std::runtime_error("Listen Error"); + } +} + +NTPClient::NTPClient() { + m_isSynchronizing = false; + m_NTPSocket = -1; +} diff --git a/simulation/command.md b/simulation/command.md new file mode 100644 index 0000000..0597baf --- /dev/null +++ b/simulation/command.md @@ -0,0 +1,17 @@ +### Gpio Commands +1. GPIO GET_PIN_STATE [PORT][PIN] + - Example: GPIO GET_PIN_STATE A12 +2. GPIO GET_ALL_STATES + - Example: GPIO GET_ALL_STATES +3. GPIO GET_PIN_MODE [PORT][PIN] + - Example: GPIO GET_PIN_MODE A12 +4. GPIO GET_ALL_MODES + - Example: GPIO GET_ALL_MODES +5. GPIO GET_PIN_ALT_FUNCTION [PORT][PIN] + - Example: GPIO GET_PIN_ALT_FUNCTION A12 +6. GPIO GET_ALL_ALT_FUNCTIONS + - Example: GPIO GET_ALL_ALT_FUNCTIONS +7. GPIO SET_PIN_STATE [PORT][PIN] [STATE] + - Example: GPIO SET_PIN_STATE A12 HIGH +8. GPIO SET_ALL_STATES [STATE] + - Example: GPIO SET_ALL_STATES HIGH diff --git a/simulation/common/inc/command_code.h b/simulation/common/inc/command_code.h new file mode 100644 index 0000000..c0f1d11 --- /dev/null +++ b/simulation/common/inc/command_code.h @@ -0,0 +1,68 @@ +#pragma once + +/************************************************************************************************ + * @file command_code.h + * + * @brief Header file defining the CommandCodes + * + * @date 2025-01-04 + * @author Aryan Kashem + ************************************************************************************************/ + +/* Standard library Headers */ +#include + +/* Inter-component Headers */ + +/* Intra-component Headers */ + +/** + * @defgroup CommandCodes + * @brief Commands Supported by the Simulation + * @{ + */ + +/** + * @brief Command Code Class + */ +enum class CommandCode { + /* MISC Commands */ + METADATA, /**< Retrieve Client Metadata Command */ + + /* GPIO Commands */ + GPIO_SET_PIN_STATE, /**< Set a Gpio Pin state */ + GPIO_SET_ALL_STATES, /**< Set all Gpio Pin states */ + GPIO_GET_PIN_STATE, /**< Get a Gpio Pin state */ + GPIO_GET_ALL_STATES, /**< Get all Gpio Pin states */ + GPIO_GET_PIN_MODE, /**< Get a Gpio Pin mode */ + GPIO_GET_ALL_MODES, /**< Get all Gpio Pin modes */ + GPIO_GET_PIN_ALT_FUNCTION, /**< Get a Gpio Pin alternate function */ + GPIO_GET_ALL_ALT_FUNCTIONS, /**< Get all Gpio Pin alternate functions */ + + /* I2C Commands */ + + /* SPI Commands */ + + /* UART Commands */ + + /* FLASH Commands */ + + NUM_COMMAND_CODES /**< Number of command codes */ +}; + +/** + * @brief Encode a command code into an existing data payload + * @param commandCode Command code to be encoded + * @param message String data payload to be modified + * @return Encoded message + */ +std::string encodeCommand(const CommandCode commandCode, std::string &message); + +/** + * @brief Decode a command code from existing data + * @param message String data payload to be decoded + * @return Pair containing + */ +std::pair decodeCommand(std::string &message); + +/** @} */ diff --git a/simulation/common/inc/gpio_datagram.h b/simulation/common/inc/gpio_datagram.h new file mode 100644 index 0000000..78b954b --- /dev/null +++ b/simulation/common/inc/gpio_datagram.h @@ -0,0 +1,203 @@ +#pragma once + +/************************************************************************************************ + * @file gpio_datagram.h + * + * @brief Header file defining the GpioDatagram class + * + * @date 2025-01-04 + * @author Aryan Kashem + ************************************************************************************************/ + +/* Standard library Headers */ +#include +#include + +/* Inter-component Headers */ + +/* Intra-component Headers */ +#include "command_code.h" + +/** + * @defgroup GpioDatagram + * @brief Shared Gpio Datagram class + * @{ + */ + +namespace Datagram { + +/** + * @class DatagramGpio + * @brief Class for managing Gpio port configurations and data transfer operations + * @details This class provides an interface for configuring and managing Gpio ports, including + * pin modes, states, and alternate functions. It supports serialization for data transfer + * and maintains a buffer for Gpio operations + */ +class Gpio { + public: + static const constexpr uint8_t PINS_PER_PORT = 16U; /**< Supported Pins per Port */ + + /** + * @brief Gpio Port definition + */ + enum class Port { + GPIO_PORT_A = 0, /**< Gpio port A */ + GPIO_PORT_B, /**< Gpio port B */ + NUM_GPIO_PORTS /**< Number of Gpio Ports */ + }; + + /** + * @brief Gpio State definition + */ + enum class State { + GPIO_STATE_LOW = 0, /**< Gpio state low */ + GPIO_STATE_HIGH, /**< Gpio state high */ + }; + + /** + * @brief Gpio Mode definition + */ + enum class Mode { + GPIO_ANALOG = 0, /**< Gpio Mode Analog */ + GPIO_INPUT_FLOATING, /**< Gpio Mode Input Floating */ + GPIO_INPUT_PULL_DOWN, /**< Gpio Mode Input Pull Down */ + GPIO_INPUT_PULL_UP, /**< Gpio Mode Input Pull Up */ + GPIO_OUTPUT_OPEN_DRAIN, /**< Gpio Mode Output Open Drain */ + GPIO_OUTPUT_PUSH_PULL, /**< Gpio Mode Output Push Pull */ + GPIO_ALFTN_OPEN_DRAIN, /**< Gpio Mode Alt Open Drain */ + GPIO_ALTFN_PUSH_PULL, /**< Gpio Mode Alt Push Pull */ + NUM_GPIO_MODES, /**< Number of Gpio Modes */ + }; + + /** + * @brief Gpio Alt function definition + */ + enum class AltFunction { + // No ALT function + GPIO_ALT_NONE = 0x00U, + + // GPIO_ALT0 - System + GPIO_ALT0_SWDIO = 0x00U, + GPIO_ALT0_SWCLK = 0x00U, + + // GPIO_ALT1 - TIM1/TIM2 + GPIO_ALT1_TIM1 = 0x01U, + GPIO_ALT1_TIM2 = 0x01U, + + // GPIO_ALT4 - I2C + GPIO_ALT4_I2C1 = 0x04U, + GPIO_ALT4_I2C2 = 0x04U, + GPIO_ALT4_I2C3 = 0x04U, + + // GPIO_ALT5 - SPI + GPIO_ALT5_SPI1 = 0x05U, + GPIO_ALT5_SPI2 = 0x05U, + + // GPIO_ALT6 - SPI3 + GPIO_ALT6_SPI3 = 0x06U, + + // GPIO_ALT7 - USART + GPIO_ALT7_USART1 = 0x07U, + GPIO_ALT7_USART2 = 0x07U, + GPIO_ALT7_USART3 = 0x07U, + + // GPIO_ALT9 - CAN1 + GPIO_ALT9_CAN1 = 0x09U, + + // GPIO_ALT14 - Timers + GPIO_ALT14_TIM15 = 0x0EU, + GPIO_ALT14_TIM16 = 0x0EU, + }; + + /** Maximum buffer size dependent on the Pins per port and Number of Gpio ports */ + static constexpr size_t GPIO_MAX_BUFFER_SIZE = PINS_PER_PORT * static_cast(Port::NUM_GPIO_PORTS); + + /** + * @brief Gpio Datagram payload storage + */ + struct Payload { + Port gpioPort; /**< Gpio port */ + uint8_t gpioPin; /**< Gpio pin */ + uint8_t bufferLength; /**< Data buffer length */ + uint8_t buffer[GPIO_MAX_BUFFER_SIZE]; /**< Data buffer */ + }; + + /** + * @brief Constructs a Gpio object with provided payload data + * @param data Reference to payload data + */ + explicit Gpio(Payload &data); + + /** + * @brief Default constructor for Gpio object + */ + Gpio() = default; + + /** + * @brief Serializes Gpio data with command code for transmission + * @param commandCode Command code to include in serialized data + * @return Serialized string containing Gpio data + */ + std::string serialize(const CommandCode &commandCode) const; + + /** + * @brief Deserializes Gpio data from payload string + * @param gpioDatagramPayload String containing serialized Gpio data + */ + void deserialize(std::string &gpioDatagramPayload); + + /** + * @brief Sets the target Gpio port + * @param gpioPort Port to set as target + */ + void setGpioPort(const Port &gpioPort); + + /** + * @brief Sets the target Gpio pin + * @param gpioPin Pin to set as target + */ + void setGpioPin(const uint8_t &gpioPin); + + /** + * @brief Sets data in the Gpio buffer + * @param data Pointer to data to copy into buffer + * @param length Length of data to copy + */ + void setBuffer(const uint8_t *data, uint8_t length); + + /** + * @brief Clear the Gpio data buffer + */ + void clearBuffer(); + + /** + * @brief Gets the target Gpio port + * @return Target port + */ + Port getGpioPort() const; + + /** + * @brief Gets the target Gpio pin + * @return Target pin + */ + uint8_t getGpioPin() const; + + /** + * @brief Gets the Gpio buffer length + * @return Buffer length + */ + uint8_t getBufferLength() const; + + /** + * @brief Gets the Gpio buffer + * @return Pointer to the buffer array + */ + const uint8_t *getBuffer() const; + + private: + Payload m_gpioDatagram; /**< Private datagram payload */ +}; + +} // namespace Datagram + +/** @} */ diff --git a/simulation/common/inc/i2c_datagram.h b/simulation/common/inc/i2c_datagram.h new file mode 100644 index 0000000..0fef937 --- /dev/null +++ b/simulation/common/inc/i2c_datagram.h @@ -0,0 +1,124 @@ +#pragma once + +/************************************************************************************************ + * @file i2c_datagram.h + * + * @brief Header file defining the I2CDatagram class + * + * @date 2025-01-04 + * @author Aryan Kashem + ************************************************************************************************/ + +/* Standard library Headers */ +#include +#include + +/* Inter-component Headers */ + +/* Intra-component Headers */ +#include "command_code.h" + +/** + * @defgroup I2CDatagram + * @brief Shared I2C Datagram class + * @{ + */ + +namespace Datagram { + +/** + * @class DatagramI2C + * @brief Class for managing I2C port configurations and data transfer operations + * @details This class provides an interface for configuring and managing I2C TX/RX data + * on selected ports. It supports serialization for data transfer and maintains + * a buffer for I2C operations + */ +class I2C { + public: + static constexpr size_t I2C_MAX_BUFFER_SIZE = 256; /**< Maximum permitted buffer size */ + + /** + * @brief I2C Port definition + */ + enum class Port { + I2C_PORT_1, /**< I2C Port 1 */ + I2C_PORT_2, /**< I2C Port 2 */ + NUM_I2C_PORTS /**< Number of I2C ports */ + }; + + /** + * @brief I2C Datagram payload storage + */ + struct Payload { + Port i2cPort; /**< I2C port */ + size_t bufferLength; /**< Data buffer length */ + uint8_t buffer[I2C_MAX_BUFFER_SIZE]; /**< Data buffer */ + }; + + /** + * @brief Constructs an I2C object with provided payload data + * @param data Reference to payload data + */ + explicit I2C(Payload &data); + + /** + * @brief Default constructor for I2C object + */ + I2C() = default; + + /** + * @brief Serializes I2C data with command code for transmission + * @param commandCode Command code to include in serialized data + * @return Serialized string containing I2C data + */ + std::string serialize(const CommandCode &commandCode) const; + + /** + * @brief Deserializes I2C data from payload string + * @param i2cDatagramPayload String containing serialized I2C data + */ + void deserialize(std::string &i2cDatagramPayload); + + /** + * @brief Sets the target I2C port + * @param i2cPort Port to set as target + */ + void setI2CPort(const Port &i2cPort); + + /** + * @brief Sets data in the I2C buffer + * @param data Pointer to data to copy into buffer + * @param length Length of data to copy + */ + void setBuffer(const uint8_t *data, size_t length); + + /** + * @brief Clear the I2C data buffer + */ + void clearBuffer(); + + /** + * @brief Gets the target I2C port + * @return Target port + */ + Port getI2CPort() const; + + /** + * @brief Gets the I2C buffer length + * @return Buffer length + */ + size_t getBufferLength() const; + + /** + * @brief Gets the I2C buffer + * @return Pointer to the buffer array + */ + const uint8_t *getBuffer() const; + + private: + Payload m_i2cDatagram; /**< Private datagram payload */ +}; + +} // namespace Datagram + +/** @} */ diff --git a/simulation/common/inc/json_manager.h b/simulation/common/inc/json_manager.h new file mode 100644 index 0000000..c7e10e6 --- /dev/null +++ b/simulation/common/inc/json_manager.h @@ -0,0 +1,202 @@ +#pragma once + +/************************************************************************************************ + * @file json_manager.h + * + * @brief Header file defining the JSONManager class + * + * @date 2025-01-04 + * @author Aryan Kashem + ************************************************************************************************/ + +/* Standard library Headers */ +#include +#include +#include +#include +#include + +/* Inter-component Headers */ +#include + +/* Intra-component Headers */ + +/** + * @defgroup JSONManager + * @brief Shared JSON Manager class + * @{ + */ + +#define PROJECT_VERSION "1.0.0" + +/** + * @class JSONManager + * @brief Class for managing JSON Files + * @details This class supports creating new JSON files, writing/reading to and from JSON files + * Default JSON generation and saving/loading data from provided paths + */ +class JSONManager { + private: + static constexpr const char *DEFAULT_JSON_PATH = "./Simulation_JSON/"; /**< Default JSON folder path */ + std::filesystem::path m_projectBasePath; /**< Temporary variable to store project file path */ + + /** + * @brief Creates a default project JSON + * @details The project shall be created within the DEFAULT_JSON_PATH folder + * @param projectName Name of the project being created + */ + void createDefaultProjectJSON(const std::string &projectName); + + /** + * @brief Get the file path of a specified project + * @param projectName Name of the project being retrieved + * @return File path relative to the root directory + */ + std::filesystem::path getProjectFilePath(const std::string &projectName); + + /** + * @brief Loads the JSON of a specified project + * @param projectName Name of the project being retrieved + * @return JSON object of the selected project + */ + nlohmann::json loadProjectJSON(const std::string &projectName); + + /** + * @brief Saves JSON data to a specified project + * @param projectName Name of the project being written to + * @param projectData JSON object of the data to be written + */ + void saveProjectJSON(const std::string &projectName, const nlohmann::json &projectData); + + public: + /** + * @brief Constructs a JSONManager object + * @details Initializes the JSONManager. The constructor sets up internal + * variables and creates the DEFAULT_JSON_PATH folder + */ + JSONManager(); + + /** + * @brief Validate if a project JSON exists + * @param projectName Name of the project to validate + * @returns TRUE if the project JSON exists + * FALSE if the project JSON does not exist + */ + bool projectExists(const std::string &projectName); + + /** + * @brief Delete an existing project JSON + * @param projectName Name of the project to delete + */ + void deleteProject(const std::string &projectName); + + /** + * @brief Sets a value for a given key in the project JSON + * @details This function loads the project's JSON, updates the value for the given key, + * and then saves the updated JSON back to the project file + * @tparam T The type of the value to be set + * @param projectName Name of the project whose JSON will be updated + * @param key The key in the JSON to update + * @param value The value to set for the given key + */ + template + void setProjectValue(const std::string &projectName, const std::string &key, T value) { + try { + nlohmann::json projectJSON = loadProjectJSON(projectName); + + projectJSON[key] = value; + + saveProjectJSON(projectName, projectJSON); + } catch (const std::exception &e) { + std::cerr << "Error setting project value: " << e.what() << std::endl; + } + } + + /** + * @brief Sets a nested value for a given key in the project JSON + * @details This function loads the project's JSON, updates the value for the given key, + * and then saves the updated JSON back to the project file + * @tparam T The type of the value to be set + * @param projectName Name of the project whose JSON will be updated + * @param keyPath The key path in the JSON to update in the format: { 'key1', 'key2', 'key3' } + * @param value The value to set for the given key + */ + template + void setProjectNestedValue(const std::string &projectName, const std::vector &keyPath, const T &value) { + try { + nlohmann::json projectJSON = loadProjectJSON(projectName); + + nlohmann::json *current = &projectJSON; + + /* Navigate to the desired key location */ + for (size_t i = 0; i < keyPath.size() - 1; ++i) { + if (!current->contains(keyPath[i])) { + (*current)[keyPath[i]] = nlohmann::json::object(); + } + /* Update the JSON pointer to the nested JSON */ + current = &((*current)[keyPath[i]]); + } + + (*current)[keyPath.back()] = value; + + saveProjectJSON(projectName, projectJSON); + } catch (const std::exception &e) { + std::cerr << "Error setting nested project value: " << e.what() << std::endl; + } + } + + /** + * @brief Gets a value for a given key in the project JSON + * @details This function loads the project's JSON, and then retrieves the value for the given key + * @tparam T The type of the value to be fetched + * @param projectName Name of the project whose JSON will be checked + * @param key The key in the JSON to check + * @returns Value saved at the JSON key + */ + template + T getProjectValue(const std::string &projectName, const std::string &key) { + try { + nlohmann::json projectJSON = loadProjectJSON(projectName); + + if (projectJSON.contains(key)) { + return projectJSON[key].get(); + } + + } catch (const std::exception &e) { + std::cerr << "Error getting project value: " << e.what() << std::endl; + } + return static_cast(0U); + } + + /** + * @brief Gets a nested value for a given key in the project JSON + * @details This function loads the project's JSON, and then retrieves the value for the given key + * @tparam T The type of the value to be fetched + * @param projectName Name of the project whose JSON will be checked + * @param keyPath The key path in the JSON to check in the format: { 'key1', 'key2', 'key3' } + * @returns Value saved at the JSON key + */ + template + T getProjectNestedValue(const std::string &projectName, const std::vector &keyPath, const T &defaultValue = T()) { + try { + nlohmann::json projectJSON = loadProjectJSON(projectName); + + const nlohmann::json *current = &projectJSON; + /* Navigate to the desired key location */ + for (const auto &key : keyPath) { + if (!current->contains(key)) { + return defaultValue; + } + /* Update the JSON pointer to the nested JSON */ + current = &((*current)[key]); + } + + return current->get(); + } catch (const std::exception &e) { + std::cerr << "Error getting nested project value: " << e.what() << std::endl; + return defaultValue; + } + } +}; + +/** @} */ \ No newline at end of file diff --git a/simulation/common/inc/metadata.h b/simulation/common/inc/metadata.h new file mode 100644 index 0000000..340702b --- /dev/null +++ b/simulation/common/inc/metadata.h @@ -0,0 +1,110 @@ +#pragma once + +/************************************************************************************************ + * @file metadata.h + * + * @brief Header file defining the Metadata class + * + * @date 2025-01-04 + * @author Aryan Kashem + ************************************************************************************************/ + +/* Standard library Headers */ +#include +#include + +/* Inter-component Headers */ + +/* Intra-component Headers */ + +/** + * @defgroup MetadataDatagram + * @brief Shared Metadata Datagram class + * @{ + */ + +namespace Datagram { + +/** + * @class DatagramMetadata + * @brief Class for managing client Metadata and data transfer operations + * @details This class provides an interface for configuring and managing client Metadata + * It supports serialization for data transfer + */ +class Metadata { + public: + /** + * @brief Metadata Datagram payload storage + */ + struct Payload { + std::string projectName; /**< String project name */ + std::string projectStatus; /**< String project status */ + std::string hardwareModel; /**< String hardware model */ + }; + + /** + * @brief Constructs a Metadata object with provided payload data + * @param data Reference to payload data + */ + explicit Metadata(Payload &data); + + /** + * @brief Default constructor for Metadata object + */ + Metadata() = default; + + /** + * @brief Serializes Metadata data with the Metadata command code for transmission + * @return Serialized string containing Metadata data + */ + std::string serialize() const; + + /** + * @brief Deserializes Metadata data from payload string + * @param metadataPayload String containing serialized Metadata data + */ + void deserialize(std::string &metadataPayload); + + /** + * @brief Sets the target project name + * @param projectName Updated project name + */ + void setProjectName(const std::string &projectName); + + /** + * @brief Sets the target project status + * @param projectStatus Updated project status + */ + void setProjectStatus(const std::string &projectStatus); + + /** + * @brief Sets the target hardware model + * @param hardwareModel Updated hardware model + */ + void setHardwareModel(const std::string &hardwareModel); + + /** + * @brief Gets the target project name + * @return Target project name + */ + std::string getProjectName() const; + + /** + * @brief Gets the target project status + * @return Target project status + */ + std::string getProjectStatus() const; + + /** + * @brief Gets the target hardware model + * @return Target hardware model + */ + std::string getHardwareModel() const; + + private: + Payload m_metadata; /**< Private datagram payload */ +}; + +} // namespace Datagram + +/** @} */ diff --git a/simulation/common/inc/network_time_protocol.h b/simulation/common/inc/network_time_protocol.h new file mode 100644 index 0000000..5f1867d --- /dev/null +++ b/simulation/common/inc/network_time_protocol.h @@ -0,0 +1,132 @@ +#pragma once + +/************************************************************************************************ + * @file network_time_protocol.h + * + * @brief Header file defining the Network time protocol helpers + * + * @date 2025-01-04 + * @author Aryan Kashem + ************************************************************************************************/ + +/* Standard library Headers */ +#include +#include + +/* Inter-component Headers */ + +/* Intra-component Headers */ + +/** + * @defgroup NetworkTimeProtocolHelpers + * @brief Network time protocol helper library + * @{ + */ + +/** + * @brief NTP version being used + */ +#define NTP_VERSION 4U + +/** + * @brief Offset for the Leap Indicator in the flags field of the NTP packet + */ +#define NTP_LEAP_INDICATOR_OFFSET 6U + +/** + * @brief Offset for the NTP version in the flags field of the NTP packet + */ +#define NTP_VERSION_OFFSET 3U + +/** + * @brief Offset for the NTP mode in the flags field of the NTP packet + */ +#define NTP_MODE_OFFSET 0U + +/** + * @brief Macro to convert NTP poll interval to seconds + * @param poll The poll interval exponent + */ +#define NTP_POLL_TO_SECONDS(poll) (1U << poll) + +/** + * @brief Difference between the NTP epoch (1900-01-01) and the Unix epoch (1970-01-01) in seconds + */ +static constexpr uint32_t NTP_UNIX_EPOCH_DIFF = 2208988800UL; + +/** + * @brief Structure representing NTP time format with seconds and fractional seconds + */ +struct NTPTime { + uint32_t seconds; /**< Seconds since NTP epoch */ + uint32_t fraction; /**< Fractional seconds (1 / 2^32 of a second). */ +}; + +/** + * @brief Structure representing an NTP packet + */ +struct NTPPacket { + uint8_t flags; /**< Flags field containing Leap Indicator, Version, and Mode */ + uint8_t stratum; /**< Stratum level of the server */ + uint8_t poll; /**< Maximum interval between successive messages, in log2 seconds */ + uint8_t precision; /**< Precision of the local clock, in log2 seconds */ + uint32_t rootDelay; /**< Total round-trip delay to the primary reference source */ + uint32_t rootDispersion; /**< Maximum error relative to the primary reference source */ + uint8_t referenceId[4]; /**< Identifier of the reference clock */ + NTPTime referenceTime; /**< Time when the system clock was last set or corrected */ + NTPTime originTime; /**< Time at the client when the request departed */ + NTPTime receiveTime; /**< Time at the server when the request was received */ + NTPTime transmitTime; /**< Time at the server when the response was sent */ +}; + +/** + * @brief Enumeration for the NTP modes + */ +enum NTPMode { + NTP_RESERVED_MODE, /**< Reserved mode (not used) */ + NTP_ACTIVE_MODE, /**< Symmetric active mode */ + NTP_PASSIVE_MODE, /**< Symmetric passive mode */ + NTP_CLIENT_MODE, /**< Client mode */ + NTP_SERVER_MODE, /**< Server mode */ + NTP_BROADCAST_MODE, /**< Broadcast mode */ + NTP_CONTROL_MODE, /**< Control mode */ + NUM_NTP_MODES /**< Number of modes */ +}; + +/** + * @brief Enumeration for the Leap Indicator values + */ +enum NTPLeapIndicator { + NTP_LI_NONE, /**< No leap second adjustment */ + NTP_LI_LAST_MINUTE_OF_THE_DAY_61_S, /**< Last minute of the day has 61 seconds */ + NTP_LI_LAST_MINUTE_OF_THE_DAY_59_S, /**< Last minute of the day has 59 seconds */ + NTP_LI_NOSYNC /**< Clock is not synchronized */ +}; + +/** + * @brief Converts an NTP time to Unix time + * @param ntpTime The NTP time to be converted + * @return The corresponding Unix time + */ +time_t ntpToUnixTime(NTPTime ntpTime); + +/** + * @brief Converts a Unix time to NTP time + * @param unixTime The Unix time to be converted + * @return The corresponding NTP time in seconds since the NTP epoch + */ +uint32_t unixToNTPTime(time_t unixTime); + +/** + * @brief Converts an NTP timestamp to its human-readable representation + * @param timestamp The NTP timestamp to be converted + */ +void convertNTPTimestamp(NTPTime ×tamp); + +/** + * @brief Dumps the data of an NTP packet for debugging purposes + * @param packet The NTP packet to be dumped + */ +void dumpNTPPacketData(const NTPPacket packet); + +/** @} */ diff --git a/simulation/common/inc/serialization.h b/simulation/common/inc/serialization.h new file mode 100644 index 0000000..f08e183 --- /dev/null +++ b/simulation/common/inc/serialization.h @@ -0,0 +1,74 @@ +#pragma once + +/************************************************************************************************ + * @file serialization.h + * + * @brief Header file defining the serialization helper functions + * + * @date 2025-01-04 + * @author Aryan Kashem + ************************************************************************************************/ + +/* Standard library Headers */ +#include +#include + +/* Inter-component Headers */ + +/* Intra-component Headers */ + +/** + * @defgroup SerializationHelpers + * @brief Serialization helper library + * @{ + */ + +/** + * @brief Serialize an integer value + * @details The integer will be converted to a string of size T. + * Size T is dependent on the type of integer. + * The stringified-integer is appended to the target. + * @param target Existing message payload + * @param value Value to be appended + */ +template +void serializeInteger(std::string &target, T value) { + target.append(reinterpret_cast(&value), sizeof(T)); +} + +/** + * @brief Serialize a string value + * @details The string will be pre-fixed with a 16-bit size to indicate the size + * The string is appended to the target. The final packet follows as such: + * | target | 16-bit length | str | + * @param target Existing message payload + * @param str String to be appended + */ +void serializeString(std::string &target, const std::string &str); + +/** + * @brief Deserialize an integer value + * @details The offset will automatically be incremented in this function + * @param source Message payload to be decoded + * @param offset Byte offset from the start of the message payload + * @return Deserialized integer value + */ +template +T deserializeInteger(const std::string &source, size_t &offset) { + T value; + std::memcpy(&value, source.data() + offset, sizeof(T)); + offset += sizeof(T); + return value; +} + +/** + * @brief Deserialize a string value + * @details This will fetch the string value based on the stored length + * The offset will automatically be incremented in this function + * @param source Message payload to be decoded + * @param offset Byte offset from the start of the message payload + * @return Deserialized string value + */ +std::string deserializeString(std::string &source, size_t &offset); + +/** @} */ diff --git a/simulation/common/inc/spi_datagram.h b/simulation/common/inc/spi_datagram.h new file mode 100644 index 0000000..5a4f8ca --- /dev/null +++ b/simulation/common/inc/spi_datagram.h @@ -0,0 +1,124 @@ +#pragma once + +/************************************************************************************************ + * @file spi_datagram.h + * + * @brief Header file defining the SPIDatagram class + * + * @date 2025-01-04 + * @author Aryan Kashem + ************************************************************************************************/ + +/* Standard library Headers */ +#include +#include + +/* Inter-component Headers */ + +/* Intra-component Headers */ +#include "command_code.h" + +/** + * @defgroup SPIDatagram + * @brief Shared SPI Datagram class + * @{ + */ + +namespace Datagram { + +/** + * @class DatagramSPI + * @brief Class for managing SPI port configurations and data transfer operations + * @details This class provides an interface for configuring and managing SPI TX/RX data + * on selected ports. It supports serialization for data transfer and maintains + * a buffer for SPI operations + */ +class SPI { + public: + static constexpr size_t SPI_MAX_BUFFER_SIZE = 256; /**< Maximum permitted buffer size */ + + /** + * @brief SPI Port definition + */ + enum class Port { + SPI_PORT_1, /**< SPI Port 1 */ + SPI_PORT_2, /**< SPI Port 2 */ + NUM_SPI_PORTS /**< Number of SPI ports */ + }; + + /** + * @brief SPI Datagram payload storage + */ + struct Payload { + Port spiPort; /**< SPI port */ + size_t bufferLength; /**< Data buffer length */ + uint8_t buffer[SPI_MAX_BUFFER_SIZE]; /**< Data buffer */ + }; + + /** + * @brief Constructs a SPI object with provided payload data + * @param data Reference to payload data + */ + explicit SPI(Payload &data); + + /** + * @brief Default constructor for SPI object + */ + SPI() = default; + + /** + * @brief Serializes SPI data with command code for transmission + * @param commandCode Command code to include in serialized data + * @return Serialized string containing SPI data + */ + std::string serialize(const CommandCode &commandCode) const; + + /** + * @brief Deserializes SPI data from payload string + * @param spiDatagramPayload String containing serialized SPI data + */ + void deserialize(std::string &spiDatagramPayload); + + /** + * @brief Sets the target SPI port + * @param spiPort Port to set as target + */ + void setSPIPort(const Port &spiPort); + + /** + * @brief Sets data in the SPI buffer + * @param data Pointer to data to copy into buffer + * @param length Length of data to copy + */ + void setBuffer(const uint8_t *data, size_t length); + + /** + * @brief Clear the SPI data buffer + */ + void clearBuffer(); + + /** + * @brief Gets the target SPI port + * @return Target port + */ + Port getSPIPort() const; + + /** + * @brief Gets the SPI buffer length + * @return Buffer length + */ + size_t getBufferLength() const; + + /** + * @brief Gets the SPI buffer + * @return Pointer to the buffer array + */ + const uint8_t *getBuffer() const; + + private: + Payload m_spiDatagram; /**< Private datagram payload */ +}; + +} // namespace Datagram + +/** @} */ diff --git a/simulation/common/inc/thread_helpers.h b/simulation/common/inc/thread_helpers.h new file mode 100644 index 0000000..937f931 --- /dev/null +++ b/simulation/common/inc/thread_helpers.h @@ -0,0 +1,36 @@ +#pragma once + +/************************************************************************************************ + * @file thread_helpers.h + * + * @brief Header file defining the thread helper functions + * + * @date 2025-01-04 + * @author Aryan Kashem + ************************************************************************************************/ + +/* Standard library Headers */ + +/* Inter-component Headers */ + +/* Intra-component Headers */ + +/** + * @defgroup ThreadHelpers + * @brief Thread helper/management library + * @{ + */ + +/** + * @brief Sleep the current Linux pthread with the specified seconds + * @param seconds Seconds to block the current thread + */ +void thread_sleep_s(unsigned int seconds); + +/** + * @brief Sleep the current Linux pthread with the specified milliseconds + * @param milliseconds Milliseconds to block the current thread + */ +void thread_sleep_ms(unsigned int milliseconds); + +/** @} */ diff --git a/simulation/common/src/command_code.cc b/simulation/common/src/command_code.cc new file mode 100644 index 0000000..55ded8a --- /dev/null +++ b/simulation/common/src/command_code.cc @@ -0,0 +1,40 @@ +/************************************************************************************************ + * @file command_code.cc + * + * @brief Source file defining the CommandCode helpers + * + * @date 2025-01-04 + * @author Aryan Kashem + ************************************************************************************************/ + +/* Standard library Headers */ +#include + +/* Inter-component Headers */ + +/* Intra-component Headers */ +#include "command_code.h" + +std::string encodeCommand(const CommandCode commandCode, std::string &message) { + return std::to_string(static_cast(commandCode)) + '|' + message; +} + +std::pair decodeCommand(std::string &message) { + size_t delimPosition = message.find('|'); + + if (delimPosition == std::string::npos) { + throw std::runtime_error("Invalid command format"); + } + + uint8_t commandCodeValue = std::stoi(message.substr(0, delimPosition)); + + if (commandCodeValue > static_cast(CommandCode::NUM_COMMAND_CODES)) { + throw std::runtime_error("CommandCode out of valid range"); + } + + CommandCode commandCode = static_cast(commandCodeValue); + + std::string payload = message.substr(delimPosition + 1); + + return { commandCode, payload }; +} diff --git a/simulation/common/src/gpio_datagram.cc b/simulation/common/src/gpio_datagram.cc new file mode 100644 index 0000000..94eda39 --- /dev/null +++ b/simulation/common/src/gpio_datagram.cc @@ -0,0 +1,91 @@ +/************************************************************************************************ + * @file gpio_datagram.cc + * + * @brief Source file defining the GpioDatagram class + * + * @date 2025-01-04 + * @author Aryan Kashem + ************************************************************************************************/ + +/* Standard library Headers */ +#include +#include +#include + +/* Inter-component Headers */ + +/* Intra-component Headers */ +#include "gpio_datagram.h" +#include "serialization.h" + +namespace Datagram { +std::string Gpio::serialize(const CommandCode &commandCode) const { + std::string serializedData; + + serializeInteger(serializedData, static_cast(m_gpioDatagram.gpioPort)); + serializeInteger(serializedData, static_cast(m_gpioDatagram.gpioPin)); + + if (m_gpioDatagram.bufferLength > GPIO_MAX_BUFFER_SIZE) { + throw std::runtime_error("Serialized Gpio buffer length exceeds maximum allowed size"); + } + + serializeInteger(serializedData, m_gpioDatagram.bufferLength); + serializedData.append(reinterpret_cast(m_gpioDatagram.buffer), m_gpioDatagram.bufferLength); + return encodeCommand(commandCode, serializedData); +} + +void Gpio::deserialize(std::string &gpioDatagramPayload) { + if (gpioDatagramPayload.size() < 3U) { + throw std::runtime_error("Invalid GPIO datagram payload"); + } + size_t offset = 0; + + m_gpioDatagram.gpioPort = static_cast(deserializeInteger(gpioDatagramPayload, offset)); + m_gpioDatagram.gpioPin = deserializeInteger(gpioDatagramPayload, offset); + m_gpioDatagram.bufferLength = deserializeInteger(gpioDatagramPayload, offset); + + if (m_gpioDatagram.bufferLength > GPIO_MAX_BUFFER_SIZE) { + throw std::runtime_error("Deserialized Gpio buffer length exceeds maximum allowed size"); + } + + std::memcpy(m_gpioDatagram.buffer, gpioDatagramPayload.data() + offset, m_gpioDatagram.bufferLength); +} + +Gpio::Gpio(Payload &data) { + m_gpioDatagram = data; +} + +void Gpio::setGpioPort(const Port &gpioPort) { + m_gpioDatagram.gpioPort = gpioPort; +} + +void Gpio::setGpioPin(const uint8_t &gpioPin) { + m_gpioDatagram.gpioPin = gpioPin; +} + +void Gpio::setBuffer(const uint8_t *data, uint8_t length) { + std::memcpy(m_gpioDatagram.buffer, data, length); + m_gpioDatagram.bufferLength = length; +} + +void Gpio::clearBuffer() { + std::memset(m_gpioDatagram.buffer, 0U, GPIO_MAX_BUFFER_SIZE); +} + +Gpio::Port Gpio::getGpioPort() const { + return m_gpioDatagram.gpioPort; +} + +uint8_t Gpio::getGpioPin() const { + return m_gpioDatagram.gpioPin; +} + +uint8_t Gpio::getBufferLength() const { + return m_gpioDatagram.bufferLength; +} + +const uint8_t *Gpio::getBuffer() const { + return m_gpioDatagram.buffer; +} + +} // namespace Datagram diff --git a/simulation/common/src/i2c_datagram.cc b/simulation/common/src/i2c_datagram.cc new file mode 100644 index 0000000..e6a9f9d --- /dev/null +++ b/simulation/common/src/i2c_datagram.cc @@ -0,0 +1,73 @@ +/************************************************************************************************ + * @file i2c_datagram.cc + * + * @brief Source file defining the I2CDatagram class + * + * @date 2025-01-04 + * @author Aryan Kashem + ************************************************************************************************/ + +/* Standard library Headers */ +#include +#include +#include + +/* Inter-component Headers */ + +/* Intra-component Headers */ +#include "i2c_datagram.h" +#include "serialization.h" + +namespace Datagram { + +I2C::I2C(Payload &data) { + m_i2cDatagram = data; +} + +std::string I2C::serialize(const CommandCode &commandCode) const { + std::string serializedData; + + serializeInteger(serializedData, static_cast(m_i2cDatagram.i2cPort)); + serializeInteger(serializedData, static_cast(m_i2cDatagram.bufferLength)); + serializedData.append(reinterpret_cast(m_i2cDatagram.buffer), m_i2cDatagram.bufferLength); +} + +void I2C::deserialize(std::string &i2cDatagramPayload) { + size_t offset = 0; + + m_i2cDatagram.i2cPort = static_cast(deserializeInteger(i2cDatagramPayload, offset)); + m_i2cDatagram.bufferLength = deserializeInteger(i2cDatagramPayload, offset); + + if (m_i2cDatagram.bufferLength > I2C_MAX_BUFFER_SIZE) { + throw std::runtime_error("Deserialized I2C buffer length exceeds maximum allowed size"); + } + + std::memcpy(m_i2cDatagram.buffer, i2cDatagramPayload.data() + offset, m_i2cDatagram.bufferLength); +} + +void I2C::setI2CPort(const Port &i2cPort) { + m_i2cDatagram.i2cPort = i2cPort; +} + +void I2C::setBuffer(const uint8_t *data, size_t length) { + std::memcpy(m_i2cDatagram.buffer, data, length); + m_i2cDatagram.bufferLength = length; +} + +void I2C::clearBuffer() { + std::memset(m_i2cDatagram.buffer, 0U, I2C_MAX_BUFFER_SIZE); +} + +I2C::Port I2C::getI2CPort() const { + return m_i2cDatagram.i2cPort; +} + +size_t I2C::getBufferLength() const { + return m_i2cDatagram.bufferLength; +} + +const uint8_t *I2C::getBuffer() const { + return m_i2cDatagram.buffer; +} + +} // namespace Datagram diff --git a/simulation/common/src/json_manager.cc b/simulation/common/src/json_manager.cc new file mode 100644 index 0000000..833bd12 --- /dev/null +++ b/simulation/common/src/json_manager.cc @@ -0,0 +1,116 @@ +/************************************************************************************************ + * @file json_manager.cc + * + * @brief Source file defining the JSONManager class + * + * @date 2025-01-04 + * @author Aryan Kashem + ************************************************************************************************/ + +/* Standard library Headers */ +#include +#include + +/* Inter-component Headers */ + +/* Intra-component Headers */ +#include "json_manager.h" + +void JSONManager::createDefaultProjectJSON(const std::string &projectName) { + if (projectExists(projectName)) { + std::cerr << "Project '" << projectName << "' already exists." << std::endl; + return; + } + + try { + std::time_t now = std::time(nullptr); + + /* YYYY-MM-DD HH:MM:SS -> 19 chars + null terminator */ + char timeBuffer[20]; + std::strftime(timeBuffer, sizeof(timeBuffer), "%Y-%m-%d %H:%M:%S", std::localtime(&now)); + + nlohmann::json defaultJSON = { { "project_name", projectName }, { "version", "1.0.0" }, { "created_at", std::string(timeBuffer) }, { "settings", nlohmann::json::object() } }; + saveProjectJSON(projectName, defaultJSON); + } catch (const std::exception &e) { + std::cerr << "Error creating project JSON: " << e.what() << std::endl; + } +} + +std::filesystem::path JSONManager::getProjectFilePath(const std::string &projectName) { + std::string sanitizedName = projectName; + + /* Get rid of special characters that aren't _ or - */ + for (char &c : sanitizedName) { + if (!std::isalnum(c) && c != '_' && c != '-') { + c = '_'; + } + } + return m_projectBasePath / (sanitizedName + ".json"); +} + +nlohmann::json JSONManager::loadProjectJSON(const std::string &projectName) { + try { + std::filesystem::path projectPath = getProjectFilePath(projectName); + + if (!std::filesystem::exists(projectPath)) { + createDefaultProjectJSON(projectName); + } + + std::ifstream projectFile(projectPath); + if (!projectFile.is_open()) { + throw std::runtime_error("Could not open project JSON file"); + } + + return nlohmann::json::parse(projectFile); + + } catch (const std::exception &e) { + std::cerr << "Error loading project JSON: " << e.what() << std::endl; + } + return nlohmann::json::object(); +} + +void JSONManager::saveProjectJSON(const std::string &projectName, const nlohmann::json &projectData) { + try { + std::filesystem::path projectPath = getProjectFilePath(projectName); + + std::ofstream projectFile(projectPath); + if (!projectFile.is_open()) { + throw std::runtime_error("Could not open project JSON file for writing"); + } + + projectFile << projectData.dump(2); + } catch (const std::exception &e) { + std::cerr << "Error saving project JSON: " << e.what() << std::endl; + } +} + +JSONManager::JSONManager() { + /* Create the JSON output directory */ + m_projectBasePath = std::filesystem::absolute(DEFAULT_JSON_PATH); + std::filesystem::create_directories(m_projectBasePath); + + /* Clean up the directory by deleting all .json files */ + for (const auto &file : std::filesystem::directory_iterator(m_projectBasePath)) { + if (file.is_regular_file() && file.path().extension() == ".json") { + std::filesystem::remove(file.path()); + } + } +} + +bool JSONManager::projectExists(const std::string &projectName) { + return std::filesystem::exists(getProjectFilePath(projectName)); +} + +void JSONManager::deleteProject(const std::string &projectName) { + try { + std::filesystem::path projectPath = getProjectFilePath(projectName); + + if (std::filesystem::exists(projectPath)) { + std::filesystem::remove(projectPath); + } else { + std::cerr << "Project '" << projectName << "' does not exist." << std::endl; + } + } catch (const std::exception &e) { + std::cerr << "Error deleting project JSON: " << e.what() << std::endl; + } +} diff --git a/simulation/common/src/metadata.cc b/simulation/common/src/metadata.cc new file mode 100644 index 0000000..eacc37e --- /dev/null +++ b/simulation/common/src/metadata.cc @@ -0,0 +1,69 @@ +/************************************************************************************************ + * @file metadata.cc + * + * @brief Source file defining the Metadata class + * + * @date 2025-01-04 + * @author Aryan Kashem + ************************************************************************************************/ + +/* Standard library Headers */ +#include +#include + +/* Inter-component Headers */ + +/* Intra-component Headers */ +#include "command_code.h" +#include "metadata.h" +#include "serialization.h" + +namespace Datagram { + +std::string Metadata::serialize() const { + std::string serializedData; + + serializeString(serializedData, m_metadata.projectName); + serializeString(serializedData, m_metadata.projectStatus); + serializeString(serializedData, m_metadata.hardwareModel); + + return encodeCommand(CommandCode::METADATA, serializedData); +} + +void Metadata::deserialize(std::string &metadataPayload) { + size_t offset = 0; + + m_metadata.projectName = deserializeString(metadataPayload, offset); + m_metadata.projectStatus = deserializeString(metadataPayload, offset); + m_metadata.hardwareModel = deserializeString(metadataPayload, offset); +} + +Metadata::Metadata(Payload &data) { + m_metadata = data; +} + +void Metadata::setProjectName(const std::string &projectName) { + m_metadata.projectName = projectName; +} + +void Metadata::setProjectStatus(const std::string &projectStatus) { + m_metadata.projectStatus = projectStatus; +} + +void Metadata::setHardwareModel(const std::string &hardwareModel) { + m_metadata.hardwareModel = hardwareModel; +} + +std::string Metadata::getProjectName() const { + return m_metadata.projectName; +} + +std::string Metadata::getProjectStatus() const { + return m_metadata.projectStatus; +} + +std::string Metadata::getHardwareModel() const { + return m_metadata.hardwareModel; +} + +} // namespace Datagram diff --git a/simulation/common/src/network_time_protocol.cc b/simulation/common/src/network_time_protocol.cc new file mode 100644 index 0000000..04dbe53 --- /dev/null +++ b/simulation/common/src/network_time_protocol.cc @@ -0,0 +1,93 @@ +/************************************************************************************************ + * @file network_time_protocol.cc + * + * @brief Source file defining the Network time protocol helpers + * + * @date 2025-01-04 + * @author Aryan Kashem + ************************************************************************************************/ + +/* Standard library Headers */ +#include +#include + +/* Inter-component Headers */ +#include + +/* Intra-component Headers */ +#include "network_time_protocol.h" + +time_t ntpToUnixTime(uint32_t ntpTime) { + return ntpTime - NTP_UNIX_EPOCH_DIFF; +} + +uint32_t unixToNTPTime(time_t unixTime) { + return unixTime + NTP_UNIX_EPOCH_DIFF; +} + +void convertNTPTimestamp(NTPTime ×tamp) { + timestamp.seconds = ntohl(timestamp.seconds); + timestamp.fraction = ntohl(timestamp.fraction); +} + +void dumpNTPPacketData(const NTPPacket packet) { + uint8_t leapIndicator = (packet.flags >> NTP_LEAP_INDICATOR_OFFSET) & 0x03U; + uint8_t version = (packet.flags >> NTP_VERSION_OFFSET) & 0x07U; + uint8_t mode = packet.flags & 0x07U; + + const char *leapStrings[] = { "No warning", "Last minute has 61 seconds", "Last minute has 59 seconds", "Alarm: clock not synchronized" }; + + const char *modeStrings[] = { "Reserved", "Symmetric Active", "Symmetric Passive", "Client", "Server", "Broadcast", "NTP Control Message", "Private Use" }; + + std::cout << "NTP Packet Dump:" << std::endl; + std::cout << "----------------" << std::endl; + + std::cout << "Leap Indicator: " << leapStrings[leapIndicator] << " (" << static_cast(leapIndicator) << ")" << std::endl; + std::cout << "NTP Version: " << static_cast(version) << std::endl; + std::cout << "Mode: " << modeStrings[mode] << " (" << static_cast(mode) << ")" << std::endl; + + std::cout << "Stratum: " << static_cast(packet.stratum); + switch (packet.stratum) { + case 0: + std::cout << " (Unspecified)"; + break; + case 1: + std::cout << " (Primary Reference)"; + break; + case 2 ... 15: + std::cout << " (Secondary Reference)"; + break; + case 16 ... 255: + std::cout << " (Reserved)"; + break; + } + std::cout << std::endl; + + auto formatTimestamp = [](NTPTime ts) { + time_t unixTime = ntpToUnixTime(ts.seconds); + char buffer[64]; + std::strftime(buffer, sizeof(buffer), "%Y-%m-%d %H:%M:%S", std::gmtime(&unixTime)); + return std::string(buffer); + }; + + std::cout << "Reference Time: " << formatTimestamp(packet.referenceTime) << " (Seconds: " << packet.referenceTime.seconds << ", Fraction: " << packet.referenceTime.fraction << ")" << std::endl; + + std::cout << "Origin Time: " << formatTimestamp(packet.originTime) << " (Seconds: " << packet.originTime.seconds << ", Fraction: " << packet.originTime.fraction << ")" << std::endl; + + std::cout << "Receive Time: " << formatTimestamp(packet.receiveTime) << " (Seconds: " << packet.receiveTime.seconds << ", Fraction: " << packet.receiveTime.fraction << ")" << std::endl; + + std::cout << "Transmit Time: " << formatTimestamp(packet.transmitTime) << " (Seconds: " << packet.transmitTime.seconds << ", Fraction: " << packet.transmitTime.fraction << ")" << std::endl; + + std::cout << "Poll Interval: " << static_cast(packet.poll) << " (2^" << static_cast(packet.poll) << " seconds)" << std::endl; + + std::cout << "Precision: " << static_cast(packet.precision) << " (2^" << static_cast(packet.precision) << " seconds)" << std::endl; + + std::cout << "Reference ID: "; + char refIdStr[5]; + memcpy(refIdStr, &packet.referenceId, 4); + refIdStr[4] = '\0'; + std::cout << refIdStr << " (Raw: 0x" << std::hex << packet.referenceId << std::dec << ")" << std::endl; + + std::cout << "Root Delay: " << (packet.rootDelay / 65536.0) << " seconds" << std::endl; + std::cout << "Root Dispersion:" << (packet.rootDispersion / 65536.0) << " seconds" << std::endl; +} diff --git a/simulation/common/src/serialization.cc b/simulation/common/src/serialization.cc new file mode 100644 index 0000000..0c41e37 --- /dev/null +++ b/simulation/common/src/serialization.cc @@ -0,0 +1,29 @@ +/************************************************************************************************ + * @file serialization.cc + * + * @brief Source file defining the Serialization helpers + * + * @date 2025-01-04 + * @author Aryan Kashem + ************************************************************************************************/ + +/* Standard library Headers */ +#include + +/* Inter-component Headers */ + +/* Intra-component Headers */ +#include "serialization.h" + +void serializeString(std::string &target, const std::string &str) { + uint16_t length = static_cast(str.length()); + serializeInteger(target, length); + target.append(str); +} + +std::string deserializeString(std::string &source, size_t &offset) { + uint16_t length = deserializeInteger(source, offset); + std::string str = source.substr(offset, length); + offset += length; + return str; +} diff --git a/simulation/common/src/spi_datagram.cc b/simulation/common/src/spi_datagram.cc new file mode 100644 index 0000000..1b64b5d --- /dev/null +++ b/simulation/common/src/spi_datagram.cc @@ -0,0 +1,73 @@ +/************************************************************************************************ + * @file spi_datagram.cc + * + * @brief Source file defining the SPIDatagram class + * + * @date 2025-01-04 + * @author Aryan Kashem + ************************************************************************************************/ + +/* Standard library Headers */ +#include +#include +#include + +/* Inter-component Headers */ + +/* Intra-component Headers */ +#include "serialization.h" +#include "spi_datagram.h" + +namespace Datagram { + +SPI::SPI(Payload &data) { + m_spiDatagram = data; +} + +std::string SPI::serialize(const CommandCode &commandCode) const { + std::string serializedData; + + serializeInteger(serializedData, static_cast(m_spiDatagram.spiPort)); + serializeInteger(serializedData, static_cast(m_spiDatagram.bufferLength)); + serializedData.append(reinterpret_cast(m_spiDatagram.buffer), m_spiDatagram.bufferLength); +} + +void SPI::deserialize(std::string &spiDatagramPayload) { + size_t offset = 0; + + m_spiDatagram.spiPort = static_cast(deserializeInteger(spiDatagramPayload, offset)); + m_spiDatagram.bufferLength = deserializeInteger(spiDatagramPayload, offset); + + if (m_spiDatagram.bufferLength > SPI_MAX_BUFFER_SIZE) { + throw std::runtime_error("Deserialized SPI buffer length exceeds maximum allowed size"); + } + + std::memcpy(m_spiDatagram.buffer, spiDatagramPayload.data() + offset, m_spiDatagram.bufferLength); +} + +void SPI::setSPIPort(const Port &spiPort) { + m_spiDatagram.spiPort = spiPort; +} + +void SPI::setBuffer(const uint8_t *data, size_t length) { + std::memcpy(m_spiDatagram.buffer, data, length); + m_spiDatagram.bufferLength = length; +} + +void SPI::clearBuffer() { + std::memset(m_spiDatagram.buffer, 0U, SPI_MAX_BUFFER_SIZE); +} + +SPI::Port SPI::getSPIPort() const { + return m_spiDatagram.spiPort; +} + +size_t SPI::getBufferLength() const { + return m_spiDatagram.bufferLength; +} + +const uint8_t *SPI::getBuffer() const { + return m_spiDatagram.buffer; +} + +} // namespace Datagram diff --git a/simulation/common/src/thread_helpers.cc b/simulation/common/src/thread_helpers.cc new file mode 100644 index 0000000..0a8e137 --- /dev/null +++ b/simulation/common/src/thread_helpers.cc @@ -0,0 +1,30 @@ +/************************************************************************************************ + * @file thread_helpers.cc + * + * @brief Source file defining the thread helpers + * + * @date 2025-01-04 + * @author Aryan Kashem + ************************************************************************************************/ + +/* Standard library Headers */ + +/* Inter-component Headers */ +#include + +/* Intra-component Headers */ +#include "thread_helpers.h" + +void thread_sleep_s(unsigned int seconds) { + struct timespec ts; + ts.tv_sec = seconds; + ts.tv_nsec = 0U; + nanosleep(&ts, NULL); +} + +void thread_sleep_ms(unsigned int milliseconds) { + struct timespec ts; + ts.tv_sec = milliseconds / 1000U; + ts.tv_nsec = (milliseconds % 1000U) * 1000000U; + nanosleep(&ts, NULL); +} diff --git a/simulation/config.json b/simulation/config.json new file mode 100644 index 0000000..e02aef2 --- /dev/null +++ b/simulation/config.json @@ -0,0 +1,2 @@ +{ +} \ No newline at end of file diff --git a/simulation/scripts/main.py b/simulation/scripts/main.py new file mode 100644 index 0000000..a00cd41 --- /dev/null +++ b/simulation/scripts/main.py @@ -0,0 +1,22 @@ +# @defgroup VehicleSimulationPy Vehicle Simulation Python Package +# This is a python package that supports running the vehicle simulation +# +# @file main.py +# @date 2024-12-26 +# @author Midnight Sun Team #24 - MSXVI +# @brief Main python module for the Vehicle Simulation +# +# @details This file shall autogenerate the required files, and initiate the client and server +# +# @ingroup VehicleSimulationPy + + +def main(): + """ + @brief Main function for sample + """ + print("Welcome to Vehicle simulation python package!") + + +if __name__ == "__main__": + main() diff --git a/simulation/server/app/inc/app.h b/simulation/server/app/inc/app.h new file mode 100644 index 0000000..787a080 --- /dev/null +++ b/simulation/server/app/inc/app.h @@ -0,0 +1,39 @@ +#pragma once + +/************************************************************************************************ + * @file app.h + * + * @brief Header file defining the Application for the server + * + * @date 2025-01-04 + * @author Aryan Kashem + ************************************************************************************************/ + +/* Standard library Headers */ +#include + +/* Inter-component Headers */ +#include "json_manager.h" + +/* Intra-component Headers */ +#include "can_listener.h" +#include "can_scheduler.h" +#include "gpio_manager.h" + +/** + * @defgroup ServerAppMain + * @brief Server Application Main Interface + * @{ + */ + +#ifndef USE_NETWORK_TIME_PROTOCOL +/** @brief Define to use network-time-protocol synchronization. 0: Disabled 1: Enabled */ +#define USE_NETWORK_TIME_PROTOCOL 0U +#endif + +extern JSONManager serverJSONManager; /**< Global JSON Manager */ +extern GpioManager serverGpioManager; /**< Global GPIO Manager */ +extern CanListener serverCanListener; /**< Global CAN Listener */ +extern CanScheduler serverCanScheduler; /**< Global CAN Scheduler */ + +/** @} */ diff --git a/simulation/server/app/inc/app_callback.h b/simulation/server/app/inc/app_callback.h new file mode 100644 index 0000000..e3047d7 --- /dev/null +++ b/simulation/server/app/inc/app_callback.h @@ -0,0 +1,47 @@ +#pragma once + +/************************************************************************************************ + * @file app_callback.h + * + * @brief Header file defining the Application Callback functions for the server + * + * @date 2025-01-04 + * @author Aryan Kashem + ************************************************************************************************/ + +/* Standard library Headers */ +#include + +/* Inter-component Headers */ +#include "client_connection.h" +#include "server.h" + +/* Intra-component Headers */ + +/** + * @defgroup ServerAppCallback + * @brief Application Callback functions for the Server + * @{ + */ + +/** + * @brief Handle receiving new client messages + * @details This shall handle all CommandCodes received + * This shall branch out to the GpioManager, I2CManager, SPIManager or InterruptManager + * based on the CommandCode + * @param server Pointer to the server + * @param client Pointer to the connected clientConnection instance + * @param message String message value that has been received + */ +void applicationMessageCallback(Server *server, ClientConnection *client, std::string &message); + +/** + * @brief Handle connecting to a new client + * @details This shall notify the user about a new client connection + * This shall refresh the application interface with the new client data + * @param server Pointer to the server + * @param client Pointer to the connected clientConnection instance + */ +void applicationConnectCallback(Server *server, ClientConnection *client); + +/** @} */ diff --git a/simulation/server/app/inc/app_terminal.h b/simulation/server/app/inc/app_terminal.h new file mode 100644 index 0000000..64f45dc --- /dev/null +++ b/simulation/server/app/inc/app_terminal.h @@ -0,0 +1,75 @@ +#pragma once + +/************************************************************************************************ + * @file app_terminal.h + * + * @brief Header file defining the Application Terminal class + * + * @date 2025-01-04 + * @author Aryan Kashem + ************************************************************************************************/ + +/* Standard library Headers */ + +/* Inter-component Headers */ +#include "client_connection.h" +#include "server.h" + +/* Intra-component Headers */ + +/** + * @defgroup AppTerminal + * @brief Application Terminal/Shell Interface + * @{ + */ + +/** + * @class Terminal + * @brief Class that interfaces with the simulation API via a terminal UI + * @details This class is responsible packaging commands based on terminal inputs + * This class is the lightest application-layer, and should be replaced in the future with a GUI + */ +class Terminal { + private: + Server *m_Server; /**< Pointer to the server instance */ + ClientConnection *m_targetClient; /**< Pointer to a target client instance */ + + /** + * @brief Convert a string input to lower case + * @param input String input to be converted + * @return Lower-case version of the input string + */ + std::string toLower(const std::string &input); + + /** + * @brief Handle GPIO commands provoidede an action statement and tokenized parameters + * @param action Action statement to select the Remote procedure call + * @param tokens List containing action parameters to format the Remote procedure call + */ + void handleGpioCommands(const std::string &action, std::vector &tokens); + + /** + * @brief Parse the tokens and branch to the appropiate handler function + * @details This shall branch to handleGpioCommands, handleI2CCommands, handleSPICommands and handleInterruptCommands + * @param tokens List containing tokenized input string + */ + void parseCommand(std::vector &tokens); + + public: + /** + * @brief Constructs a Terminal object + * @details Initializes the Terminal. The constructor sets up internal variables + * The constructor shall bind to the server + * @param server Pointer to a server instance + */ + Terminal(Server *server); + + /** + * @brief While true spin-loop to handle shell inputs + * @details This function must be called after server initialization + * This function will never return. To exit safely, you must enter 'exit' + */ + void run(); +}; + +/** @} */ diff --git a/simulation/server/app/inc/can_listener.h b/simulation/server/app/inc/can_listener.h new file mode 100644 index 0000000..e91cc02 --- /dev/null +++ b/simulation/server/app/inc/can_listener.h @@ -0,0 +1,103 @@ +#pragma once + +/************************************************************************************************ + * @file can_listener.h + * + * @brief Header file defining the CanListener class + * + * @date 2025-01-04 + * @author Aryan Kashem + ************************************************************************************************/ + +/* Standard library Headers */ +#include +#include +#include +#include +#include + +/* Inter-component Headers */ +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +/* Intra-component Headers */ + +/** + * @defgroup CanListener + * @brief Raw SocketCAN abstraction class + * @{ + */ + +/** + * @class CanListener + * @brief Class that handles message listening over a SocketCAN interface + * @details This class is responsible listening to CAN messages and updating a JSON file + * This class shall cache the latest data and update the JSON every UPDATE_CAN_JSON_PERIOD_MS + */ +class CanListener { + private: + const std::string CAN_INTERFACE_NAME = "vcan0"; /**< SocketCAN interface name */ + const std::string CAN_JSON_NAME = "CANListener"; /**< CAN JSON file name */ + + static const constexpr unsigned int UPDATE_CAN_JSON_PERIOD_MS = 1000U; /**< JSON Update period in milliseconds */ + + std::unordered_map m_canInfo; /**< Hash-map to cache the latest CAN data */ + + pthread_mutex_t m_mutex; /**< Mutex to protect m_canInfo */ + pthread_t m_listenCanBusId; /**< Thread Id for listening to the CAN bus */ + pthread_t m_updateJSONId; /**< Thread Id for updating the CAN JSON */ + + int m_rawCanSocket; /**< Raw SocketCAN FD */ + std::atomic m_isListening; /** Boolean flag to track the CAN bus connection status */ + + /** + * @brief Handles receiving new CAN messages + * @details This function is called by the listeningCanBusProcedure + * This function is autogenerated + */ + void canMessageHandler(uint32_t id, const uint8_t *data); + + public: + /** + * @brief Constructs a CanListener object + * @details Initializes the CanListener. The constructor sets up internal variables + * The constructor shall intialize the mutex + */ + CanListener(); + + /** + * @brief Destructs a CanListener object + * @details Initializes the CanListener. The constructor sets up internal variables + * The destructor shall release the mutex + */ + ~CanListener(); + + /** + * @brief Initiate the CAN Bus listener on a Raw SocketCAN port + * @details This shall start the listenCanBusProcedure and updateJSONProcedure + */ + void listenCanBus(); + + /** + * @brief Thread procedure for listening to the CAN bus + * @details This thread shall store the latest CAN data in the m_canInfo unordered map + */ + void listenCanBusProcedure(); + + /** + * @brief Thread procedure for updating the global JSON object + * @details This thread shall update the JSON at UPDATE_CAN_JSON_PERIOD_MS + */ + void updateJSONProcedure(); +}; + +/** @} */ diff --git a/simulation/server/app/inc/can_scheduler.h b/simulation/server/app/inc/can_scheduler.h new file mode 100644 index 0000000..02d4938 --- /dev/null +++ b/simulation/server/app/inc/can_scheduler.h @@ -0,0 +1,342 @@ +#pragma once + +/************************************************************************************************ + * @file can_scheduler.h + * + * @brief Header file defining the CanScheduler class + * + * @date 2025-01-04 + * @author Aryan Kashem + ************************************************************************************************/ + +/** @warning This file is autogenerated */ + +/* Standard library Headers */ +#include +#include + +/* Inter-component Headers */ +#include +#include +#include +#include +#include +#include +#include +#include +#include + +/* Intra-component Headers */ + +/** + * @defgroup CanScheduler + * @brief SocketCAN Broadcast Manager abstraction class + * @{ + */ + +/** + * @class CanScheduler + * @brief Class that handles message scheduling over a SocketCAN interface + * @details This class is responsible scheduling CAN messages based on their cycle speed + * Only 3 cycle speeds are supported, Fast (1kHz), medium (10Hz) and slow (1Hz) + * The class shall support message updating during run-time for further bus simulation + */ +class CanScheduler { + private: + const std::string CAN_INTERFACE_NAME = "vcan0"; /**< SocketCAN interface name */ + + static const constexpr unsigned int FAST_CYCLE_SPEED_MS = 1U; /**< CAN fast cycle period in milliseconds */ + static const constexpr unsigned int MEDIUM_CYCLE_SPEED_MS = 100U; /**< CAN medium cycle period in milliseconds */ + static const constexpr unsigned int SLOW_CYCLE_SPEED_MS = 1000U; /**< CAN slow cycle period in milliseconds */ + + static const constexpr unsigned int SLOW_CYCLE_BCM_ID = 0U; /**< Linux Broadcast Manager Id for tracking fast cycle messages */ + static const constexpr unsigned int MEDIUM_CYCLE_BCM_ID = 1U; /**< Linux Broadcast Manager Id for tracking medium cycle messages */ + static const constexpr unsigned int FAST_CYCLE_BCM_ID = 2U; /**< Linux Broadcast Manager Id for tracking slow cycle messages */ + + static const constexpr unsigned int NUM_FAST_CYCLE_MESSAGES = 3U; /**< Number of fast cycle messages */ + static const constexpr unsigned int NUM_MEDIUM_CYCLE_MESSAGES = 5U; /**< Number of medium cycle messages */ + static const constexpr unsigned int NUM_SLOW_CYCLE_MESSAGES = 5U; /**< Number of slow cycle messages */ + static const constexpr unsigned int NUM_TOTAL_MESSAGES = 13U; /**< Total number of messages */ + static const constexpr unsigned int MAX_MESSAGE_LENGTH = 8U; /**< Max message length in bytes */ + + static const constexpr unsigned int FAST_BMS_CARRIER_BATTERY_VT_FRAME_INDEX = 0U; /**< Broadcast Manager battery_vt to Frame index mapping */ + static const constexpr unsigned int FAST_CAN_COMMUNICATION_FAST_ONE_SHOT_MSG_FRAME_INDEX = 1U; /**< Broadcast Manager fast_one_shot_msg to Frame index mapping */ + static const constexpr unsigned int FAST_CENTRE_CONSOLE_CC_PEDAL_FRAME_INDEX = 2U; /**< Broadcast Manager cc_pedal to Frame index mapping */ + + static const constexpr unsigned int MEDIUM_BMS_CARRIER_BATTERY_STATUS_FRAME_INDEX = 0U; /**< Broadcast Manager battery_status to Frame index mapping */ + static const constexpr unsigned int MEDIUM_CAN_COMMUNICATION_MEDIUM_ONE_SHOT_MSG_FRAME_INDEX = 1U; /**< Broadcast Manager medium_one_shot_msg to Frame index mapping */ + static const constexpr unsigned int MEDIUM_CENTRE_CONSOLE_CC_INFO_FRAME_INDEX = 2U; /**< Broadcast Manager cc_info to Frame index mapping */ + static const constexpr unsigned int MEDIUM_CENTRE_CONSOLE_CC_STEERING_FRAME_INDEX = 3U; /**< Broadcast Manager cc_steering to Frame index mapping */ + static const constexpr unsigned int MEDIUM_CENTRE_CONSOLE_CC_REGEN_PERCENTAGE_FRAME_INDEX = 4U; /**< Broadcast Manager cc_regen_percentage to Frame index mapping */ + + static const constexpr unsigned int SLOW_BMS_CARRIER_BATTERY_INFO_FRAME_INDEX = 0U; /**< Broadcast Manager battery_info to Frame index mapping */ + static const constexpr unsigned int SLOW_BMS_CARRIER_AFE1_STATUS_FRAME_INDEX = 1U; /**< Broadcast Manager afe1_status to Frame index mapping */ + static const constexpr unsigned int SLOW_BMS_CARRIER_AFE2_STATUS_FRAME_INDEX = 2U; /**< Broadcast Manager afe2_status to Frame index mapping */ + static const constexpr unsigned int SLOW_BMS_CARRIER_AFE3_STATUS_FRAME_INDEX = 3U; /**< Broadcast Manager afe3_status to Frame index mapping */ + static const constexpr unsigned int SLOW_CAN_COMMUNICATION_SLOW_ONE_SHOT_MSG_FRAME_INDEX = 4U; /**< Broadcast Manager slow_one_shot_msg to Frame index mapping */ + + /** + * @brief Fast cycle Broadcast Manager message for the Linux Kernel + */ + struct { + struct bcm_msg_head msg_head; /**< Broadcast Manager message head containing metadata */ + struct can_frame frame[NUM_FAST_CYCLE_MESSAGES]; /**< CAN message frames that shall be scheduled for fast cycle */ + } canFastCycleBCM; + + /** + * @brief Medium cycle Broadcast Manager message for the Linux Kernel + */ + struct { + struct bcm_msg_head msg_head; /**< Broadcast Manager message head containing metadata */ + struct can_frame frame[NUM_MEDIUM_CYCLE_MESSAGES]; /**< CAN message frames that shall be scheduled for medium cycle */ + } canMediumCycleBCM; + + /** + * @brief Slow cycle Broadcast Manager message for the Linux Kernel + */ + struct { + struct bcm_msg_head msg_head; /**< Broadcast Manager message head containing metadata */ + struct can_frame frame[NUM_SLOW_CYCLE_MESSAGES]; /**< CAN message frames that shall be scheduled for slow cycle */ + } canSlowCycleBCM; + + int m_bcmCanSocket; /**< The CAN schedulers Broadcast Manager socket FD */ + std::atomic m_isConnected; /**< Boolean flag to track the CAN schedulers connection status */ + + /** + * @brief Schedules all CAN data by updating the Broacast Manager socket + * @details This function is called by startCanScheduler + * This function shall initialize all CAN message values to 0 + */ + void scheduleCanMessages(); + + public: + /** + * @brief Constructs a CanScheduler object + * @details Initializes the CanScheduler. The constructor sets up internal variables + */ + CanScheduler(); + + /** + * @brief Starts the CAN scheduler and sets all messages to 0. Must only be called once + * @details This function will connect to the Linux Broadcast Manager + * This function must only be called once, and it will set all messages to 0 + */ + void startCanScheduler(); + /** + * @brief Update the CAN value for battery_status fault + * @param fault_value New value for the signal + */ + void update_battery_status_fault(uint16_t fault_value); + /** + * @brief Update the CAN value for battery_status fault_val + * @param fault_val_value New value for the signal + */ + void update_battery_status_fault_val(uint16_t fault_val_value); + /** + * @brief Update the CAN value for battery_status aux_batt_v + * @param aux_batt_v_value New value for the signal + */ + void update_battery_status_aux_batt_v(uint16_t aux_batt_v_value); + /** + * @brief Update the CAN value for battery_status afe_status + * @param afe_status_value New value for the signal + */ + void update_battery_status_afe_status(uint8_t afe_status_value); + /** + * @brief Update the CAN value for battery_vt voltage + * @param voltage_value New value for the signal + */ + void update_battery_vt_voltage(uint16_t voltage_value); + /** + * @brief Update the CAN value for battery_vt current + * @param current_value New value for the signal + */ + void update_battery_vt_current(uint16_t current_value); + /** + * @brief Update the CAN value for battery_vt temperature + * @param temperature_value New value for the signal + */ + void update_battery_vt_temperature(uint16_t temperature_value); + /** + * @brief Update the CAN value for battery_vt batt_perc + * @param batt_perc_value New value for the signal + */ + void update_battery_vt_batt_perc(uint16_t batt_perc_value); + /** + * @brief Update the CAN value for battery_info fan1 + * @param fan1_value New value for the signal + */ + void update_battery_info_fan1(uint8_t fan1_value); + /** + * @brief Update the CAN value for battery_info fan2 + * @param fan2_value New value for the signal + */ + void update_battery_info_fan2(uint8_t fan2_value); + /** + * @brief Update the CAN value for battery_info max_cell_v + * @param max_cell_v_value New value for the signal + */ + void update_battery_info_max_cell_v(uint16_t max_cell_v_value); + /** + * @brief Update the CAN value for battery_info min_cell_v + * @param min_cell_v_value New value for the signal + */ + void update_battery_info_min_cell_v(uint16_t min_cell_v_value); + /** + * @brief Update the CAN value for afe1_status id + * @param id_value New value for the signal + */ + void update_afe1_status_id(uint8_t id_value); + /** + * @brief Update the CAN value for afe1_status temp + * @param temp_value New value for the signal + */ + void update_afe1_status_temp(uint8_t temp_value); + /** + * @brief Update the CAN value for afe1_status v1 + * @param v1_value New value for the signal + */ + void update_afe1_status_v1(uint16_t v1_value); + /** + * @brief Update the CAN value for afe1_status v2 + * @param v2_value New value for the signal + */ + void update_afe1_status_v2(uint16_t v2_value); + /** + * @brief Update the CAN value for afe1_status v3 + * @param v3_value New value for the signal + */ + void update_afe1_status_v3(uint16_t v3_value); + /** + * @brief Update the CAN value for afe2_status id + * @param id_value New value for the signal + */ + void update_afe2_status_id(uint8_t id_value); + /** + * @brief Update the CAN value for afe2_status temp + * @param temp_value New value for the signal + */ + void update_afe2_status_temp(uint8_t temp_value); + /** + * @brief Update the CAN value for afe2_status v1 + * @param v1_value New value for the signal + */ + void update_afe2_status_v1(uint16_t v1_value); + /** + * @brief Update the CAN value for afe2_status v2 + * @param v2_value New value for the signal + */ + void update_afe2_status_v2(uint16_t v2_value); + /** + * @brief Update the CAN value for afe2_status v3 + * @param v3_value New value for the signal + */ + void update_afe2_status_v3(uint16_t v3_value); + /** + * @brief Update the CAN value for afe3_status id + * @param id_value New value for the signal + */ + void update_afe3_status_id(uint8_t id_value); + /** + * @brief Update the CAN value for afe3_status temp + * @param temp_value New value for the signal + */ + void update_afe3_status_temp(uint8_t temp_value); + /** + * @brief Update the CAN value for afe3_status v1 + * @param v1_value New value for the signal + */ + void update_afe3_status_v1(uint16_t v1_value); + /** + * @brief Update the CAN value for afe3_status v2 + * @param v2_value New value for the signal + */ + void update_afe3_status_v2(uint16_t v2_value); + /** + * @brief Update the CAN value for afe3_status v3 + * @param v3_value New value for the signal + */ + void update_afe3_status_v3(uint16_t v3_value); + /** + * @brief Update the CAN value for fast_one_shot_msg sig1 + * @param sig1_value New value for the signal + */ + void update_fast_one_shot_msg_sig1(uint16_t sig1_value); + /** + * @brief Update the CAN value for fast_one_shot_msg sig2 + * @param sig2_value New value for the signal + */ + void update_fast_one_shot_msg_sig2(uint16_t sig2_value); + /** + * @brief Update the CAN value for medium_one_shot_msg sig1 + * @param sig1_value New value for the signal + */ + void update_medium_one_shot_msg_sig1(uint16_t sig1_value); + /** + * @brief Update the CAN value for medium_one_shot_msg sig2 + * @param sig2_value New value for the signal + */ + void update_medium_one_shot_msg_sig2(uint16_t sig2_value); + /** + * @brief Update the CAN value for slow_one_shot_msg sig1 + * @param sig1_value New value for the signal + */ + void update_slow_one_shot_msg_sig1(uint16_t sig1_value); + /** + * @brief Update the CAN value for slow_one_shot_msg sig2 + * @param sig2_value New value for the signal + */ + void update_slow_one_shot_msg_sig2(uint16_t sig2_value); + /** + * @brief Update the CAN value for cc_pedal throttle_output + * @param throttle_output_value New value for the signal + */ + void update_cc_pedal_throttle_output(uint32_t throttle_output_value); + /** + * @brief Update the CAN value for cc_pedal brake_output + * @param brake_output_value New value for the signal + */ + void update_cc_pedal_brake_output(uint8_t brake_output_value); + /** + * @brief Update the CAN value for cc_info target_velocity + * @param target_velocity_value New value for the signal + */ + void update_cc_info_target_velocity(uint32_t target_velocity_value); + /** + * @brief Update the CAN value for cc_info drive_state + * @param drive_state_value New value for the signal + */ + void update_cc_info_drive_state(uint8_t drive_state_value); + /** + * @brief Update the CAN value for cc_info cruise_control + * @param cruise_control_value New value for the signal + */ + void update_cc_info_cruise_control(uint8_t cruise_control_value); + /** + * @brief Update the CAN value for cc_info regen_braking + * @param regen_braking_value New value for the signal + */ + void update_cc_info_regen_braking(uint8_t regen_braking_value); + /** + * @brief Update the CAN value for cc_info hazard_enabled + * @param hazard_enabled_value New value for the signal + */ + void update_cc_info_hazard_enabled(uint8_t hazard_enabled_value); + /** + * @brief Update the CAN value for cc_steering input_cc + * @param input_cc_value New value for the signal + */ + void update_cc_steering_input_cc(uint8_t input_cc_value); + /** + * @brief Update the CAN value for cc_steering input_lights + * @param input_lights_value New value for the signal + */ + void update_cc_steering_input_lights(uint8_t input_lights_value); + /** + * @brief Update the CAN value for cc_regen_percentage percent + * @param percent_value New value for the signal + */ + void update_cc_regen_percentage_percent(uint32_t percent_value); +}; + +/** @} */ \ No newline at end of file diff --git a/simulation/server/app/inc/gpio_manager.h b/simulation/server/app/inc/gpio_manager.h new file mode 100644 index 0000000..5adbf35 --- /dev/null +++ b/simulation/server/app/inc/gpio_manager.h @@ -0,0 +1,143 @@ +#pragma once + +/************************************************************************************************ + * @file gpio_manager.h + * + * @brief Header file defining the Server GpioManager class + * + * @date 2025-01-04 + * @author Aryan Kashem + ************************************************************************************************/ + +/* Standard library Headers */ +#include +#include + +#include + +/* Inter-component Headers */ +#include "gpio_datagram.h" + +/* Intra-component Headers */ + +/** + * @defgroup ServerGpioManager + * @brief GpioManager for the Server + * @{ + */ + +/** + * @class GpioManager + * @brief Class that manages receiving and transmitting Gpio commands and JSON logging + * @details This class is responsible for transmitting serialized messages for reading pin modes, + * alternate functions and states. It shall support reading all pins or individual pins + * The class shall cache current Gpio data using a hash-map + */ +class GpioManager { + private: + /** @brief Hash-map to store Pin specific data such as state, mode, or alternate function */ + using PinInfo = std::unordered_map; + + std::unordered_map m_gpioInfo; /**< Hash-map to cache all Gpio data */ + Datagram::Gpio m_gpioDatagram; /**< Datagram class to serialize/deserialize commands */ + + /** + * @brief Matches a string based on the received Gpio mode + * @param mode Received mode that shall be stringified + * @return String that matches the input mode + */ + std::string stringifyPinMode(Datagram::Gpio::Mode mode); + + /** + * @brief Matches a string based on the received Gpio mode + * @param altFunction Received alternate function that shall be stringified + * @return String that matches the input altFunction + */ + std::string stringifyPinAltFunction(Datagram::Gpio::AltFunction altFunction); + + /** + * @brief Loads the Hash-map cache with a projects Gpio data + * @param projectName Selector for which project shall be loaded into the cache + */ + void loadGpioInfo(std::string &projectName); + + /** + * @brief Saves the Hash-map cache to a projects Gpio data + * @param projectName Selector for which project shall be updated with the current cache + */ + void saveGpioInfo(std::string &projectName); + + public: + /** + * @brief Constructs a GpioManager object + * @details Default constructor + */ + GpioManager() = default; + + /** + * @brief Update a Gpio Pin state for a provided project given the data payload + * @details This function shall be called upon receiving a pin-specific payload + * Determining if this function is applicable is the responsibility of the terminal/UI + * @param projectName Name of the project to be updated + * @param payload Message data payload to be parsed + */ + void updateGpioPinState(std::string &projectName, std::string &payload); + + /** + * @brief Update all Gpio Pin states for a provided project given the data payload + * @details This function shall be called upon receiving a payload containing all pin data + * Determining if this function is applicable is the responsibility of the terminal/UI + * @param projectName Name of the project to be updated + * @param payload Message data payload to be parsed + */ + void updateGpioAllStates(std::string &projectName, std::string &payload); + + /** + * @brief Update a Gpio Pin mode for a provided project given the data payload + * @details This function shall be called upon receiving a payload containing all pin data + * Determining if this function is applicable is the responsibility of the terminal/UI + * @param projectName Name of the project to be updated + * @param payload Message data payload to be parsed + */ + void updateGpioPinMode(std::string &projectName, std::string &payload); + + /** + * @brief Update all Gpio Pin modes for a provided project given the data payload + * @details This function shall be called upon receiving a payload containing all pin data + * Determining if this function is applicable is the responsibility of the terminal/UI + * @param projectName Name of the project to be updated + * @param payload Message data payload to be parsed + */ + void updateGpioAllModes(std::string &projectName, std::string &payload); + + /** + * @brief Update a Gpio Pin alternate function for a provided project given the data payload + * @details This function shall be called upon receiving a payload containing all pin data + * Determining if this function is applicable is the responsibility of the terminal/UI + * @param projectName Name of the project to be updated + * @param payload Message data payload to be parsed + */ + void updateGpioPinAltFunction(std::string &projectName, std::string &payload); + + /** + * @brief Update all Gpio Pin alternate functions for a provided project given the data payload + * @details This function shall be called upon receiving a payload containing all pin data + * Determining if this function is applicable is the responsibility of the terminal/UI + * @param projectName Name of the project to be updated + * @param payload Message data payload to be parsed + */ + void updateGpioAllAltFunctions(std::string &projectName, std::string &payload); + + /** + * @brief Create a Gpio command given a CommandCode and if required, a specific Port/Pin map and data + * @details This function shall support all Gpio CommandCodes + * If the command is requesting data from all pins, no gpioPortPin and data is required + * @param commandCode Command reference to be transmitted to the client + * @param gpioPortPin Request reference to the port and pin. Must be in this form: 'A9' or 'B12' or 'C14' etc. + * @param data Data payload to be transmitted. This parameter currently only supports SET_STATE commands and can be set to 'LOW' or 'HIGH' + * @return Fully serialized data payload to be transmitted to the client + */ + std::string createGpioCommand(CommandCode commandCode, std::string &gpioPortPin, std::string data); +}; + +/** @} */ diff --git a/simulation/server/app/inc/i2c_manager.h b/simulation/server/app/inc/i2c_manager.h new file mode 100644 index 0000000..9c642f8 --- /dev/null +++ b/simulation/server/app/inc/i2c_manager.h @@ -0,0 +1,43 @@ +#pragma once + +/************************************************************************************************ + * @file i2c_manager.h + * + * @brief Header file defining the Server I2CManager class + * + * @date 2025-01-04 + * @author Aryan Kashem + ************************************************************************************************/ + +/* Standard library Headers */ +#include +#include + +/* Inter-component Headers */ +#include "i2c_datagram.h" + +/* Intra-component Headers */ + +/** + * @defgroup ServerI2CManager + * @brief I2CManager for the Server + * @{ + */ + +/** + * @class ServerI2CManager + * @brief Class that manages receiving and transmitting I2C commands and JSON logging + * @details This class is responsible for transmitting serialized messages for setting/retrieving RX/TX data buffers + * The class shall provide an interface to control I2C transactions + */ +class I2CManager { + private: + public: + /** + * @brief Constructs a I2CManager object + * @details Default constructor + */ + I2CManager() = default; +}; + +/** @} */ diff --git a/simulation/server/app/inc/spi_manager.h b/simulation/server/app/inc/spi_manager.h new file mode 100644 index 0000000..7093100 --- /dev/null +++ b/simulation/server/app/inc/spi_manager.h @@ -0,0 +1,43 @@ +#pragma once + +/************************************************************************************************ + * @file spi_manager.h + * + * @brief Header file defining the Server SPIManager class + * + * @date 2025-01-04 + * @author Aryan Kashem + ************************************************************************************************/ + +/* Standard library Headers */ +#include +#include + +/* Inter-component Headers */ +#include "spi_datagram.h" + +/* Intra-component Headers */ + +/** + * @defgroup ServerSPIManager + * @brief SPIManager for the Server + * @{ + */ + +/** + * @class ServerSPIManager + * @brief Class that manages receiving and transmitting SPI commands and JSON logging + * @details This class is responsible for transmitting serialized messages for setting/retrieving RX/TX data buffers + * The class shall provide an interface to control SPI transactions + */ +class SPIManager { + private: + public: + /** + * @brief Constructs a SPIManager object + * @details Default constructor + */ + SPIManager() = default; +}; + +/** @} */ diff --git a/simulation/server/app/src/app_callback.cc b/simulation/server/app/src/app_callback.cc new file mode 100644 index 0000000..052ab3e --- /dev/null +++ b/simulation/server/app/src/app_callback.cc @@ -0,0 +1,76 @@ +/************************************************************************************************ + * @file app_callback.cc + * + * @brief Source file defining the Application Callbacks for the server + * + * @date 2025-01-04 + * @author Aryan Kashem + ************************************************************************************************/ + +/* Standard library Headers */ +#include +#include + +/* Inter-component Headers */ +#include "command_code.h" +#include "gpio_datagram.h" +#include "i2c_datagram.h" +#include "json_manager.h" +#include "metadata.h" +#include "spi_datagram.h" + +/* Intra-component Headers */ +#include "app.h" +#include "app_callback.h" + +void applicationMessageCallback(Server *server, ClientConnection *client, std::string &message) { + std::string clientName = client->getClientName(); + + auto [commandCode, payload] = decodeCommand(message); + switch (commandCode) { + case CommandCode::METADATA: { + Datagram::Metadata clientMetadata; + clientMetadata.deserialize(payload); + + if (client->getClientName() != clientMetadata.getProjectName()) { + server->updateClientName(client, clientMetadata.getProjectName()); + } + + serverJSONManager.setProjectValue(client->getClientName(), "project_name", client->getClientName()); /* Get the updated name if there are duplicates */ + serverJSONManager.setProjectValue(client->getClientName(), "project_status", clientMetadata.getProjectStatus()); + serverJSONManager.setProjectValue(client->getClientName(), "hardware_model", clientMetadata.getHardwareModel()); + break; + } + case CommandCode::GPIO_GET_PIN_STATE: { + serverGpioManager.updateGpioPinState(clientName, payload); + break; + } + case CommandCode::GPIO_GET_ALL_STATES: { + serverGpioManager.updateGpioAllStates(clientName, payload); + break; + } + case CommandCode::GPIO_GET_PIN_MODE: { + serverGpioManager.updateGpioPinMode(clientName, payload); + break; + } + case CommandCode::GPIO_GET_ALL_MODES: { + serverGpioManager.updateGpioAllModes(clientName, payload); + break; + } + case CommandCode::GPIO_GET_PIN_ALT_FUNCTION: { + serverGpioManager.updateGpioPinAltFunction(clientName, payload); + break; + } + case CommandCode::GPIO_GET_ALL_ALT_FUNCTIONS: { + serverGpioManager.updateGpioAllAltFunctions(clientName, payload); + break; + } + default: { + break; + } + } +} + +void applicationConnectCallback(Server *server, ClientConnection *client) { + std::cout << "Connected to new client on address: " << client->getClientAddress() << std::endl; +} \ No newline at end of file diff --git a/simulation/server/app/src/app_terminal.cc b/simulation/server/app/src/app_terminal.cc new file mode 100644 index 0000000..16574a7 --- /dev/null +++ b/simulation/server/app/src/app_terminal.cc @@ -0,0 +1,138 @@ +/************************************************************************************************ + * @file app_terminal.cc + * + * @brief Source file defining the Application Terminal Class + * + * @date 2025-01-04 + * @author Aryan Kashem + ************************************************************************************************/ + +/* Standard library Headers */ +#include +#include +#include +#include + +/* Inter-component Headers */ +#include "command_code.h" + +/* Intra-component Headers */ +#include "app.h" +#include "app_terminal.h" + +Terminal::Terminal(Server *server) { + m_Server = server; +} + +std::string Terminal::toLower(const std::string &input) { + std::string lowered = input; + std::transform(lowered.begin(), lowered.end(), lowered.begin(), [](unsigned char c) { return std::tolower(c); }); + return lowered; +} + +void Terminal::handleGpioCommands(const std::string &action, std::vector &tokens) { + std::string message; + if (action == "get_pin_state" && tokens.size() >= 3) { + message = serverGpioManager.createGpioCommand(CommandCode::GPIO_GET_PIN_STATE, tokens[2], ""); + } else if (action == "get_all_states" && tokens.size() >= 2) { + message = serverGpioManager.createGpioCommand(CommandCode::GPIO_GET_ALL_STATES, tokens[0], ""); + } else if (action == "get_pin_mode" && tokens.size() >= 3) { + message = serverGpioManager.createGpioCommand(CommandCode::GPIO_GET_PIN_MODE, tokens[2], ""); + } else if (action == "get_all_modes" && tokens.size() >= 2) { + message = serverGpioManager.createGpioCommand(CommandCode::GPIO_GET_ALL_MODES, tokens[0], ""); + } else if (action == "get_pin_alt_function" && tokens.size() >= 3) { + message = serverGpioManager.createGpioCommand(CommandCode::GPIO_GET_PIN_ALT_FUNCTION, tokens[2], ""); + } else if (action == "get_all_alt_functions" && tokens.size() >= 2) { + message = serverGpioManager.createGpioCommand(CommandCode::GPIO_GET_ALL_ALT_FUNCTIONS, tokens[0], ""); + } else if (action == "set_pin_state" && tokens.size() >= 4) { + message = serverGpioManager.createGpioCommand(CommandCode::GPIO_SET_PIN_STATE, tokens[2], tokens[3]); + } else if (action == "set_all_states" && tokens.size() >= 3) { + message = serverGpioManager.createGpioCommand(CommandCode::GPIO_SET_ALL_STATES, tokens[0], tokens[2]); + } else { + std::cerr << "Unsupported action: " << action << std::endl; + } + + if (!message.empty()) { + m_Server->sendMessage(m_targetClient, message); + } else { + std::cout << "Invalid command. Refer to sim_command.md" << std::endl; + } + m_targetClient = nullptr; +} + +void Terminal::parseCommand(std::vector &tokens) { + if (tokens.size() < 2) { + std::cout << "Invalid command. Format: \n"; + return; + } + + std::string interface = toLower(tokens[0]); + std::string action = toLower(tokens[1]); + + try { + if (interface == "gpio") { + handleGpioCommands(action, tokens); + } else if (interface == "i2c") { + } else if (interface == "spi") { + } else { + std::cerr << "Unsupported interface: " << interface << std::endl; + } + } catch (const std::exception &e) { + std::cerr << "Terminal command error: " << e.what() << std::endl; + } +} + +void Terminal::run() { + std::string input; + + while (true) { + std::cout << "Client List:" << std::endl; + std::cout << "------------" << std::endl; + m_Server->dumpClientList(); + std::cout << "------------" << std::endl; + + std::cout << "Select Client by Name (Enter to refresh) > "; + std::getline(std::cin, input); + + input.erase(0, input.find_first_not_of(" \t")); + input.erase(input.find_last_not_of(" \t") + 1); + + if (toLower(input) == "exit") { + break; + } + + m_targetClient = m_Server->getClientByName(input); + + if (m_targetClient == nullptr) { + std::cout << "Invalid client selection." << std::endl << std::endl; + continue; + } + std::cout << "Selected " << m_targetClient->getClientName() << std::endl << std::endl; + + std::cout << "Enter commmand > "; + std::getline(std::cin, input); + + input.erase(0, input.find_first_not_of(" \t")); + input.erase(input.find_last_not_of(" \t") + 1); + + if (toLower(input) == "exit") { + break; + } + + /** + * Create tokens out of input string + * Input = "GPIO GET_PIN_STATE A9" + * tokens = ["GPIO", "GET_PIN_STATE", "A9"] + */ + std::vector tokens; + std::istringstream iss(input); + std::string token; + while (iss >> token) { + tokens.push_back(token); + } + + if (!tokens.empty()) { + parseCommand(tokens); + } + } +} diff --git a/simulation/server/app/src/can_listener.cc b/simulation/server/app/src/can_listener.cc new file mode 100644 index 0000000..befc073 --- /dev/null +++ b/simulation/server/app/src/can_listener.cc @@ -0,0 +1,123 @@ +/************************************************************************************************ + * @file can_message_handler.cc + * + * @brief Source file defining the CanListener Class + * + * @date 2025-01-04 + * @author Aryan Kashem + ************************************************************************************************/ + +/* Standard library Headers */ +#include +#include + +/* Inter-component Headers */ +#include + +#include "system_can.h" +#include "thread_helpers.h" + +/* Intra-component Headers */ +#include "app.h" +#include "can_listener.h" + +#define CAN_MESSAGE_JSON_KEY "messages" + +CanListener::CanListener() { + m_isListening = false; + m_rawCanSocket = -1; + pthread_mutex_init(&m_mutex, nullptr); +} + +CanListener::~CanListener() { + pthread_mutex_destroy(&m_mutex); +} + +void CanListener::listenCanBusProcedure() { + m_rawCanSocket = socket(PF_CAN, SOCK_RAW, CAN_RAW); + + if (m_rawCanSocket < 0) { + throw std::runtime_error("Error creating raw CAN socket"); + } + + struct ifreq ifr; + strcpy(ifr.ifr_name, CAN_INTERFACE_NAME.c_str()); + if (ioctl(m_rawCanSocket, SIOCGIFINDEX, &ifr) < 0) { + throw std::runtime_error("Error binding raw CAN socket to interface"); + } + + struct sockaddr_can addr = {}; + addr.can_family = AF_CAN; + addr.can_ifindex = ifr.ifr_ifindex; + + if (bind(m_rawCanSocket, (struct sockaddr *)&addr, sizeof(addr)) < 0) { + throw std::runtime_error("Error binding raw CAN socket"); + } + + m_isListening = true; + + struct can_frame canFrame; + int numBytes; + + while (m_isListening) { + numBytes = read(m_rawCanSocket, &canFrame, sizeof(struct can_frame)); + + if (numBytes < 0) { + throw std::runtime_error("Error reading CAN data"); + break; + } + + pthread_mutex_lock(&m_mutex); + canMessageHandler(canFrame.can_id, canFrame.data); + pthread_mutex_unlock(&m_mutex); + } + + close(m_rawCanSocket); + m_isListening = false; +} + +void CanListener::updateJSONProcedure() { + while (m_isListening) { + pthread_mutex_lock(&m_mutex); + serverJSONManager.setProjectValue(CAN_JSON_NAME, CAN_MESSAGE_JSON_KEY, m_canInfo); + pthread_mutex_unlock(&m_mutex); + + thread_sleep_ms(UPDATE_CAN_JSON_PERIOD_MS); + } +} + +void *listenCanBusWrapper(void *param) { + CanListener *canListener = static_cast(param); + + try { + canListener->listenCanBusProcedure(); + } catch (std::exception &e) { + std::cerr << "Can Listener Listener Thread Error: " << e.what() << std::endl; + } + + return nullptr; +} + +void *updateJSONWrapper(void *param) { + CanListener *canListener = static_cast(param); + + try { + canListener->updateJSONProcedure(); + } catch (std::exception &e) { + std::cerr << "Can Listener Update JSON Thread Error: " << e.what() << std::endl; + } + + return nullptr; +} + +void CanListener::listenCanBus() { + if (m_isListening) return; + + if (pthread_create(&m_listenCanBusId, nullptr, listenCanBusWrapper, this)) { + throw std::runtime_error("CAN listener thread creation error"); + } + + if (pthread_create(&m_updateJSONId, nullptr, updateJSONWrapper, this)) { + throw std::runtime_error("CAN update JSON thread creation error"); + } +} diff --git a/simulation/server/app/src/can_message_handler.cc b/simulation/server/app/src/can_message_handler.cc new file mode 100644 index 0000000..b593979 --- /dev/null +++ b/simulation/server/app/src/can_message_handler.cc @@ -0,0 +1,872 @@ +/************************************************************************************************ + * @file can_message_handler.cc + * + * @brief Source file defining the Can Message Handler function + * + * @date 2025-01-07 + * @author Aryan Kashem + ************************************************************************************************/ + +/** @warning This file is autogenerated */ + +/* Standard library Headers */ +#include +#include +#include +#include + +/* Inter-component Headers */ +#include + +#include "system_can.h" + +/* Intra-component Headers */ +#include "can_listener.h" + +/** + * @brief Storage class for battery_status CAN message + */ +struct battery_status { + uint16_t fault; /**< CAN signal 'fault' defined in *.yaml */ + uint16_t fault_val; /**< CAN signal 'fault_val' defined in *.yaml */ + uint16_t aux_batt_v; /**< CAN signal 'aux_batt_v' defined in *.yaml */ + uint8_t afe_status; /**< CAN signal 'afe_status' defined in *.yaml */ + + /** + * @brief Decode new CAN data and update the storage for battery_status + * @param data Pointer to the CAN message to be decoded + */ + void decode(const uint8_t *data) { + uint64_t raw_val = 0U; + uint8_t start_byte = 0U; + { + raw_val = 0U; + start_byte = 0; + raw_val |= static_cast(data[start_byte + 0]) << 0U; + raw_val |= static_cast(data[start_byte + 1]) << 8U; + + fault = raw_val; + } + { + raw_val = 0U; + start_byte = 2; + raw_val |= static_cast(data[start_byte + 0]) << 0U; + raw_val |= static_cast(data[start_byte + 1]) << 8U; + + fault_val = raw_val; + } + { + raw_val = 0U; + start_byte = 4; + raw_val |= static_cast(data[start_byte + 0]) << 0U; + raw_val |= static_cast(data[start_byte + 1]) << 8U; + + aux_batt_v = raw_val; + } + { + raw_val = 0U; + start_byte = 6; + raw_val |= static_cast(data[start_byte + 0]) << 0U; + + afe_status = raw_val; + } + } + + /** + * @brief Create a JSON object for battery_status using the storage + */ + nlohmann::json to_json() const { + return { { "fault", fault }, { "fault_val", fault_val }, { "aux_batt_v", aux_batt_v }, { "afe_status", afe_status } }; + } + + /** + * @brief Get the message name: battery_status + * @return Returns the message name + */ + std::string get_message_name() const { + return "battery_status"; + } +}; +/** + * @brief Storage class for battery_vt CAN message + */ +struct battery_vt { + uint16_t voltage; /**< CAN signal 'voltage' defined in *.yaml */ + uint16_t current; /**< CAN signal 'current' defined in *.yaml */ + uint16_t temperature; /**< CAN signal 'temperature' defined in *.yaml */ + uint16_t batt_perc; /**< CAN signal 'batt_perc' defined in *.yaml */ + + /** + * @brief Decode new CAN data and update the storage for battery_vt + * @param data Pointer to the CAN message to be decoded + */ + void decode(const uint8_t *data) { + uint64_t raw_val = 0U; + uint8_t start_byte = 0U; + { + raw_val = 0U; + start_byte = 0; + raw_val |= static_cast(data[start_byte + 0]) << 0U; + raw_val |= static_cast(data[start_byte + 1]) << 8U; + + voltage = raw_val; + } + { + raw_val = 0U; + start_byte = 2; + raw_val |= static_cast(data[start_byte + 0]) << 0U; + raw_val |= static_cast(data[start_byte + 1]) << 8U; + + current = raw_val; + } + { + raw_val = 0U; + start_byte = 4; + raw_val |= static_cast(data[start_byte + 0]) << 0U; + raw_val |= static_cast(data[start_byte + 1]) << 8U; + + temperature = raw_val; + } + { + raw_val = 0U; + start_byte = 6; + raw_val |= static_cast(data[start_byte + 0]) << 0U; + raw_val |= static_cast(data[start_byte + 1]) << 8U; + + batt_perc = raw_val; + } + } + + /** + * @brief Create a JSON object for battery_vt using the storage + */ + nlohmann::json to_json() const { + return { { "voltage", voltage }, { "current", current }, { "temperature", temperature }, { "batt_perc", batt_perc } }; + } + + /** + * @brief Get the message name: battery_vt + * @return Returns the message name + */ + std::string get_message_name() const { + return "battery_vt"; + } +}; +/** + * @brief Storage class for battery_info CAN message + */ +struct battery_info { + uint8_t fan1; /**< CAN signal 'fan1' defined in *.yaml */ + uint8_t fan2; /**< CAN signal 'fan2' defined in *.yaml */ + uint16_t max_cell_v; /**< CAN signal 'max_cell_v' defined in *.yaml */ + uint16_t min_cell_v; /**< CAN signal 'min_cell_v' defined in *.yaml */ + + /** + * @brief Decode new CAN data and update the storage for battery_info + * @param data Pointer to the CAN message to be decoded + */ + void decode(const uint8_t *data) { + uint64_t raw_val = 0U; + uint8_t start_byte = 0U; + { + raw_val = 0U; + start_byte = 0; + raw_val |= static_cast(data[start_byte + 0]) << 0U; + + fan1 = raw_val; + } + { + raw_val = 0U; + start_byte = 1; + raw_val |= static_cast(data[start_byte + 0]) << 0U; + + fan2 = raw_val; + } + { + raw_val = 0U; + start_byte = 2; + raw_val |= static_cast(data[start_byte + 0]) << 0U; + raw_val |= static_cast(data[start_byte + 1]) << 8U; + + max_cell_v = raw_val; + } + { + raw_val = 0U; + start_byte = 4; + raw_val |= static_cast(data[start_byte + 0]) << 0U; + raw_val |= static_cast(data[start_byte + 1]) << 8U; + + min_cell_v = raw_val; + } + } + + /** + * @brief Create a JSON object for battery_info using the storage + */ + nlohmann::json to_json() const { + return { { "fan1", fan1 }, { "fan2", fan2 }, { "max_cell_v", max_cell_v }, { "min_cell_v", min_cell_v } }; + } + + /** + * @brief Get the message name: battery_info + * @return Returns the message name + */ + std::string get_message_name() const { + return "battery_info"; + } +}; +/** + * @brief Storage class for afe1_status CAN message + */ +struct afe1_status { + uint8_t id; /**< CAN signal 'id' defined in *.yaml */ + uint8_t temp; /**< CAN signal 'temp' defined in *.yaml */ + uint16_t v1; /**< CAN signal 'v1' defined in *.yaml */ + uint16_t v2; /**< CAN signal 'v2' defined in *.yaml */ + uint16_t v3; /**< CAN signal 'v3' defined in *.yaml */ + + /** + * @brief Decode new CAN data and update the storage for AFE1_status + * @param data Pointer to the CAN message to be decoded + */ + void decode(const uint8_t *data) { + uint64_t raw_val = 0U; + uint8_t start_byte = 0U; + { + raw_val = 0U; + start_byte = 0; + raw_val |= static_cast(data[start_byte + 0]) << 0U; + + id = raw_val; + } + { + raw_val = 0U; + start_byte = 1; + raw_val |= static_cast(data[start_byte + 0]) << 0U; + + temp = raw_val; + } + { + raw_val = 0U; + start_byte = 2; + raw_val |= static_cast(data[start_byte + 0]) << 0U; + raw_val |= static_cast(data[start_byte + 1]) << 8U; + + v1 = raw_val; + } + { + raw_val = 0U; + start_byte = 4; + raw_val |= static_cast(data[start_byte + 0]) << 0U; + raw_val |= static_cast(data[start_byte + 1]) << 8U; + + v2 = raw_val; + } + { + raw_val = 0U; + start_byte = 6; + raw_val |= static_cast(data[start_byte + 0]) << 0U; + raw_val |= static_cast(data[start_byte + 1]) << 8U; + + v3 = raw_val; + } + } + + /** + * @brief Create a JSON object for AFE1_status using the storage + */ + nlohmann::json to_json() const { + return { { "id", id }, { "temp", temp }, { "v1", v1 }, { "v2", v2 }, { "v3", v3 } }; + } + + /** + * @brief Get the message name: AFE1_status + * @return Returns the message name + */ + std::string get_message_name() const { + return "AFE1_status"; + } +}; +/** + * @brief Storage class for afe2_status CAN message + */ +struct afe2_status { + uint8_t id; /**< CAN signal 'id' defined in *.yaml */ + uint8_t temp; /**< CAN signal 'temp' defined in *.yaml */ + uint16_t v1; /**< CAN signal 'v1' defined in *.yaml */ + uint16_t v2; /**< CAN signal 'v2' defined in *.yaml */ + uint16_t v3; /**< CAN signal 'v3' defined in *.yaml */ + + /** + * @brief Decode new CAN data and update the storage for AFE2_status + * @param data Pointer to the CAN message to be decoded + */ + void decode(const uint8_t *data) { + uint64_t raw_val = 0U; + uint8_t start_byte = 0U; + { + raw_val = 0U; + start_byte = 0; + raw_val |= static_cast(data[start_byte + 0]) << 0U; + + id = raw_val; + } + { + raw_val = 0U; + start_byte = 1; + raw_val |= static_cast(data[start_byte + 0]) << 0U; + + temp = raw_val; + } + { + raw_val = 0U; + start_byte = 2; + raw_val |= static_cast(data[start_byte + 0]) << 0U; + raw_val |= static_cast(data[start_byte + 1]) << 8U; + + v1 = raw_val; + } + { + raw_val = 0U; + start_byte = 4; + raw_val |= static_cast(data[start_byte + 0]) << 0U; + raw_val |= static_cast(data[start_byte + 1]) << 8U; + + v2 = raw_val; + } + { + raw_val = 0U; + start_byte = 6; + raw_val |= static_cast(data[start_byte + 0]) << 0U; + raw_val |= static_cast(data[start_byte + 1]) << 8U; + + v3 = raw_val; + } + } + + /** + * @brief Create a JSON object for AFE2_status using the storage + */ + nlohmann::json to_json() const { + return { { "id", id }, { "temp", temp }, { "v1", v1 }, { "v2", v2 }, { "v3", v3 } }; + } + + /** + * @brief Get the message name: AFE2_status + * @return Returns the message name + */ + std::string get_message_name() const { + return "AFE2_status"; + } +}; +/** + * @brief Storage class for afe3_status CAN message + */ +struct afe3_status { + uint8_t id; /**< CAN signal 'id' defined in *.yaml */ + uint8_t temp; /**< CAN signal 'temp' defined in *.yaml */ + uint16_t v1; /**< CAN signal 'v1' defined in *.yaml */ + uint16_t v2; /**< CAN signal 'v2' defined in *.yaml */ + uint16_t v3; /**< CAN signal 'v3' defined in *.yaml */ + + /** + * @brief Decode new CAN data and update the storage for AFE3_status + * @param data Pointer to the CAN message to be decoded + */ + void decode(const uint8_t *data) { + uint64_t raw_val = 0U; + uint8_t start_byte = 0U; + { + raw_val = 0U; + start_byte = 0; + raw_val |= static_cast(data[start_byte + 0]) << 0U; + + id = raw_val; + } + { + raw_val = 0U; + start_byte = 1; + raw_val |= static_cast(data[start_byte + 0]) << 0U; + + temp = raw_val; + } + { + raw_val = 0U; + start_byte = 2; + raw_val |= static_cast(data[start_byte + 0]) << 0U; + raw_val |= static_cast(data[start_byte + 1]) << 8U; + + v1 = raw_val; + } + { + raw_val = 0U; + start_byte = 4; + raw_val |= static_cast(data[start_byte + 0]) << 0U; + raw_val |= static_cast(data[start_byte + 1]) << 8U; + + v2 = raw_val; + } + { + raw_val = 0U; + start_byte = 6; + raw_val |= static_cast(data[start_byte + 0]) << 0U; + raw_val |= static_cast(data[start_byte + 1]) << 8U; + + v3 = raw_val; + } + } + + /** + * @brief Create a JSON object for AFE3_status using the storage + */ + nlohmann::json to_json() const { + return { { "id", id }, { "temp", temp }, { "v1", v1 }, { "v2", v2 }, { "v3", v3 } }; + } + + /** + * @brief Get the message name: AFE3_status + * @return Returns the message name + */ + std::string get_message_name() const { + return "AFE3_status"; + } +}; +/** + * @brief Storage class for fast_one_shot_msg CAN message + */ +struct fast_one_shot_msg { + uint16_t sig1; /**< CAN signal 'sig1' defined in *.yaml */ + uint16_t sig2; /**< CAN signal 'sig2' defined in *.yaml */ + + /** + * @brief Decode new CAN data and update the storage for fast_one_shot_msg + * @param data Pointer to the CAN message to be decoded + */ + void decode(const uint8_t *data) { + uint64_t raw_val = 0U; + uint8_t start_byte = 0U; + { + raw_val = 0U; + start_byte = 0; + raw_val |= static_cast(data[start_byte + 0]) << 0U; + raw_val |= static_cast(data[start_byte + 1]) << 8U; + + sig1 = raw_val; + } + { + raw_val = 0U; + start_byte = 2; + raw_val |= static_cast(data[start_byte + 0]) << 0U; + raw_val |= static_cast(data[start_byte + 1]) << 8U; + + sig2 = raw_val; + } + } + + /** + * @brief Create a JSON object for fast_one_shot_msg using the storage + */ + nlohmann::json to_json() const { + return { { "sig1", sig1 }, { "sig2", sig2 } }; + } + + /** + * @brief Get the message name: fast_one_shot_msg + * @return Returns the message name + */ + std::string get_message_name() const { + return "fast_one_shot_msg"; + } +}; +/** + * @brief Storage class for medium_one_shot_msg CAN message + */ +struct medium_one_shot_msg { + uint16_t sig1; /**< CAN signal 'sig1' defined in *.yaml */ + uint16_t sig2; /**< CAN signal 'sig2' defined in *.yaml */ + + /** + * @brief Decode new CAN data and update the storage for medium_one_shot_msg + * @param data Pointer to the CAN message to be decoded + */ + void decode(const uint8_t *data) { + uint64_t raw_val = 0U; + uint8_t start_byte = 0U; + { + raw_val = 0U; + start_byte = 0; + raw_val |= static_cast(data[start_byte + 0]) << 0U; + raw_val |= static_cast(data[start_byte + 1]) << 8U; + + sig1 = raw_val; + } + { + raw_val = 0U; + start_byte = 2; + raw_val |= static_cast(data[start_byte + 0]) << 0U; + raw_val |= static_cast(data[start_byte + 1]) << 8U; + + sig2 = raw_val; + } + } + + /** + * @brief Create a JSON object for medium_one_shot_msg using the storage + */ + nlohmann::json to_json() const { + return { { "sig1", sig1 }, { "sig2", sig2 } }; + } + + /** + * @brief Get the message name: medium_one_shot_msg + * @return Returns the message name + */ + std::string get_message_name() const { + return "medium_one_shot_msg"; + } +}; +/** + * @brief Storage class for slow_one_shot_msg CAN message + */ +struct slow_one_shot_msg { + uint16_t sig1; /**< CAN signal 'sig1' defined in *.yaml */ + uint16_t sig2; /**< CAN signal 'sig2' defined in *.yaml */ + + /** + * @brief Decode new CAN data and update the storage for slow_one_shot_msg + * @param data Pointer to the CAN message to be decoded + */ + void decode(const uint8_t *data) { + uint64_t raw_val = 0U; + uint8_t start_byte = 0U; + { + raw_val = 0U; + start_byte = 0; + raw_val |= static_cast(data[start_byte + 0]) << 0U; + raw_val |= static_cast(data[start_byte + 1]) << 8U; + + sig1 = raw_val; + } + { + raw_val = 0U; + start_byte = 2; + raw_val |= static_cast(data[start_byte + 0]) << 0U; + raw_val |= static_cast(data[start_byte + 1]) << 8U; + + sig2 = raw_val; + } + } + + /** + * @brief Create a JSON object for slow_one_shot_msg using the storage + */ + nlohmann::json to_json() const { + return { { "sig1", sig1 }, { "sig2", sig2 } }; + } + + /** + * @brief Get the message name: slow_one_shot_msg + * @return Returns the message name + */ + std::string get_message_name() const { + return "slow_one_shot_msg"; + } +}; +/** + * @brief Storage class for cc_pedal CAN message + */ +struct cc_pedal { + uint32_t throttle_output; /**< CAN signal 'throttle_output' defined in *.yaml */ + uint8_t brake_output; /**< CAN signal 'brake_output' defined in *.yaml */ + + /** + * @brief Decode new CAN data and update the storage for cc_pedal + * @param data Pointer to the CAN message to be decoded + */ + void decode(const uint8_t *data) { + uint64_t raw_val = 0U; + uint8_t start_byte = 0U; + { + raw_val = 0U; + start_byte = 0; + raw_val |= static_cast(data[start_byte + 0]) << 0U; + raw_val |= static_cast(data[start_byte + 1]) << 8U; + raw_val |= static_cast(data[start_byte + 2]) << 16U; + raw_val |= static_cast(data[start_byte + 3]) << 24U; + + throttle_output = raw_val; + } + { + raw_val = 0U; + start_byte = 4; + raw_val |= static_cast(data[start_byte + 0]) << 0U; + + brake_output = raw_val; + } + } + + /** + * @brief Create a JSON object for cc_pedal using the storage + */ + nlohmann::json to_json() const { + return { { "throttle_output", throttle_output }, { "brake_output", brake_output } }; + } + + /** + * @brief Get the message name: cc_pedal + * @return Returns the message name + */ + std::string get_message_name() const { + return "cc_pedal"; + } +}; +/** + * @brief Storage class for cc_info CAN message + */ +struct cc_info { + uint32_t target_velocity; /**< CAN signal 'target_velocity' defined in *.yaml */ + uint8_t drive_state; /**< CAN signal 'drive_state' defined in *.yaml */ + uint8_t cruise_control; /**< CAN signal 'cruise_control' defined in *.yaml */ + uint8_t regen_braking; /**< CAN signal 'regen_braking' defined in *.yaml */ + uint8_t hazard_enabled; /**< CAN signal 'hazard_enabled' defined in *.yaml */ + + /** + * @brief Decode new CAN data and update the storage for cc_info + * @param data Pointer to the CAN message to be decoded + */ + void decode(const uint8_t *data) { + uint64_t raw_val = 0U; + uint8_t start_byte = 0U; + { + raw_val = 0U; + start_byte = 0; + raw_val |= static_cast(data[start_byte + 0]) << 0U; + raw_val |= static_cast(data[start_byte + 1]) << 8U; + raw_val |= static_cast(data[start_byte + 2]) << 16U; + raw_val |= static_cast(data[start_byte + 3]) << 24U; + + target_velocity = raw_val; + } + { + raw_val = 0U; + start_byte = 4; + raw_val |= static_cast(data[start_byte + 0]) << 0U; + + drive_state = raw_val; + } + { + raw_val = 0U; + start_byte = 5; + raw_val |= static_cast(data[start_byte + 0]) << 0U; + + cruise_control = raw_val; + } + { + raw_val = 0U; + start_byte = 6; + raw_val |= static_cast(data[start_byte + 0]) << 0U; + + regen_braking = raw_val; + } + { + raw_val = 0U; + start_byte = 7; + raw_val |= static_cast(data[start_byte + 0]) << 0U; + + hazard_enabled = raw_val; + } + } + + /** + * @brief Create a JSON object for cc_info using the storage + */ + nlohmann::json to_json() const { + return { { "target_velocity", target_velocity }, { "drive_state", drive_state }, { "cruise_control", cruise_control }, { "regen_braking", regen_braking }, { "hazard_enabled", hazard_enabled } }; + } + + /** + * @brief Get the message name: cc_info + * @return Returns the message name + */ + std::string get_message_name() const { + return "cc_info"; + } +}; +/** + * @brief Storage class for cc_steering CAN message + */ +struct cc_steering { + uint8_t input_cc; /**< CAN signal 'input_cc' defined in *.yaml */ + uint8_t input_lights; /**< CAN signal 'input_lights' defined in *.yaml */ + + /** + * @brief Decode new CAN data and update the storage for cc_steering + * @param data Pointer to the CAN message to be decoded + */ + void decode(const uint8_t *data) { + uint64_t raw_val = 0U; + uint8_t start_byte = 0U; + { + raw_val = 0U; + start_byte = 0; + raw_val |= static_cast(data[start_byte + 0]) << 0U; + + input_cc = raw_val; + } + { + raw_val = 0U; + start_byte = 1; + raw_val |= static_cast(data[start_byte + 0]) << 0U; + + input_lights = raw_val; + } + } + + /** + * @brief Create a JSON object for cc_steering using the storage + */ + nlohmann::json to_json() const { + return { { "input_cc", input_cc }, { "input_lights", input_lights } }; + } + + /** + * @brief Get the message name: cc_steering + * @return Returns the message name + */ + std::string get_message_name() const { + return "cc_steering"; + } +}; +/** + * @brief Storage class for cc_regen_percentage CAN message + */ +struct cc_regen_percentage { + uint32_t percent; /**< CAN signal 'percent' defined in *.yaml */ + + /** + * @brief Decode new CAN data and update the storage for cc_regen_percentage + * @param data Pointer to the CAN message to be decoded + */ + void decode(const uint8_t *data) { + uint64_t raw_val = 0U; + uint8_t start_byte = 0U; + { + raw_val = 0U; + start_byte = 0; + raw_val |= static_cast(data[start_byte + 0]) << 0U; + raw_val |= static_cast(data[start_byte + 1]) << 8U; + raw_val |= static_cast(data[start_byte + 2]) << 16U; + raw_val |= static_cast(data[start_byte + 3]) << 24U; + + percent = raw_val; + } + } + + /** + * @brief Create a JSON object for cc_regen_percentage using the storage + */ + nlohmann::json to_json() const { + return { { "percent", percent } }; + } + + /** + * @brief Get the message name: cc_regen_percentage + * @return Returns the message name + */ + std::string get_message_name() const { + return "cc_regen_percentage"; + } +}; + +/** + * @brief Main CAN message Handler + * @details This function shall dynamically allocate a struct based on the CAN ID + * This function shall decode the data, then update the CAN Cache 'm_canInfo' with JSON data + * @param id Can message ID + * @param data Pointer to the CAN message data + */ +void CanListener::canMessageHandler(uint32_t id, const uint8_t *data) { + switch (id) { + case SYSTEM_CAN_MESSAGE_BMS_CARRIER_BATTERY_STATUS: { + battery_status *message = new battery_status(); + message->decode(data); + m_canInfo[message->get_message_name()] = message->to_json(); + break; + } + case SYSTEM_CAN_MESSAGE_BMS_CARRIER_BATTERY_VT: { + battery_vt *message = new battery_vt(); + message->decode(data); + m_canInfo[message->get_message_name()] = message->to_json(); + break; + } + case SYSTEM_CAN_MESSAGE_BMS_CARRIER_BATTERY_INFO: { + battery_info *message = new battery_info(); + message->decode(data); + m_canInfo[message->get_message_name()] = message->to_json(); + break; + } + case SYSTEM_CAN_MESSAGE_BMS_CARRIER_AFE1_STATUS: { + afe1_status *message = new afe1_status(); + message->decode(data); + m_canInfo[message->get_message_name()] = message->to_json(); + break; + } + case SYSTEM_CAN_MESSAGE_BMS_CARRIER_AFE2_STATUS: { + afe2_status *message = new afe2_status(); + message->decode(data); + m_canInfo[message->get_message_name()] = message->to_json(); + break; + } + case SYSTEM_CAN_MESSAGE_BMS_CARRIER_AFE3_STATUS: { + afe3_status *message = new afe3_status(); + message->decode(data); + m_canInfo[message->get_message_name()] = message->to_json(); + break; + } + case SYSTEM_CAN_MESSAGE_CAN_COMMUNICATION_FAST_ONE_SHOT_MSG: { + fast_one_shot_msg *message = new fast_one_shot_msg(); + message->decode(data); + m_canInfo[message->get_message_name()] = message->to_json(); + break; + } + case SYSTEM_CAN_MESSAGE_CAN_COMMUNICATION_MEDIUM_ONE_SHOT_MSG: { + medium_one_shot_msg *message = new medium_one_shot_msg(); + message->decode(data); + m_canInfo[message->get_message_name()] = message->to_json(); + break; + } + case SYSTEM_CAN_MESSAGE_CAN_COMMUNICATION_SLOW_ONE_SHOT_MSG: { + slow_one_shot_msg *message = new slow_one_shot_msg(); + message->decode(data); + m_canInfo[message->get_message_name()] = message->to_json(); + break; + } + case SYSTEM_CAN_MESSAGE_CENTRE_CONSOLE_CC_PEDAL: { + cc_pedal *message = new cc_pedal(); + message->decode(data); + m_canInfo[message->get_message_name()] = message->to_json(); + break; + } + case SYSTEM_CAN_MESSAGE_CENTRE_CONSOLE_CC_INFO: { + cc_info *message = new cc_info(); + message->decode(data); + m_canInfo[message->get_message_name()] = message->to_json(); + break; + } + case SYSTEM_CAN_MESSAGE_CENTRE_CONSOLE_CC_STEERING: { + cc_steering *message = new cc_steering(); + message->decode(data); + m_canInfo[message->get_message_name()] = message->to_json(); + break; + } + case SYSTEM_CAN_MESSAGE_CENTRE_CONSOLE_CC_REGEN_PERCENTAGE: { + cc_regen_percentage *message = new cc_regen_percentage(); + message->decode(data); + m_canInfo[message->get_message_name()] = message->to_json(); + break; + } + default: { + std::cout << "Unknown message ID: " << static_cast(id) << std::endl; + } + } +} \ No newline at end of file diff --git a/simulation/server/app/src/can_scheduler.cc b/simulation/server/app/src/can_scheduler.cc new file mode 100644 index 0000000..fd97985 --- /dev/null +++ b/simulation/server/app/src/can_scheduler.cc @@ -0,0 +1,692 @@ +/************************************************************************************************ + * @file can_scheduler.cc + * + * @brief Source file defining the CanScheduler class + * + * @date 2025-01-07 + * @author Aryan Kashem + ************************************************************************************************/ + +/** @warning This file is autogenerated */ + +/* Standard library Headers */ +#include +#include + +/* Inter-component Headers */ +#include "system_can.h" + +/* Intra-component Headers */ +#include "can_scheduler.h" + +CanScheduler::CanScheduler() { + m_isConnected = false; + m_bcmCanSocket = -1; +} + +void CanScheduler::scheduleCanMessages() { + canFastCycleBCM.msg_head.opcode = TX_SETUP; + canFastCycleBCM.msg_head.can_id = FAST_CYCLE_BCM_ID; + canFastCycleBCM.msg_head.flags = SETTIMER | STARTTIMER; + canFastCycleBCM.msg_head.nframes = NUM_FAST_CYCLE_MESSAGES; + canFastCycleBCM.msg_head.count = 0; + + canFastCycleBCM.msg_head.ival1.tv_sec = 0U; + canFastCycleBCM.msg_head.ival1.tv_usec = 0U; + canFastCycleBCM.msg_head.ival2.tv_sec = FAST_CYCLE_SPEED_MS / 1000U; + canFastCycleBCM.msg_head.ival2.tv_usec = (FAST_CYCLE_SPEED_MS % 1000U) * 1000U; + + canFastCycleBCM.frame[FAST_BMS_CARRIER_BATTERY_VT_FRAME_INDEX].can_id = SYSTEM_CAN_MESSAGE_BMS_CARRIER_BATTERY_VT; + canFastCycleBCM.frame[FAST_BMS_CARRIER_BATTERY_VT_FRAME_INDEX].can_dlc = 8U; + memset(canFastCycleBCM.frame[FAST_BMS_CARRIER_BATTERY_VT_FRAME_INDEX].data, 0U, MAX_MESSAGE_LENGTH); + canFastCycleBCM.frame[FAST_CAN_COMMUNICATION_FAST_ONE_SHOT_MSG_FRAME_INDEX].can_id = SYSTEM_CAN_MESSAGE_CAN_COMMUNICATION_FAST_ONE_SHOT_MSG; + canFastCycleBCM.frame[FAST_CAN_COMMUNICATION_FAST_ONE_SHOT_MSG_FRAME_INDEX].can_dlc = 4U; + memset(canFastCycleBCM.frame[FAST_CAN_COMMUNICATION_FAST_ONE_SHOT_MSG_FRAME_INDEX].data, 0U, MAX_MESSAGE_LENGTH); + canFastCycleBCM.frame[FAST_CENTRE_CONSOLE_CC_PEDAL_FRAME_INDEX].can_id = SYSTEM_CAN_MESSAGE_CENTRE_CONSOLE_CC_PEDAL; + canFastCycleBCM.frame[FAST_CENTRE_CONSOLE_CC_PEDAL_FRAME_INDEX].can_dlc = 5U; + memset(canFastCycleBCM.frame[FAST_CENTRE_CONSOLE_CC_PEDAL_FRAME_INDEX].data, 0U, MAX_MESSAGE_LENGTH); + + if (write(m_bcmCanSocket, &canFastCycleBCM, sizeof(canFastCycleBCM)) < 0) { + throw std::runtime_error("Failed to schedule CAN BCM Fast cycle messages"); + } + + canMediumCycleBCM.msg_head.opcode = TX_SETUP; + canMediumCycleBCM.msg_head.can_id = MEDIUM_CYCLE_BCM_ID; + canMediumCycleBCM.msg_head.flags = SETTIMER | STARTTIMER; + canMediumCycleBCM.msg_head.nframes = NUM_MEDIUM_CYCLE_MESSAGES; + canMediumCycleBCM.msg_head.count = 0; + + canMediumCycleBCM.msg_head.ival1.tv_sec = 0U; + canMediumCycleBCM.msg_head.ival1.tv_usec = 0U; + canMediumCycleBCM.msg_head.ival2.tv_sec = MEDIUM_CYCLE_SPEED_MS / 1000U; + canMediumCycleBCM.msg_head.ival2.tv_usec = (MEDIUM_CYCLE_SPEED_MS % 1000U) * 1000U; + + canMediumCycleBCM.frame[MEDIUM_BMS_CARRIER_BATTERY_STATUS_FRAME_INDEX].can_id = SYSTEM_CAN_MESSAGE_BMS_CARRIER_BATTERY_STATUS; + canMediumCycleBCM.frame[MEDIUM_BMS_CARRIER_BATTERY_STATUS_FRAME_INDEX].can_dlc = 7U; + memset(canMediumCycleBCM.frame[MEDIUM_BMS_CARRIER_BATTERY_STATUS_FRAME_INDEX].data, 0U, MAX_MESSAGE_LENGTH); + canMediumCycleBCM.frame[MEDIUM_CAN_COMMUNICATION_MEDIUM_ONE_SHOT_MSG_FRAME_INDEX].can_id = SYSTEM_CAN_MESSAGE_CAN_COMMUNICATION_MEDIUM_ONE_SHOT_MSG; + canMediumCycleBCM.frame[MEDIUM_CAN_COMMUNICATION_MEDIUM_ONE_SHOT_MSG_FRAME_INDEX].can_dlc = 4U; + memset(canMediumCycleBCM.frame[MEDIUM_CAN_COMMUNICATION_MEDIUM_ONE_SHOT_MSG_FRAME_INDEX].data, 0U, MAX_MESSAGE_LENGTH); + canMediumCycleBCM.frame[MEDIUM_CENTRE_CONSOLE_CC_INFO_FRAME_INDEX].can_id = SYSTEM_CAN_MESSAGE_CENTRE_CONSOLE_CC_INFO; + canMediumCycleBCM.frame[MEDIUM_CENTRE_CONSOLE_CC_INFO_FRAME_INDEX].can_dlc = 8U; + memset(canMediumCycleBCM.frame[MEDIUM_CENTRE_CONSOLE_CC_INFO_FRAME_INDEX].data, 0U, MAX_MESSAGE_LENGTH); + canMediumCycleBCM.frame[MEDIUM_CENTRE_CONSOLE_CC_STEERING_FRAME_INDEX].can_id = SYSTEM_CAN_MESSAGE_CENTRE_CONSOLE_CC_STEERING; + canMediumCycleBCM.frame[MEDIUM_CENTRE_CONSOLE_CC_STEERING_FRAME_INDEX].can_dlc = 2U; + memset(canMediumCycleBCM.frame[MEDIUM_CENTRE_CONSOLE_CC_STEERING_FRAME_INDEX].data, 0U, MAX_MESSAGE_LENGTH); + canMediumCycleBCM.frame[MEDIUM_CENTRE_CONSOLE_CC_REGEN_PERCENTAGE_FRAME_INDEX].can_id = SYSTEM_CAN_MESSAGE_CENTRE_CONSOLE_CC_REGEN_PERCENTAGE; + canMediumCycleBCM.frame[MEDIUM_CENTRE_CONSOLE_CC_REGEN_PERCENTAGE_FRAME_INDEX].can_dlc = 4U; + memset(canMediumCycleBCM.frame[MEDIUM_CENTRE_CONSOLE_CC_REGEN_PERCENTAGE_FRAME_INDEX].data, 0U, MAX_MESSAGE_LENGTH); + + if (write(m_bcmCanSocket, &canMediumCycleBCM, sizeof(canMediumCycleBCM)) < 0) { + throw std::runtime_error("Failed to schedule CAN BCM Medium cycle messages"); + } + + canSlowCycleBCM.msg_head.opcode = TX_SETUP; + canSlowCycleBCM.msg_head.can_id = SLOW_CYCLE_BCM_ID; + canSlowCycleBCM.msg_head.flags = SETTIMER | STARTTIMER; + canSlowCycleBCM.msg_head.nframes = NUM_SLOW_CYCLE_MESSAGES; + canSlowCycleBCM.msg_head.count = 0; + + canSlowCycleBCM.msg_head.ival1.tv_sec = 0U; + canSlowCycleBCM.msg_head.ival1.tv_usec = 0U; + canSlowCycleBCM.msg_head.ival2.tv_sec = SLOW_CYCLE_SPEED_MS / 1000U; + canSlowCycleBCM.msg_head.ival2.tv_usec = (SLOW_CYCLE_SPEED_MS % 1000U) * 1000U; + + canSlowCycleBCM.frame[SLOW_BMS_CARRIER_BATTERY_INFO_FRAME_INDEX].can_id = SYSTEM_CAN_MESSAGE_BMS_CARRIER_BATTERY_INFO; + canSlowCycleBCM.frame[SLOW_BMS_CARRIER_BATTERY_INFO_FRAME_INDEX].can_dlc = 6U; + memset(canSlowCycleBCM.frame[SLOW_BMS_CARRIER_BATTERY_INFO_FRAME_INDEX].data, 0U, MAX_MESSAGE_LENGTH); + canSlowCycleBCM.frame[SLOW_BMS_CARRIER_AFE1_STATUS_FRAME_INDEX].can_id = SYSTEM_CAN_MESSAGE_BMS_CARRIER_AFE1_STATUS; + canSlowCycleBCM.frame[SLOW_BMS_CARRIER_AFE1_STATUS_FRAME_INDEX].can_dlc = 8U; + memset(canSlowCycleBCM.frame[SLOW_BMS_CARRIER_AFE1_STATUS_FRAME_INDEX].data, 0U, MAX_MESSAGE_LENGTH); + canSlowCycleBCM.frame[SLOW_BMS_CARRIER_AFE2_STATUS_FRAME_INDEX].can_id = SYSTEM_CAN_MESSAGE_BMS_CARRIER_AFE2_STATUS; + canSlowCycleBCM.frame[SLOW_BMS_CARRIER_AFE2_STATUS_FRAME_INDEX].can_dlc = 8U; + memset(canSlowCycleBCM.frame[SLOW_BMS_CARRIER_AFE2_STATUS_FRAME_INDEX].data, 0U, MAX_MESSAGE_LENGTH); + canSlowCycleBCM.frame[SLOW_BMS_CARRIER_AFE3_STATUS_FRAME_INDEX].can_id = SYSTEM_CAN_MESSAGE_BMS_CARRIER_AFE3_STATUS; + canSlowCycleBCM.frame[SLOW_BMS_CARRIER_AFE3_STATUS_FRAME_INDEX].can_dlc = 8U; + memset(canSlowCycleBCM.frame[SLOW_BMS_CARRIER_AFE3_STATUS_FRAME_INDEX].data, 0U, MAX_MESSAGE_LENGTH); + canSlowCycleBCM.frame[SLOW_CAN_COMMUNICATION_SLOW_ONE_SHOT_MSG_FRAME_INDEX].can_id = SYSTEM_CAN_MESSAGE_CAN_COMMUNICATION_SLOW_ONE_SHOT_MSG; + canSlowCycleBCM.frame[SLOW_CAN_COMMUNICATION_SLOW_ONE_SHOT_MSG_FRAME_INDEX].can_dlc = 4U; + memset(canSlowCycleBCM.frame[SLOW_CAN_COMMUNICATION_SLOW_ONE_SHOT_MSG_FRAME_INDEX].data, 0U, MAX_MESSAGE_LENGTH); + + if (write(m_bcmCanSocket, &canSlowCycleBCM, sizeof(canSlowCycleBCM)) < 0) { + throw std::runtime_error("Failed to schedule CAN BCM Slow cycle messages"); + } +} + +void CanScheduler::startCanScheduler() { + try { + m_bcmCanSocket = socket(PF_CAN, SOCK_DGRAM, CAN_BCM); + + if (m_bcmCanSocket < 0) { + throw std::runtime_error("Error creating socket for CAN Broadcast Manager"); + } + + struct ifreq ifr; + strcpy(ifr.ifr_name, CAN_INTERFACE_NAME.c_str()); + if (ioctl(m_bcmCanSocket, SIOCGIFINDEX, &ifr) < 0) { + throw std::runtime_error("Error writing interface name to socketCAN file descriptor. Check if vcan0 is enabled?"); + } + + struct sockaddr_can addr = {}; + addr.can_family = AF_CAN; + addr.can_ifindex = ifr.ifr_ifindex; + + if (connect(m_bcmCanSocket, (struct sockaddr *)&addr, sizeof(addr)) < 0) { + throw std::runtime_error("Error connecting to SocketCAN broadcast manager"); + } + + scheduleCanMessages(); + + } catch (std::exception &e) { + std::cerr << "Error running CAN Scheduler: " << e.what() << std::endl; + } +} +void CanScheduler::update_battery_status_fault(uint16_t fault_value) { + try { + unsigned int start_byte = 0; + + canMediumCycleBCM.frame[MEDIUM_BMS_CARRIER_BATTERY_STATUS_FRAME_INDEX].data[start_byte + 0U] = (fault_value >> 0U) & 0xFFU; + canMediumCycleBCM.frame[MEDIUM_BMS_CARRIER_BATTERY_STATUS_FRAME_INDEX].data[start_byte + 1U] = (fault_value >> 8U) & 0xFFU; + if (write(m_bcmCanSocket, &canMediumCycleBCM, sizeof(canMediumCycleBCM)) < 0) { + throw std::runtime_error("Failed to update battery_status fault}"); + } + } catch (std::exception &e) { + std::cerr << e.what() << std::endl; + } +} +void CanScheduler::update_battery_status_fault_val(uint16_t fault_val_value) { + try { + unsigned int start_byte = 2; + + canMediumCycleBCM.frame[MEDIUM_BMS_CARRIER_BATTERY_STATUS_FRAME_INDEX].data[start_byte + 0U] = (fault_val_value >> 0U) & 0xFFU; + canMediumCycleBCM.frame[MEDIUM_BMS_CARRIER_BATTERY_STATUS_FRAME_INDEX].data[start_byte + 1U] = (fault_val_value >> 8U) & 0xFFU; + if (write(m_bcmCanSocket, &canMediumCycleBCM, sizeof(canMediumCycleBCM)) < 0) { + throw std::runtime_error("Failed to update battery_status fault_val}"); + } + } catch (std::exception &e) { + std::cerr << e.what() << std::endl; + } +} +void CanScheduler::update_battery_status_aux_batt_v(uint16_t aux_batt_v_value) { + try { + unsigned int start_byte = 4; + + canMediumCycleBCM.frame[MEDIUM_BMS_CARRIER_BATTERY_STATUS_FRAME_INDEX].data[start_byte + 0U] = (aux_batt_v_value >> 0U) & 0xFFU; + canMediumCycleBCM.frame[MEDIUM_BMS_CARRIER_BATTERY_STATUS_FRAME_INDEX].data[start_byte + 1U] = (aux_batt_v_value >> 8U) & 0xFFU; + if (write(m_bcmCanSocket, &canMediumCycleBCM, sizeof(canMediumCycleBCM)) < 0) { + throw std::runtime_error("Failed to update battery_status aux_batt_v}"); + } + } catch (std::exception &e) { + std::cerr << e.what() << std::endl; + } +} +void CanScheduler::update_battery_status_afe_status(uint8_t afe_status_value) { + try { + unsigned int start_byte = 6; + + canMediumCycleBCM.frame[MEDIUM_BMS_CARRIER_BATTERY_STATUS_FRAME_INDEX].data[start_byte + 0U] = (afe_status_value >> 0U) & 0xFFU; + if (write(m_bcmCanSocket, &canMediumCycleBCM, sizeof(canMediumCycleBCM)) < 0) { + throw std::runtime_error("Failed to update battery_status afe_status}"); + } + } catch (std::exception &e) { + std::cerr << e.what() << std::endl; + } +} +void CanScheduler::update_battery_vt_voltage(uint16_t voltage_value) { + try { + unsigned int start_byte = 0; + + canFastCycleBCM.frame[FAST_BMS_CARRIER_BATTERY_VT_FRAME_INDEX].data[start_byte + 0U] = (voltage_value >> 0U) & 0xFFU; + canFastCycleBCM.frame[FAST_BMS_CARRIER_BATTERY_VT_FRAME_INDEX].data[start_byte + 1U] = (voltage_value >> 8U) & 0xFFU; + if (write(m_bcmCanSocket, &canFastCycleBCM, sizeof(canFastCycleBCM)) < 0) { + throw std::runtime_error("Failed to update battery_vt voltage"); + } + } catch (std::exception &e) { + std::cerr << e.what() << std::endl; + } +} +void CanScheduler::update_battery_vt_current(uint16_t current_value) { + try { + unsigned int start_byte = 2; + + canFastCycleBCM.frame[FAST_BMS_CARRIER_BATTERY_VT_FRAME_INDEX].data[start_byte + 0U] = (current_value >> 0U) & 0xFFU; + canFastCycleBCM.frame[FAST_BMS_CARRIER_BATTERY_VT_FRAME_INDEX].data[start_byte + 1U] = (current_value >> 8U) & 0xFFU; + if (write(m_bcmCanSocket, &canFastCycleBCM, sizeof(canFastCycleBCM)) < 0) { + throw std::runtime_error("Failed to update battery_vt current"); + } + } catch (std::exception &e) { + std::cerr << e.what() << std::endl; + } +} +void CanScheduler::update_battery_vt_temperature(uint16_t temperature_value) { + try { + unsigned int start_byte = 4; + + canFastCycleBCM.frame[FAST_BMS_CARRIER_BATTERY_VT_FRAME_INDEX].data[start_byte + 0U] = (temperature_value >> 0U) & 0xFFU; + canFastCycleBCM.frame[FAST_BMS_CARRIER_BATTERY_VT_FRAME_INDEX].data[start_byte + 1U] = (temperature_value >> 8U) & 0xFFU; + if (write(m_bcmCanSocket, &canFastCycleBCM, sizeof(canFastCycleBCM)) < 0) { + throw std::runtime_error("Failed to update battery_vt temperature"); + } + } catch (std::exception &e) { + std::cerr << e.what() << std::endl; + } +} +void CanScheduler::update_battery_vt_batt_perc(uint16_t batt_perc_value) { + try { + unsigned int start_byte = 6; + + canFastCycleBCM.frame[FAST_BMS_CARRIER_BATTERY_VT_FRAME_INDEX].data[start_byte + 0U] = (batt_perc_value >> 0U) & 0xFFU; + canFastCycleBCM.frame[FAST_BMS_CARRIER_BATTERY_VT_FRAME_INDEX].data[start_byte + 1U] = (batt_perc_value >> 8U) & 0xFFU; + if (write(m_bcmCanSocket, &canFastCycleBCM, sizeof(canFastCycleBCM)) < 0) { + throw std::runtime_error("Failed to update battery_vt batt_perc"); + } + } catch (std::exception &e) { + std::cerr << e.what() << std::endl; + } +} +void CanScheduler::update_battery_info_fan1(uint8_t fan1_value) { + try { + unsigned int start_byte = 0; + + canSlowCycleBCM.frame[SLOW_BMS_CARRIER_BATTERY_INFO_FRAME_INDEX].data[start_byte + 0U] = (fan1_value >> 0U) & 0xFFU; + if (write(m_bcmCanSocket, &canSlowCycleBCM, sizeof(canSlowCycleBCM)) < 0) { + throw std::runtime_error("Failed to update battery_info fan1"); + } + } catch (std::exception &e) { + std::cerr << e.what() << std::endl; + } +} +void CanScheduler::update_battery_info_fan2(uint8_t fan2_value) { + try { + unsigned int start_byte = 1; + + canSlowCycleBCM.frame[SLOW_BMS_CARRIER_BATTERY_INFO_FRAME_INDEX].data[start_byte + 0U] = (fan2_value >> 0U) & 0xFFU; + if (write(m_bcmCanSocket, &canSlowCycleBCM, sizeof(canSlowCycleBCM)) < 0) { + throw std::runtime_error("Failed to update battery_info fan2"); + } + } catch (std::exception &e) { + std::cerr << e.what() << std::endl; + } +} +void CanScheduler::update_battery_info_max_cell_v(uint16_t max_cell_v_value) { + try { + unsigned int start_byte = 2; + + canSlowCycleBCM.frame[SLOW_BMS_CARRIER_BATTERY_INFO_FRAME_INDEX].data[start_byte + 0U] = (max_cell_v_value >> 0U) & 0xFFU; + canSlowCycleBCM.frame[SLOW_BMS_CARRIER_BATTERY_INFO_FRAME_INDEX].data[start_byte + 1U] = (max_cell_v_value >> 8U) & 0xFFU; + if (write(m_bcmCanSocket, &canSlowCycleBCM, sizeof(canSlowCycleBCM)) < 0) { + throw std::runtime_error("Failed to update battery_info max_cell_v"); + } + } catch (std::exception &e) { + std::cerr << e.what() << std::endl; + } +} +void CanScheduler::update_battery_info_min_cell_v(uint16_t min_cell_v_value) { + try { + unsigned int start_byte = 4; + + canSlowCycleBCM.frame[SLOW_BMS_CARRIER_BATTERY_INFO_FRAME_INDEX].data[start_byte + 0U] = (min_cell_v_value >> 0U) & 0xFFU; + canSlowCycleBCM.frame[SLOW_BMS_CARRIER_BATTERY_INFO_FRAME_INDEX].data[start_byte + 1U] = (min_cell_v_value >> 8U) & 0xFFU; + if (write(m_bcmCanSocket, &canSlowCycleBCM, sizeof(canSlowCycleBCM)) < 0) { + throw std::runtime_error("Failed to update battery_info min_cell_v"); + } + } catch (std::exception &e) { + std::cerr << e.what() << std::endl; + } +} +void CanScheduler::update_afe1_status_id(uint8_t id_value) { + try { + unsigned int start_byte = 0; + + canSlowCycleBCM.frame[SLOW_BMS_CARRIER_AFE1_STATUS_FRAME_INDEX].data[start_byte + 0U] = (id_value >> 0U) & 0xFFU; + if (write(m_bcmCanSocket, &canSlowCycleBCM, sizeof(canSlowCycleBCM)) < 0) { + throw std::runtime_error("Failed to update afe1_status id"); + } + } catch (std::exception &e) { + std::cerr << e.what() << std::endl; + } +} +void CanScheduler::update_afe1_status_temp(uint8_t temp_value) { + try { + unsigned int start_byte = 1; + + canSlowCycleBCM.frame[SLOW_BMS_CARRIER_AFE1_STATUS_FRAME_INDEX].data[start_byte + 0U] = (temp_value >> 0U) & 0xFFU; + if (write(m_bcmCanSocket, &canSlowCycleBCM, sizeof(canSlowCycleBCM)) < 0) { + throw std::runtime_error("Failed to update afe1_status temp"); + } + } catch (std::exception &e) { + std::cerr << e.what() << std::endl; + } +} +void CanScheduler::update_afe1_status_v1(uint16_t v1_value) { + try { + unsigned int start_byte = 2; + + canSlowCycleBCM.frame[SLOW_BMS_CARRIER_AFE1_STATUS_FRAME_INDEX].data[start_byte + 0U] = (v1_value >> 0U) & 0xFFU; + canSlowCycleBCM.frame[SLOW_BMS_CARRIER_AFE1_STATUS_FRAME_INDEX].data[start_byte + 1U] = (v1_value >> 8U) & 0xFFU; + if (write(m_bcmCanSocket, &canSlowCycleBCM, sizeof(canSlowCycleBCM)) < 0) { + throw std::runtime_error("Failed to update afe1_status v1"); + } + } catch (std::exception &e) { + std::cerr << e.what() << std::endl; + } +} +void CanScheduler::update_afe1_status_v2(uint16_t v2_value) { + try { + unsigned int start_byte = 4; + + canSlowCycleBCM.frame[SLOW_BMS_CARRIER_AFE1_STATUS_FRAME_INDEX].data[start_byte + 0U] = (v2_value >> 0U) & 0xFFU; + canSlowCycleBCM.frame[SLOW_BMS_CARRIER_AFE1_STATUS_FRAME_INDEX].data[start_byte + 1U] = (v2_value >> 8U) & 0xFFU; + if (write(m_bcmCanSocket, &canSlowCycleBCM, sizeof(canSlowCycleBCM)) < 0) { + throw std::runtime_error("Failed to update afe1_status v2"); + } + } catch (std::exception &e) { + std::cerr << e.what() << std::endl; + } +} +void CanScheduler::update_afe1_status_v3(uint16_t v3_value) { + try { + unsigned int start_byte = 6; + + canSlowCycleBCM.frame[SLOW_BMS_CARRIER_AFE1_STATUS_FRAME_INDEX].data[start_byte + 0U] = (v3_value >> 0U) & 0xFFU; + canSlowCycleBCM.frame[SLOW_BMS_CARRIER_AFE1_STATUS_FRAME_INDEX].data[start_byte + 1U] = (v3_value >> 8U) & 0xFFU; + if (write(m_bcmCanSocket, &canSlowCycleBCM, sizeof(canSlowCycleBCM)) < 0) { + throw std::runtime_error("Failed to update afe1_status v3"); + } + } catch (std::exception &e) { + std::cerr << e.what() << std::endl; + } +} +void CanScheduler::update_afe2_status_id(uint8_t id_value) { + try { + unsigned int start_byte = 0; + + canSlowCycleBCM.frame[SLOW_BMS_CARRIER_AFE2_STATUS_FRAME_INDEX].data[start_byte + 0U] = (id_value >> 0U) & 0xFFU; + if (write(m_bcmCanSocket, &canSlowCycleBCM, sizeof(canSlowCycleBCM)) < 0) { + throw std::runtime_error("Failed to update afe2_status id"); + } + } catch (std::exception &e) { + std::cerr << e.what() << std::endl; + } +} +void CanScheduler::update_afe2_status_temp(uint8_t temp_value) { + try { + unsigned int start_byte = 1; + + canSlowCycleBCM.frame[SLOW_BMS_CARRIER_AFE2_STATUS_FRAME_INDEX].data[start_byte + 0U] = (temp_value >> 0U) & 0xFFU; + if (write(m_bcmCanSocket, &canSlowCycleBCM, sizeof(canSlowCycleBCM)) < 0) { + throw std::runtime_error("Failed to update afe2_status temp"); + } + } catch (std::exception &e) { + std::cerr << e.what() << std::endl; + } +} +void CanScheduler::update_afe2_status_v1(uint16_t v1_value) { + try { + unsigned int start_byte = 2; + + canSlowCycleBCM.frame[SLOW_BMS_CARRIER_AFE2_STATUS_FRAME_INDEX].data[start_byte + 0U] = (v1_value >> 0U) & 0xFFU; + canSlowCycleBCM.frame[SLOW_BMS_CARRIER_AFE2_STATUS_FRAME_INDEX].data[start_byte + 1U] = (v1_value >> 8U) & 0xFFU; + if (write(m_bcmCanSocket, &canSlowCycleBCM, sizeof(canSlowCycleBCM)) < 0) { + throw std::runtime_error("Failed to update afe2_status v1"); + } + } catch (std::exception &e) { + std::cerr << e.what() << std::endl; + } +} +void CanScheduler::update_afe2_status_v2(uint16_t v2_value) { + try { + unsigned int start_byte = 4; + + canSlowCycleBCM.frame[SLOW_BMS_CARRIER_AFE2_STATUS_FRAME_INDEX].data[start_byte + 0U] = (v2_value >> 0U) & 0xFFU; + canSlowCycleBCM.frame[SLOW_BMS_CARRIER_AFE2_STATUS_FRAME_INDEX].data[start_byte + 1U] = (v2_value >> 8U) & 0xFFU; + if (write(m_bcmCanSocket, &canSlowCycleBCM, sizeof(canSlowCycleBCM)) < 0) { + throw std::runtime_error("Failed to update afe2_status v2"); + } + } catch (std::exception &e) { + std::cerr << e.what() << std::endl; + } +} +void CanScheduler::update_afe2_status_v3(uint16_t v3_value) { + try { + unsigned int start_byte = 6; + + canSlowCycleBCM.frame[SLOW_BMS_CARRIER_AFE2_STATUS_FRAME_INDEX].data[start_byte + 0U] = (v3_value >> 0U) & 0xFFU; + canSlowCycleBCM.frame[SLOW_BMS_CARRIER_AFE2_STATUS_FRAME_INDEX].data[start_byte + 1U] = (v3_value >> 8U) & 0xFFU; + if (write(m_bcmCanSocket, &canSlowCycleBCM, sizeof(canSlowCycleBCM)) < 0) { + throw std::runtime_error("Failed to update afe2_status v3"); + } + } catch (std::exception &e) { + std::cerr << e.what() << std::endl; + } +} +void CanScheduler::update_afe3_status_id(uint8_t id_value) { + try { + unsigned int start_byte = 0; + + canSlowCycleBCM.frame[SLOW_BMS_CARRIER_AFE3_STATUS_FRAME_INDEX].data[start_byte + 0U] = (id_value >> 0U) & 0xFFU; + if (write(m_bcmCanSocket, &canSlowCycleBCM, sizeof(canSlowCycleBCM)) < 0) { + throw std::runtime_error("Failed to update afe3_status id"); + } + } catch (std::exception &e) { + std::cerr << e.what() << std::endl; + } +} +void CanScheduler::update_afe3_status_temp(uint8_t temp_value) { + try { + unsigned int start_byte = 1; + + canSlowCycleBCM.frame[SLOW_BMS_CARRIER_AFE3_STATUS_FRAME_INDEX].data[start_byte + 0U] = (temp_value >> 0U) & 0xFFU; + if (write(m_bcmCanSocket, &canSlowCycleBCM, sizeof(canSlowCycleBCM)) < 0) { + throw std::runtime_error("Failed to update afe3_status temp"); + } + } catch (std::exception &e) { + std::cerr << e.what() << std::endl; + } +} +void CanScheduler::update_afe3_status_v1(uint16_t v1_value) { + try { + unsigned int start_byte = 2; + + canSlowCycleBCM.frame[SLOW_BMS_CARRIER_AFE3_STATUS_FRAME_INDEX].data[start_byte + 0U] = (v1_value >> 0U) & 0xFFU; + canSlowCycleBCM.frame[SLOW_BMS_CARRIER_AFE3_STATUS_FRAME_INDEX].data[start_byte + 1U] = (v1_value >> 8U) & 0xFFU; + if (write(m_bcmCanSocket, &canSlowCycleBCM, sizeof(canSlowCycleBCM)) < 0) { + throw std::runtime_error("Failed to update afe3_status v1"); + } + } catch (std::exception &e) { + std::cerr << e.what() << std::endl; + } +} +void CanScheduler::update_afe3_status_v2(uint16_t v2_value) { + try { + unsigned int start_byte = 4; + + canSlowCycleBCM.frame[SLOW_BMS_CARRIER_AFE3_STATUS_FRAME_INDEX].data[start_byte + 0U] = (v2_value >> 0U) & 0xFFU; + canSlowCycleBCM.frame[SLOW_BMS_CARRIER_AFE3_STATUS_FRAME_INDEX].data[start_byte + 1U] = (v2_value >> 8U) & 0xFFU; + if (write(m_bcmCanSocket, &canSlowCycleBCM, sizeof(canSlowCycleBCM)) < 0) { + throw std::runtime_error("Failed to update afe3_status v2"); + } + } catch (std::exception &e) { + std::cerr << e.what() << std::endl; + } +} +void CanScheduler::update_afe3_status_v3(uint16_t v3_value) { + try { + unsigned int start_byte = 6; + + canSlowCycleBCM.frame[SLOW_BMS_CARRIER_AFE3_STATUS_FRAME_INDEX].data[start_byte + 0U] = (v3_value >> 0U) & 0xFFU; + canSlowCycleBCM.frame[SLOW_BMS_CARRIER_AFE3_STATUS_FRAME_INDEX].data[start_byte + 1U] = (v3_value >> 8U) & 0xFFU; + if (write(m_bcmCanSocket, &canSlowCycleBCM, sizeof(canSlowCycleBCM)) < 0) { + throw std::runtime_error("Failed to update afe3_status v3"); + } + } catch (std::exception &e) { + std::cerr << e.what() << std::endl; + } +} +void CanScheduler::update_fast_one_shot_msg_sig1(uint16_t sig1_value) { + try { + unsigned int start_byte = 0; + + canFastCycleBCM.frame[FAST_CAN_COMMUNICATION_FAST_ONE_SHOT_MSG_FRAME_INDEX].data[start_byte + 0U] = (sig1_value >> 0U) & 0xFFU; + canFastCycleBCM.frame[FAST_CAN_COMMUNICATION_FAST_ONE_SHOT_MSG_FRAME_INDEX].data[start_byte + 1U] = (sig1_value >> 8U) & 0xFFU; + if (write(m_bcmCanSocket, &canFastCycleBCM, sizeof(canFastCycleBCM)) < 0) { + throw std::runtime_error("Failed to update fast_one_shot_msg sig1"); + } + } catch (std::exception &e) { + std::cerr << e.what() << std::endl; + } +} +void CanScheduler::update_fast_one_shot_msg_sig2(uint16_t sig2_value) { + try { + unsigned int start_byte = 2; + + canFastCycleBCM.frame[FAST_CAN_COMMUNICATION_FAST_ONE_SHOT_MSG_FRAME_INDEX].data[start_byte + 0U] = (sig2_value >> 0U) & 0xFFU; + canFastCycleBCM.frame[FAST_CAN_COMMUNICATION_FAST_ONE_SHOT_MSG_FRAME_INDEX].data[start_byte + 1U] = (sig2_value >> 8U) & 0xFFU; + if (write(m_bcmCanSocket, &canFastCycleBCM, sizeof(canFastCycleBCM)) < 0) { + throw std::runtime_error("Failed to update fast_one_shot_msg sig2"); + } + } catch (std::exception &e) { + std::cerr << e.what() << std::endl; + } +} +void CanScheduler::update_medium_one_shot_msg_sig1(uint16_t sig1_value) { + try { + unsigned int start_byte = 0; + + canMediumCycleBCM.frame[MEDIUM_CAN_COMMUNICATION_MEDIUM_ONE_SHOT_MSG_FRAME_INDEX].data[start_byte + 0U] = (sig1_value >> 0U) & 0xFFU; + canMediumCycleBCM.frame[MEDIUM_CAN_COMMUNICATION_MEDIUM_ONE_SHOT_MSG_FRAME_INDEX].data[start_byte + 1U] = (sig1_value >> 8U) & 0xFFU; + if (write(m_bcmCanSocket, &canMediumCycleBCM, sizeof(canMediumCycleBCM)) < 0) { + throw std::runtime_error("Failed to update medium_one_shot_msg sig1}"); + } + } catch (std::exception &e) { + std::cerr << e.what() << std::endl; + } +} +void CanScheduler::update_medium_one_shot_msg_sig2(uint16_t sig2_value) { + try { + unsigned int start_byte = 2; + + canMediumCycleBCM.frame[MEDIUM_CAN_COMMUNICATION_MEDIUM_ONE_SHOT_MSG_FRAME_INDEX].data[start_byte + 0U] = (sig2_value >> 0U) & 0xFFU; + canMediumCycleBCM.frame[MEDIUM_CAN_COMMUNICATION_MEDIUM_ONE_SHOT_MSG_FRAME_INDEX].data[start_byte + 1U] = (sig2_value >> 8U) & 0xFFU; + if (write(m_bcmCanSocket, &canMediumCycleBCM, sizeof(canMediumCycleBCM)) < 0) { + throw std::runtime_error("Failed to update medium_one_shot_msg sig2}"); + } + } catch (std::exception &e) { + std::cerr << e.what() << std::endl; + } +} +void CanScheduler::update_slow_one_shot_msg_sig1(uint16_t sig1_value) { + try { + unsigned int start_byte = 0; + + canSlowCycleBCM.frame[SLOW_CAN_COMMUNICATION_SLOW_ONE_SHOT_MSG_FRAME_INDEX].data[start_byte + 0U] = (sig1_value >> 0U) & 0xFFU; + canSlowCycleBCM.frame[SLOW_CAN_COMMUNICATION_SLOW_ONE_SHOT_MSG_FRAME_INDEX].data[start_byte + 1U] = (sig1_value >> 8U) & 0xFFU; + if (write(m_bcmCanSocket, &canSlowCycleBCM, sizeof(canSlowCycleBCM)) < 0) { + throw std::runtime_error("Failed to update slow_one_shot_msg sig1"); + } + } catch (std::exception &e) { + std::cerr << e.what() << std::endl; + } +} +void CanScheduler::update_slow_one_shot_msg_sig2(uint16_t sig2_value) { + try { + unsigned int start_byte = 2; + + canSlowCycleBCM.frame[SLOW_CAN_COMMUNICATION_SLOW_ONE_SHOT_MSG_FRAME_INDEX].data[start_byte + 0U] = (sig2_value >> 0U) & 0xFFU; + canSlowCycleBCM.frame[SLOW_CAN_COMMUNICATION_SLOW_ONE_SHOT_MSG_FRAME_INDEX].data[start_byte + 1U] = (sig2_value >> 8U) & 0xFFU; + if (write(m_bcmCanSocket, &canSlowCycleBCM, sizeof(canSlowCycleBCM)) < 0) { + throw std::runtime_error("Failed to update slow_one_shot_msg sig2"); + } + } catch (std::exception &e) { + std::cerr << e.what() << std::endl; + } +} +void CanScheduler::update_cc_pedal_throttle_output(uint32_t throttle_output_value) { + try { + unsigned int start_byte = 0; + + canFastCycleBCM.frame[FAST_CENTRE_CONSOLE_CC_PEDAL_FRAME_INDEX].data[start_byte + 0U] = (throttle_output_value >> 0U) & 0xFFU; + canFastCycleBCM.frame[FAST_CENTRE_CONSOLE_CC_PEDAL_FRAME_INDEX].data[start_byte + 1U] = (throttle_output_value >> 8U) & 0xFFU; + canFastCycleBCM.frame[FAST_CENTRE_CONSOLE_CC_PEDAL_FRAME_INDEX].data[start_byte + 2U] = (throttle_output_value >> 16U) & 0xFFU; + canFastCycleBCM.frame[FAST_CENTRE_CONSOLE_CC_PEDAL_FRAME_INDEX].data[start_byte + 3U] = (throttle_output_value >> 24U) & 0xFFU; + if (write(m_bcmCanSocket, &canFastCycleBCM, sizeof(canFastCycleBCM)) < 0) { + throw std::runtime_error("Failed to update cc_pedal throttle_output"); + } + } catch (std::exception &e) { + std::cerr << e.what() << std::endl; + } +} +void CanScheduler::update_cc_pedal_brake_output(uint8_t brake_output_value) { + try { + unsigned int start_byte = 4; + + canFastCycleBCM.frame[FAST_CENTRE_CONSOLE_CC_PEDAL_FRAME_INDEX].data[start_byte + 0U] = (brake_output_value >> 0U) & 0xFFU; + if (write(m_bcmCanSocket, &canFastCycleBCM, sizeof(canFastCycleBCM)) < 0) { + throw std::runtime_error("Failed to update cc_pedal brake_output"); + } + } catch (std::exception &e) { + std::cerr << e.what() << std::endl; + } +} +void CanScheduler::update_cc_info_target_velocity(uint32_t target_velocity_value) { + try { + unsigned int start_byte = 0; + + canMediumCycleBCM.frame[MEDIUM_CENTRE_CONSOLE_CC_INFO_FRAME_INDEX].data[start_byte + 0U] = (target_velocity_value >> 0U) & 0xFFU; + canMediumCycleBCM.frame[MEDIUM_CENTRE_CONSOLE_CC_INFO_FRAME_INDEX].data[start_byte + 1U] = (target_velocity_value >> 8U) & 0xFFU; + canMediumCycleBCM.frame[MEDIUM_CENTRE_CONSOLE_CC_INFO_FRAME_INDEX].data[start_byte + 2U] = (target_velocity_value >> 16U) & 0xFFU; + canMediumCycleBCM.frame[MEDIUM_CENTRE_CONSOLE_CC_INFO_FRAME_INDEX].data[start_byte + 3U] = (target_velocity_value >> 24U) & 0xFFU; + if (write(m_bcmCanSocket, &canMediumCycleBCM, sizeof(canMediumCycleBCM)) < 0) { + throw std::runtime_error("Failed to update cc_info target_velocity}"); + } + } catch (std::exception &e) { + std::cerr << e.what() << std::endl; + } +} +void CanScheduler::update_cc_info_drive_state(uint8_t drive_state_value) { + try { + unsigned int start_byte = 4; + + canMediumCycleBCM.frame[MEDIUM_CENTRE_CONSOLE_CC_INFO_FRAME_INDEX].data[start_byte + 0U] = (drive_state_value >> 0U) & 0xFFU; + if (write(m_bcmCanSocket, &canMediumCycleBCM, sizeof(canMediumCycleBCM)) < 0) { + throw std::runtime_error("Failed to update cc_info drive_state}"); + } + } catch (std::exception &e) { + std::cerr << e.what() << std::endl; + } +} +void CanScheduler::update_cc_info_cruise_control(uint8_t cruise_control_value) { + try { + unsigned int start_byte = 5; + + canMediumCycleBCM.frame[MEDIUM_CENTRE_CONSOLE_CC_INFO_FRAME_INDEX].data[start_byte + 0U] = (cruise_control_value >> 0U) & 0xFFU; + if (write(m_bcmCanSocket, &canMediumCycleBCM, sizeof(canMediumCycleBCM)) < 0) { + throw std::runtime_error("Failed to update cc_info cruise_control}"); + } + } catch (std::exception &e) { + std::cerr << e.what() << std::endl; + } +} +void CanScheduler::update_cc_info_regen_braking(uint8_t regen_braking_value) { + try { + unsigned int start_byte = 6; + + canMediumCycleBCM.frame[MEDIUM_CENTRE_CONSOLE_CC_INFO_FRAME_INDEX].data[start_byte + 0U] = (regen_braking_value >> 0U) & 0xFFU; + if (write(m_bcmCanSocket, &canMediumCycleBCM, sizeof(canMediumCycleBCM)) < 0) { + throw std::runtime_error("Failed to update cc_info regen_braking}"); + } + } catch (std::exception &e) { + std::cerr << e.what() << std::endl; + } +} +void CanScheduler::update_cc_info_hazard_enabled(uint8_t hazard_enabled_value) { + try { + unsigned int start_byte = 7; + + canMediumCycleBCM.frame[MEDIUM_CENTRE_CONSOLE_CC_INFO_FRAME_INDEX].data[start_byte + 0U] = (hazard_enabled_value >> 0U) & 0xFFU; + if (write(m_bcmCanSocket, &canMediumCycleBCM, sizeof(canMediumCycleBCM)) < 0) { + throw std::runtime_error("Failed to update cc_info hazard_enabled}"); + } + } catch (std::exception &e) { + std::cerr << e.what() << std::endl; + } +} +void CanScheduler::update_cc_steering_input_cc(uint8_t input_cc_value) { + try { + unsigned int start_byte = 0; + + canMediumCycleBCM.frame[MEDIUM_CENTRE_CONSOLE_CC_STEERING_FRAME_INDEX].data[start_byte + 0U] = (input_cc_value >> 0U) & 0xFFU; + if (write(m_bcmCanSocket, &canMediumCycleBCM, sizeof(canMediumCycleBCM)) < 0) { + throw std::runtime_error("Failed to update cc_steering input_cc}"); + } + } catch (std::exception &e) { + std::cerr << e.what() << std::endl; + } +} +void CanScheduler::update_cc_steering_input_lights(uint8_t input_lights_value) { + try { + unsigned int start_byte = 1; + + canMediumCycleBCM.frame[MEDIUM_CENTRE_CONSOLE_CC_STEERING_FRAME_INDEX].data[start_byte + 0U] = (input_lights_value >> 0U) & 0xFFU; + if (write(m_bcmCanSocket, &canMediumCycleBCM, sizeof(canMediumCycleBCM)) < 0) { + throw std::runtime_error("Failed to update cc_steering input_lights}"); + } + } catch (std::exception &e) { + std::cerr << e.what() << std::endl; + } +} +void CanScheduler::update_cc_regen_percentage_percent(uint32_t percent_value) { + try { + unsigned int start_byte = 0; + + canMediumCycleBCM.frame[MEDIUM_CENTRE_CONSOLE_CC_REGEN_PERCENTAGE_FRAME_INDEX].data[start_byte + 0U] = (percent_value >> 0U) & 0xFFU; + canMediumCycleBCM.frame[MEDIUM_CENTRE_CONSOLE_CC_REGEN_PERCENTAGE_FRAME_INDEX].data[start_byte + 1U] = (percent_value >> 8U) & 0xFFU; + canMediumCycleBCM.frame[MEDIUM_CENTRE_CONSOLE_CC_REGEN_PERCENTAGE_FRAME_INDEX].data[start_byte + 2U] = (percent_value >> 16U) & 0xFFU; + canMediumCycleBCM.frame[MEDIUM_CENTRE_CONSOLE_CC_REGEN_PERCENTAGE_FRAME_INDEX].data[start_byte + 3U] = (percent_value >> 24U) & 0xFFU; + if (write(m_bcmCanSocket, &canMediumCycleBCM, sizeof(canMediumCycleBCM)) < 0) { + throw std::runtime_error("Failed to update cc_regen_percentage percent}"); + } + } catch (std::exception &e) { + std::cerr << e.what() << std::endl; + } +} \ No newline at end of file diff --git a/simulation/server/app/src/gpio_manager.cc b/simulation/server/app/src/gpio_manager.cc new file mode 100644 index 0000000..be59a03 --- /dev/null +++ b/simulation/server/app/src/gpio_manager.cc @@ -0,0 +1,362 @@ +/************************************************************************************************ + * @file gpio_manager.cc + * + * @brief Source file defining the GpioManager Class for the server + * + * @date 2025-01-04 + * @author Aryan Kashem + ************************************************************************************************/ + +/* Standard library Headers */ +#include + +/* Inter-component Headers */ +#include "command_code.h" + +/* Intra-component Headers */ +#include "app.h" +#include "gpio_manager.h" + +#define GPIO_KEY "gpio" +#define PIN_STATE_KEY "state" +#define PIN_MODE_KEY "mode" +#define PIN_ALT_FUNC_KEY "alternate_function" + +const char *gpioPortNames[] = { + "A", /* GPIO_PORT_A */ + "B", /* GPIO_PORT_B */ +}; + +std::string GpioManager::stringifyPinMode(Datagram::Gpio::Mode mode) { + std::string result = ""; + + switch (mode) { + case Datagram::Gpio::Mode::GPIO_ANALOG: { + result = "Analog"; + break; + } + case Datagram::Gpio::Mode::GPIO_INPUT_FLOATING: { + result = "Floating Input"; + break; + } + case Datagram::Gpio::Mode::GPIO_INPUT_PULL_DOWN: { + result = "Pull-down Input"; + break; + } + case Datagram::Gpio::Mode::GPIO_INPUT_PULL_UP: { + result = "Pull-up Input"; + break; + } + case Datagram::Gpio::Mode::GPIO_OUTPUT_OPEN_DRAIN: { + result = "Open-drain Output"; + break; + } + case Datagram::Gpio::Mode::GPIO_OUTPUT_PUSH_PULL: { + result = "Push-pull Output"; + break; + } + case Datagram::Gpio::Mode::GPIO_ALFTN_OPEN_DRAIN: { + result = "Open-drain Alternate Function"; + break; + } + case Datagram::Gpio::Mode::GPIO_ALTFN_PUSH_PULL: { + result = "Push-pull Alternate Function"; + break; + } + default: { + result = "Invalid Mode"; + break; + } + } + + return result; +} + +std::string GpioManager::stringifyPinAltFunction(Datagram::Gpio::AltFunction altFunction) { + std::string result = ""; + + switch (altFunction) { + /* Duplicate value is SWCLK/SWDIO */ + case Datagram::Gpio::AltFunction::GPIO_ALT_NONE: { + result = "None/SWDIO/SWCLK ALT0"; + break; + } + /* Duplicate value is Timer 2 */ + case Datagram::Gpio::AltFunction::GPIO_ALT1_TIM1: { + result = "Timer1/2 ALT1"; + break; + } + /* Duplicate values is I2C2/3 */ + case Datagram::Gpio::AltFunction::GPIO_ALT4_I2C1: { + result = "I2C1/2/3 ALT4"; + break; + } + /* Duplicate value is SPI2 */ + case Datagram::Gpio::AltFunction::GPIO_ALT5_SPI1: { + result = "SPI1/2 ALT5"; + break; + } + case Datagram::Gpio::AltFunction::GPIO_ALT6_SPI3: { + result = "SPI3 ALT6"; + break; + } + case Datagram::Gpio::AltFunction::GPIO_ALT7_USART1: { + result = "USART1/2/3 ALT7"; + break; + } + case Datagram::Gpio::AltFunction::GPIO_ALT9_CAN1: { + result = "CAN1 ALT9"; + break; + } + /* Duplicate value is Timer 16 */ + case Datagram::Gpio::AltFunction::GPIO_ALT14_TIM15: { + result = "TIMER15/16 ALT14"; + break; + } + default: { + result = "Invalid Alternate Function"; + } + } + + return result; +} + +void GpioManager::loadGpioInfo(std::string &projectName) { + m_gpioInfo = serverJSONManager.getProjectValue>(projectName, GPIO_KEY); +} + +void GpioManager::saveGpioInfo(std::string &projectName) { + serverJSONManager.setProjectValue(projectName, GPIO_KEY, m_gpioInfo); + + /* Upon save, clear the memory */ + m_gpioInfo.clear(); +} + +void GpioManager::updateGpioPinState(std::string &projectName, std::string &payload) { + loadGpioInfo(projectName); + + m_gpioDatagram.deserialize(payload); + + std::string key = gpioPortNames[static_cast(m_gpioDatagram.getGpioPort())]; + key += std::to_string(m_gpioDatagram.getGpioPin()); + const uint8_t *receivedData = m_gpioDatagram.getBuffer(); + + if (static_cast(receivedData[0U]) == Datagram::Gpio::State::GPIO_STATE_HIGH) { + m_gpioInfo[key][PIN_STATE_KEY] = "HIGH"; + } else if (static_cast(receivedData[0U]) == Datagram::Gpio::State::GPIO_STATE_LOW) { + m_gpioInfo[key][PIN_STATE_KEY] = "LOW"; + } else { + m_gpioInfo[key][PIN_STATE_KEY] = "INVALID"; + } + + saveGpioInfo(projectName); +} + +void GpioManager::updateGpioAllStates(std::string &projectName, std::string &payload) { + loadGpioInfo(projectName); + + m_gpioDatagram.deserialize(payload); + + const uint8_t *receivedData = m_gpioDatagram.getBuffer(); + + for (uint8_t i = 0U; i < static_cast(Datagram::Gpio::Port::NUM_GPIO_PORTS) * static_cast(Datagram::Gpio::PINS_PER_PORT); i++) { + size_t blockIndex = i / 8U; + size_t bitPosition = i % 8U; + bool pinState = (receivedData[blockIndex] & (1U << bitPosition)) != 0; + std::string key = gpioPortNames[i / Datagram::Gpio::PINS_PER_PORT]; + key += std::to_string(i % Datagram::Gpio::PINS_PER_PORT); + + if (pinState) { + m_gpioInfo[key][PIN_STATE_KEY] = "HIGH"; + } else { + m_gpioInfo[key][PIN_STATE_KEY] = "LOW"; + } + } + + saveGpioInfo(projectName); +} + +void GpioManager::updateGpioPinMode(std::string &projectName, std::string &payload) { + loadGpioInfo(projectName); + + m_gpioDatagram.deserialize(payload); + + std::string key = gpioPortNames[static_cast(m_gpioDatagram.getGpioPort())]; + key += std::to_string(m_gpioDatagram.getGpioPin()); + + const uint8_t *receivedData = m_gpioDatagram.getBuffer(); + + m_gpioInfo[key][PIN_MODE_KEY] = stringifyPinMode(static_cast(receivedData[0U])); + + saveGpioInfo(projectName); +} + +void GpioManager::updateGpioAllModes(std::string &projectName, std::string &payload) { + loadGpioInfo(projectName); + + m_gpioDatagram.deserialize(payload); + + const uint32_t *receivedData = reinterpret_cast(m_gpioDatagram.getBuffer()); + + for (uint8_t i = 0U; i < static_cast(Datagram::Gpio::Port::NUM_GPIO_PORTS) * static_cast(Datagram::Gpio::PINS_PER_PORT); i++) { + size_t blockIndex = (i / 8U); /* 4 bits per pin so there is only 8 pins per block */ + size_t bitOffset = (i % 8U) * 4U; /* Multiply by 4 because each each mode is 4 bit */ + + uint8_t pinMode = (receivedData[blockIndex] >> bitOffset) & 0x0F; + + std::string key = gpioPortNames[i / Datagram::Gpio::PINS_PER_PORT]; + key += std::to_string(i % Datagram::Gpio::PINS_PER_PORT); + + m_gpioInfo[key][PIN_MODE_KEY] = stringifyPinMode(static_cast(pinMode)); + } + + saveGpioInfo(projectName); +} + +void GpioManager::updateGpioPinAltFunction(std::string &projectName, std::string &payload) { + loadGpioInfo(projectName); + + m_gpioDatagram.deserialize(payload); + + std::string key = gpioPortNames[static_cast(m_gpioDatagram.getGpioPort())]; + key += std::to_string(m_gpioDatagram.getGpioPin()); + + const uint8_t *receivedData = m_gpioDatagram.getBuffer(); + + m_gpioInfo[key][PIN_ALT_FUNC_KEY] = stringifyPinAltFunction(static_cast(receivedData[0U])); + + saveGpioInfo(projectName); +} + +void GpioManager::updateGpioAllAltFunctions(std::string &projectName, std::string &payload) { + loadGpioInfo(projectName); + + m_gpioDatagram.deserialize(payload); + + const uint32_t *receivedData = reinterpret_cast(m_gpioDatagram.getBuffer()); + + for (uint8_t i = 0U; i < static_cast(Datagram::Gpio::Port::NUM_GPIO_PORTS) * static_cast(Datagram::Gpio::PINS_PER_PORT); i++) { + size_t blockIndex = (i / 8U); /* 4 bits per pin so there is only 8 pins per block */ + size_t bitOffset = (i % 8U) * 4U; /* Multiply by 4 because each each mode is 4 bit */ + + uint8_t pinAltFunction = (receivedData[blockIndex] >> bitOffset) & 0x0F; + + std::string key = gpioPortNames[i / Datagram::Gpio::PINS_PER_PORT]; + key += std::to_string(i % Datagram::Gpio::PINS_PER_PORT); + + m_gpioInfo[key][PIN_ALT_FUNC_KEY] = stringifyPinAltFunction(static_cast(pinAltFunction)); + } + + saveGpioInfo(projectName); +} + +std::string GpioManager::createGpioCommand(CommandCode commandCode, std::string &gpioPortPin, std::string data) { + try { + switch (commandCode) { + case CommandCode::GPIO_GET_PIN_STATE: + case CommandCode::GPIO_GET_PIN_MODE: + case CommandCode::GPIO_GET_PIN_ALT_FUNCTION: { + if (gpioPortPin.empty() || gpioPortPin.size() < 2) { + throw std::runtime_error( + "Invalid format for port/pin specification. Good examples: 'A9' " + "'A12' 'B13'"); + break; + } + + Datagram::Gpio::Port port = static_cast(gpioPortPin[0] - 'A'); + uint8_t pin = static_cast(std::stoi(gpioPortPin.substr(1))); + + if (port >= Datagram::Gpio::Port::NUM_GPIO_PORTS) { + throw std::runtime_error("Invalid selection for Gpio ports. Expected: A or B"); + break; + } + + if (pin >= Datagram::Gpio::PINS_PER_PORT) { + throw std::runtime_error("Exceeded maximum number of Gpio pins: " + std::to_string(static_cast(Datagram::Gpio::PINS_PER_PORT))); + break; + } + + m_gpioDatagram.setGpioPort(port); + m_gpioDatagram.setGpioPin(pin); + m_gpioDatagram.setBuffer(nullptr, 0U); + break; + } + + case CommandCode::GPIO_GET_ALL_STATES: + case CommandCode::GPIO_GET_ALL_MODES: + case CommandCode::GPIO_GET_ALL_ALT_FUNCTIONS: { + m_gpioDatagram.setGpioPort(Datagram::Gpio::Port::NUM_GPIO_PORTS); + m_gpioDatagram.setGpioPin(Datagram::Gpio::PINS_PER_PORT); + m_gpioDatagram.setBuffer(nullptr, 0U); + break; + } + + case CommandCode::GPIO_SET_PIN_STATE: { + if (gpioPortPin.empty() || gpioPortPin.size() < 2) { + throw std::runtime_error( + "Invalid format for port/pin specification. Good examples: 'A9' " + "'A12' 'B13'"); + break; + } + + Datagram::Gpio::Port port = static_cast(gpioPortPin[0] - 'A'); + uint8_t pin = static_cast(std::stoi(gpioPortPin.substr(1))); + uint8_t pinState; + + if (port >= Datagram::Gpio::Port::NUM_GPIO_PORTS) { + throw std::runtime_error("Invalid selection for Gpio ports. Expected: A or B"); + break; + } + + if (pin >= Datagram::Gpio::PINS_PER_PORT) { + throw std::runtime_error("Exceeded maximum number of Gpio pins: " + std::to_string(static_cast(Datagram::Gpio::PINS_PER_PORT))); + break; + } + + if (data == "HIGH") { + pinState = static_cast(Datagram::Gpio::State::GPIO_STATE_HIGH); + } else if (data == "LOW") { + pinState = static_cast(Datagram::Gpio::State::GPIO_STATE_LOW); + } else { + throw std::runtime_error("Invalid Gpio state: " + data); + break; + } + + m_gpioDatagram.setGpioPort(port); + m_gpioDatagram.setGpioPin(pin); + m_gpioDatagram.setBuffer(&pinState, sizeof(pinState)); + + break; + } + + case CommandCode::GPIO_SET_ALL_STATES: { + uint8_t pinState; + if (data == "HIGH") { + pinState = static_cast(Datagram::Gpio::State::GPIO_STATE_HIGH); + } else if (data == "LOW") { + pinState = static_cast(Datagram::Gpio::State::GPIO_STATE_LOW); + } else { + throw std::runtime_error("Invalid Gpio state: " + data); + break; + } + + m_gpioDatagram.setGpioPort(Datagram::Gpio::Port::NUM_GPIO_PORTS); + m_gpioDatagram.setGpioPin(Datagram::Gpio::PINS_PER_PORT); + m_gpioDatagram.setBuffer(&pinState, sizeof(pinState)); + + break; + } + + default: { + throw std::runtime_error("Invalid command code"); + break; + } + } + + return m_gpioDatagram.serialize(commandCode); + + } catch (std::exception &e) { + std::cerr << "Gpio Manager error: " << e.what() << std::endl; + } + return ""; +} diff --git a/simulation/server/app/src/i2c_manager.cc b/simulation/server/app/src/i2c_manager.cc new file mode 100644 index 0000000..5f23d18 --- /dev/null +++ b/simulation/server/app/src/i2c_manager.cc @@ -0,0 +1,18 @@ +/************************************************************************************************ + * @file i2c_manager.cc + * + * @brief Source file defining the I2CManager Class for the server + * + * @date 2025-01-04 + * @author Aryan Kashem + ************************************************************************************************/ + +/* Standard library Headers */ +#include + +/* Inter-component Headers */ +#include "command_code.h" + +/* Intra-component Headers */ +#include "app.h" +#include "i2c_manager.h" diff --git a/simulation/server/app/src/main.cc b/simulation/server/app/src/main.cc new file mode 100644 index 0000000..338fa51 --- /dev/null +++ b/simulation/server/app/src/main.cc @@ -0,0 +1,73 @@ +/************************************************************************************************ + * @file main.cc + * + * @brief Main source file + * + * @date 2025-01-04 + * @author Aryan Kashem + ************************************************************************************************/ + +/* Standard library Headers */ +#include +#include + +/* Inter-component Headers */ +#include "client_connection.h" +#include "json_manager.h" +#include "ntp_server.h" +#include "server.h" + +/* Intra-component Headers */ +#include "app.h" +#include "app_callback.h" +#include "app_terminal.h" +#include "can_listener.h" +#include "can_scheduler.h" +#include "gpio_manager.h" + +JSONManager serverJSONManager; +GpioManager serverGpioManager; +CanListener serverCanListener; +CanScheduler serverCanScheduler; + +int main(int argc, char **argv) { + std::cout << "Running Server" << std::endl; + Server Server; + Terminal applicationTerminal(&Server); + + Server.listenClients(1024, applicationMessageCallback, applicationConnectCallback); + + serverCanListener.listenCanBus(); + + serverCanScheduler.startCanScheduler(); + + serverCanScheduler.update_battery_vt_voltage(15000); + serverCanScheduler.update_battery_vt_current(45000); + serverCanScheduler.update_battery_vt_batt_perc(79); + serverCanScheduler.update_battery_status_aux_batt_v(12000); + + serverCanScheduler.update_afe1_status_v1(42500); + serverCanScheduler.update_afe1_status_v2(40512); + serverCanScheduler.update_afe1_status_v3(41500); + + serverCanScheduler.update_afe2_status_v1(42356); + serverCanScheduler.update_afe2_status_v2(39786); + serverCanScheduler.update_afe2_status_v3(40020); + + serverCanScheduler.update_afe3_status_v1(41340); + serverCanScheduler.update_afe3_status_v2(40520); + serverCanScheduler.update_afe3_status_v3(39612); + + serverCanScheduler.update_afe1_status_temp(27); + serverCanScheduler.update_afe2_status_temp(26); + serverCanScheduler.update_afe3_status_temp(28); + +#if USE_NETWORK_TIME_PROTOCOL == 1U + ntp_server.startListening("127.0.0.1", "time.google.com"); + NTPServer ntp_server; +#endif + + applicationTerminal.run(); + + return 0; +} diff --git a/simulation/server/app/src/spi_manager.cc b/simulation/server/app/src/spi_manager.cc new file mode 100644 index 0000000..50329fd --- /dev/null +++ b/simulation/server/app/src/spi_manager.cc @@ -0,0 +1,18 @@ +/************************************************************************************************ + * @file spi_manager.cc + * + * @brief Source file defining the SPIManager Class for the server + * + * @date 2025-01-04 + * @author Aryan Kashem + ************************************************************************************************/ + +/* Standard library Headers */ +#include + +/* Inter-component Headers */ +#include "command_code.h" + +/* Intra-component Headers */ +#include "app.h" +#include "spi_manager.h" diff --git a/simulation/server/utils/inc/client_connection.h b/simulation/server/utils/inc/client_connection.h new file mode 100644 index 0000000..c3819c4 --- /dev/null +++ b/simulation/server/utils/inc/client_connection.h @@ -0,0 +1,114 @@ +#pragma once + +/************************************************************************************************ + * @file client_connection.h + * + * @brief Header file defining the ClientConnection class + * + * @date 2025-01-04 + * @author Aryan Kashem + ************************************************************************************************/ + +/* Standard library Headers */ +#include +#include + +/* Inter-component Headers */ +#include +#include + +/* Intra-component Headers */ + +/** + * @defgroup Server_Utils + * @brief Server Utilities and Infrastructure + * @{ + */ + +/* Forward declaration */ +class Server; + +/** + * @class ClientConnection + * @brief Class that represents a connection between the server and a client + * @details This class is responsible for managing a single client connection, + * monitoring client activity, and sending messages between the server and the client + */ +class ClientConnection { + private: + std::atomic m_isConnected; /**< Atomic flag indicating whether the client is connected */ + int m_clientPort; /**< The clients port which it is connected on */ + int m_clientSocket; /**< The clients file descriptor (FD) */ + struct sockaddr_in m_clientAddress; /**< The clients address */ + std::string m_clientName; /**< The clients name */ + + Server *server; /**< Pointer to the server instance */ + + public: + /** + * @brief Constructs a ClientConnection object + * @details Initializes the client connection with the server. The constructor sets up internal + * variables and prepares the client to accept and communicate + * @param server The server that this client will be connected to + */ + ClientConnection(Server *server); + + /** + * @brief Destructs a ClientConnection object + * @details If using TCP this closes the existing socket connection + */ + ~ClientConnection(); + + /** + * @brief Accepts a client on the listening socket + * @details This sets the client socket as a non-blocking FD. The server shall add the + * client socket to the EPOLL list for listening + * @param listeningSocket The listening sockets FD + */ + bool acceptClient(int listeningSocket); + + /** + * @brief Sends a message to the client + * @param message String message to be sent + */ + void sendMessage(const std::string &message); + + /** + * @brief Gets the clients name + * @return The clients name + */ + std::string getClientName() const; + + /** + * @brief Sets the clients name + * @param name The new client name + */ + void setClientName(const std::string &name); + + /** + * @brief Gets the clients port + * @return The clients port + */ + int getClientPort() const; + + /** + * @brief Gets the clients socket FD + * @return The clients socket FD + */ + int getSocketFd() const; + + /** + * @brief Gets the clients address + * @return The clients address + */ + std::string getClientAddress() const; + + /** + * @brief Gets the clients connection status + * @return TRUE if the client is connected + * FALSE if the client is disconnected + */ + bool isConnected(); +}; + +/** @} */ diff --git a/simulation/server/utils/inc/ntp_server.h b/simulation/server/utils/inc/ntp_server.h new file mode 100644 index 0000000..16e5bf7 --- /dev/null +++ b/simulation/server/utils/inc/ntp_server.h @@ -0,0 +1,92 @@ +#pragma once + +/************************************************************************************************ + * @file ntp_server.h + * + * @brief Header file defining the NTPServer class + * + * @date 2025-01-04 + * @author Aryan Kashem + ************************************************************************************************/ + +/* Standard library Headers */ +#include +#include +#include +#include + +/* Inter-component Headers */ +#include +#include +#include +#include + +#include "network_time_protocol.h" + +/* Intra-component Headers */ + +/** + * @defgroup Server_Utils + * @brief Server Utilities and Infrastructure + * @{ + */ + +/** + * @class NTPServer + * @brief Class that represents the netowrk-time-protocol server that synchronizes clients + * @details This class is responsible for fetching stratum 0/1 time and forwarding it to clients + * The server shall account for timing errors/differences + * The client shall be responsibile for adjusting their timing + */ +class NTPServer { + private: + static constexpr int NTP_PORT = 123; /**< Universal port for network-time-protocol */ + static constexpr unsigned int NTP_SYNC_PERIOD_S = 60U; /**< Client synchronization period */ + + int m_NTPSocket; /**< The NTP servers socket FD */ + std::string m_bindAddress; /**< The NTP servers bind address */ + std::string m_NTPServerAddress; /**< The NTP servers address for fetching stratum 0/1 data (ie: time.google.com) */ + pthread_t m_NTPServerThreadId; /**< Thread Id for synchronizing clients */ + std::atomic m_isListening; /**< Boolean flag to indicate the NTP servers status */ + + /** + * @brief Processes an incoming NTP request and returns the response packet + * @details This shall create a stratum 2 time image + * If the NTP server query is succesful, the times shall be updated + * @param request The incoming NTP request packet + * @return The NTP response packet + */ + NTPPacket processNTPRequest(const NTPPacket &request); + + /** + * @brief Queries the remote NTP server and fills the response packet + * @details This shall fetch stratum 0/1 timing data + * The response from the server will contain the most accurate time information + * @param response The NTP response packet that will be filled with data from the server + * @return True if the query was successful, otherwise false + */ + bool queryNTPServer(NTPPacket &response); + + public: + /** + * @brief Constructs a NTPServer object + * @details Initializes the NTP server. The constructor sets up internal variables + */ + NTPServer(); + + /** + * @brief Thread procedure for handling NTP synchronization + * @details This thread shall run at the NTP_SYNC_PERIOD_S frequency + */ + void NTPServerProcedure(); + + /** + * @brief Starts listening to NTP clients to begin synchronization + * @details This function will spawn the NTPServerProcedure thread + * @param bindAddress The local address to bind the NTP server + * @param NTPServerAddress The external NTP server address to query for time data + */ + void startListening(const std::string &bindAddress, const std::string &NTPServerAddress); +}; + +/** @} */ diff --git a/simulation/server/utils/inc/server.h b/simulation/server/utils/inc/server.h new file mode 100644 index 0000000..3edbee8 --- /dev/null +++ b/simulation/server/utils/inc/server.h @@ -0,0 +1,159 @@ +#pragma once + +/************************************************************************************************ + * @file server.h + * + * @brief Header file defining the Server class + * + * @date 2025-01-04 + * @author Aryan Kashem + ************************************************************************************************/ + +/* Standard library Headers */ +#include +#include +#include +#include +#include + +/* Inter-component Headers */ +#include +#include +#include + +/* Intra-component Headers */ +#include "client_connection.h" + +/** + * @defgroup Server_Utils + * @brief Server Utilities and Infrastructure + * @{ + */ + +/** + * @class Server + * @brief Class that represents the central server that connects to multiple clients + * @details This class is responsible for managing all clients connections, accepting connections, + * monitoring client activity, and sending messages between the server and the client + */ +class Server { + private: + /** @brief The message callback function definition */ + using messageCallback = std::function; + /** @brief The connection callback function definition */ + using connectCallback = std::function; + + static const constexpr unsigned int MAX_SERVER_EPOLL_EVENTS = 64U; /**< Maximum permitted EPOLL events for tracking clients */ + static const constexpr size_t MAX_CLIENT_READ_SIZE = 256U; /**< Maximum permitted read size for all clients */ + + pthread_t m_listenNewClientsId; /**< Thread Id for listening to new clients */ + pthread_t m_epollClientsId; /**< Thread Id for reading incoming client data */ + pthread_mutex_t m_mutex; /**< Mutex to protect m_connections map */ + + messageCallback m_messageCallback; /**< Function pointer to store the message callback */ + connectCallback m_connectCallback; /**< Function pointer to store the connection callback */ + + std::unordered_map m_connections; /**< Hash-map to store connections based on their string names */ + + std::atomic m_serverListening; /**< Boolean flag to indicate the servers status */ + + int m_listenPort; /**< The servers port to listen on */ + int m_listeningSocket; /**< The servers listening socket FD */ + struct sockaddr_in m_serverAddress; /**< The servers address */ + + int m_epollFd; /**< The servers EPOLL FD to manage reading all clients */ + struct epoll_event m_epollEvents[MAX_SERVER_EPOLL_EVENTS]; /**< An array to store all EPOLL events as bitmasks */ + + public: + /** + * @brief Constructs a Server object + * @details Initializes the server. The constructor sets up internal + * variables and prepares the server to accept and communicate + */ + Server(); + + /** + * @brief Destructs a Server object + * @details If using TCP this closes all existing socket connections + * This shall also delete the mutex, and terminate both running threads + */ + ~Server(); + + /** + * @brief Thread procedure for listening for new client connections + * @details This thread shall be blocked while no new clients are available + * Upon connection, the client shall be declared as non-blocking, and will + * be added to the EPOLL list as an input FD. Further, the connection callback + * shall be called + */ + void listenNewClientsProcedure(); + + /** + * @brief Thread procedure for reading incoming client data + * @details This thread shall be blocked while no new client data is available + * Upon receiving a message, the message callback shall be called + */ + void epollClientsProcedure(); + + /** + * @brief Stops the server + * @details This shall terminate all connections safely, and terminate running threads + */ + void stop(); + + /** + * @brief Initiate the server to listen to clients on a provided port + * @details This shall start the listenNewClientsProcedure and epollClientsProcedure + * @param port Port for the server to listen on + * @param messageCallback Function pointer to a message callback + * @param connectCallback Function pointer to a connection callback + */ + void listenClients(int port, messageCallback messageCallback, connectCallback connectCallback); + + /** + * @brief Function wrapper around the message callback + * @param client Pointer to the client which has received a message + * @param message String message value that has been received + */ + void messageReceived(ClientConnection *client, std::string &message); + + /** + * @brief Function wrapper to transmit a message + * @param client Pointer to the client which shall be written to + * @param message String message value to be sent + */ + void sendMessage(ClientConnection *client, const std::string &message); + + /** + * @brief Function wrapper to broadcast a message to all clients + * @param message String message value to be sent + */ + void broadcastMessage(const std::string &message); + + /** + * @brief Update a clients name + * @param client Pointer to the client which shall be updated + * @param newName String value of the clients new name + */ + void updateClientName(ClientConnection *client, std::string newName); + + /** + * @brief Remove a client + * @param client Pointer to the client which shall be removed + */ + void removeClient(ClientConnection *client); + + /** + * @brief Get a client by its name + * @param clientName String value of the clients name + * @return Pointer to the respective client connection object + */ + ClientConnection *getClientByName(std::string &clientName); + + /** + * @brief Print a list of all connected clients + */ + void dumpClientList(); +}; + +/** @} */ diff --git a/simulation/server/utils/src/client_connection.cc b/simulation/server/utils/src/client_connection.cc new file mode 100644 index 0000000..d70e762 --- /dev/null +++ b/simulation/server/utils/src/client_connection.cc @@ -0,0 +1,89 @@ +/************************************************************************************************ + * @file client_connection.cc + * + * @brief Source file defining the ClientConnection class + * + * @date 2025-01-04 + * @author Aryan Kashem + ************************************************************************************************/ + +/* Standard library Headers */ +#include + +/* Inter-component Headers */ +#include +#include + +/* Intra-component Headers */ +#include "client_connection.h" +#include "server.h" + +std::string ClientConnection::getClientAddress() const { + char ip_str[INET_ADDRSTRLEN]; + inet_ntop(AF_INET, &(m_clientAddress.sin_addr), ip_str, INET_ADDRSTRLEN); + return std::string(ip_str); +} + +ClientConnection::ClientConnection(Server *server) { + this->server = server; + m_isConnected = false; +} + +ClientConnection::~ClientConnection() { + close(m_clientSocket); +} + +bool ClientConnection::acceptClient(int listeningSocket) { + socklen_t clientLength = sizeof(m_clientAddress); + m_clientSocket = accept(listeningSocket, (struct sockaddr *)&m_clientAddress, &clientLength); + + if (m_clientSocket < 0) { + std::cerr << "Failed to accept client connection" << std::endl; + return false; + } + + /* Set as non blocking */ + int flags = fcntl(m_clientSocket, F_GETFL, 0); + fcntl(m_clientSocket, F_SETFL, flags | O_NONBLOCK); + + if (m_clientName.empty()) { + m_clientName = getClientAddress(); + } + + m_isConnected = true; + return true; +} + +void ClientConnection::sendMessage(const std::string &message) { + if (!m_isConnected) { + throw std::runtime_error("Attempting to send on unconnected socket"); + } + + size_t bytesSent = write(m_clientSocket, message.c_str(), message.length()); + if (bytesSent != message.length()) { + throw std::runtime_error("Failed to send complete message"); + } +} + +std::string ClientConnection::getClientName() const { + return m_clientName; +} + +void ClientConnection::setClientName(const std::string &name) { + m_clientName = name; +} + +int ClientConnection::getClientPort() const { + return m_clientPort; +} + +int ClientConnection::getSocketFd() const { + if (m_clientSocket < 0) { + throw std::runtime_error("Attempting to get invalid socket descriptor"); + } + return m_clientSocket; +} + +bool ClientConnection::isConnected() { + return m_isConnected; +} diff --git a/simulation/server/utils/src/ntp_server.cc b/simulation/server/utils/src/ntp_server.cc new file mode 100644 index 0000000..5426a1d --- /dev/null +++ b/simulation/server/utils/src/ntp_server.cc @@ -0,0 +1,197 @@ +/************************************************************************************************ + * @file ntp_server.cc + * + * @brief Source file defining the NTPServer class + * + * @date 2025-01-04 + * @author Aryan Kashem + ************************************************************************************************/ + +/* Standard library Headers */ +#include +#include + +/* Inter-component Headers */ +#include + +/* Intra-component Headers */ +#include "ntp_server.h" + +bool NTPServer::queryNTPServer(NTPPacket &response) { + struct addrinfo hints = {}, *addrs; + hints.ai_family = AF_INET; + hints.ai_socktype = SOCK_DGRAM; + hints.ai_protocol = IPPROTO_UDP; + + if (getaddrinfo(m_NTPServerAddress.c_str(), "123", &hints, &addrs) != 0) { + freeaddrinfo(addrs); + throw std::runtime_error("DNS resolution failed"); + } + + int ntpSocket = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP); + if (ntpSocket < 0) { + std::cerr << "Error creating socket" << std::endl; + return false; + } + + NTPPacket request = {}; + request.flags = (NTP_VERSION << NTP_VERSION_OFFSET) | (NTPLeapIndicator::NTP_LI_NOSYNC << NTP_LEAP_INDICATOR_OFFSET) | (NTPMode::NTP_CLIENT_MODE << NTP_MODE_OFFSET); + + auto now = std::chrono::system_clock::now(); + auto duration = now.time_since_epoch(); + auto seconds = std::chrono::duration_cast(duration); + auto nanoSeconds = std::chrono::duration_cast(duration); + + request.transmitTime.seconds = seconds.count() + NTP_UNIX_EPOCH_DIFF; + request.transmitTime.fraction = static_cast((nanoSeconds.count() % 1000000000ULL) * 4.294967296); + + if (sendto(ntpSocket, &request, sizeof(request), 0, addrs->ai_addr, addrs->ai_addrlen) < 0) { + close(ntpSocket); + freeaddrinfo(addrs); + throw std::runtime_error("Send failed"); + } + + NTPPacket serverResponse = {}; + sockaddr_storage serverAddr; + socklen_t serverAddrLen = sizeof(serverAddr); + + ssize_t receivedBytes = recvfrom(ntpSocket, &serverResponse, sizeof(serverResponse), 0, reinterpret_cast(&serverAddr), &serverAddrLen); + + freeaddrinfo(addrs); + + if (receivedBytes < 0) { + close(ntpSocket); + throw std::runtime_error("Receive timeout or error"); + } + + convertNTPTimestamp(serverResponse.referenceTime); + convertNTPTimestamp(serverResponse.originTime); + convertNTPTimestamp(serverResponse.receiveTime); + convertNTPTimestamp(serverResponse.transmitTime); + + serverResponse.rootDelay = ntohl(serverResponse.rootDelay); + serverResponse.rootDispersion = ntohl(serverResponse.rootDispersion); + + response = serverResponse; + + close(ntpSocket); + return true; +} + +NTPPacket NTPServer::processNTPRequest(const NTPPacket &request) { + NTPPacket response = {}; + response.flags = (NTP_VERSION << NTP_VERSION_OFFSET) | (NTPLeapIndicator::NTP_LI_NONE << NTP_LEAP_INDICATOR_OFFSET) | (NTPMode::NTP_SERVER_MODE << NTP_MODE_OFFSET); + + response.stratum = 2U; + response.poll = 4U; /* 2^4 = 16 seconds between succesive messages. */ + response.precision = -6; /* 2^-6 = 15.625 microseconds precision */ + response.rootDelay = htonl(0U); + response.rootDispersion = htonl(0U); + + /* Local reference Id */ + response.referenceId[0] = 'L'; + response.referenceId[1] = 'O'; + response.referenceId[2] = 'C'; + response.referenceId[3] = 'L'; + + auto now = std::chrono::system_clock::now(); + uint32_t currentSeconds = unixToNTPTime(std::chrono::system_clock::to_time_t(now)); + uint32_t currentFraction = static_cast(std::chrono::duration_cast(now.time_since_epoch()).count() % 1000000); + + response.referenceTime.seconds = htonl(currentSeconds); + response.referenceTime.fraction = htonl(currentFraction); + response.originTime.seconds = ntohl(request.transmitTime.seconds); + response.originTime.fraction = ntohl(request.transmitTime.fraction); + response.receiveTime.seconds = htonl(currentSeconds); + response.receiveTime.fraction = htonl(currentFraction); + response.transmitTime.seconds = htonl(currentSeconds); + response.transmitTime.fraction = htonl(currentFraction); + + NTPPacket externalResponse; + if (queryNTPServer(externalResponse)) { + response.referenceTime = externalResponse.referenceTime; + response.originTime = externalResponse.originTime; + response.receiveTime = externalResponse.receiveTime; + response.transmitTime = externalResponse.transmitTime; + } + + return response; +} + +void NTPServer::NTPServerProcedure() { + m_NTPSocket = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP); + + if (m_NTPSocket < 0) { + throw std::runtime_error("Error creating socket for NTP server: " + m_NTPServerAddress); + } + + int enable = 1; + if (setsockopt(m_NTPSocket, SOL_SOCKET, SO_REUSEADDR, &enable, sizeof(int)) < 0) { + throw std::runtime_error("Error setting socket option SO_REUSEADDR"); + } + + struct sockaddr_in serverAddr; + memset((char *)&serverAddr, 0U, sizeof(serverAddr)); + serverAddr.sin_family = AF_INET; + serverAddr.sin_port = htons(NTP_PORT); + + if (inet_pton(AF_INET, m_bindAddress.c_str(), &serverAddr.sin_addr) <= 0) { + close(m_NTPSocket); + throw std::runtime_error("Error converting IPv4 bind address to binary form"); + } + + if (bind(m_NTPSocket, (struct sockaddr *)&serverAddr, sizeof(serverAddr)) < 0) { + close(m_NTPSocket); + throw std::runtime_error("Error binding NTP socket"); + } + + m_isListening = true; + + while (m_isListening) { + NTPPacket request; + struct sockaddr_in clientAddress; + socklen_t clientLength = sizeof(clientAddress); + + if (recvfrom(m_NTPSocket, &request, sizeof(request), 0, (struct sockaddr *)&clientAddress, &clientLength) <= 0) { + std::cerr << "Error receiving NTP request" << std::endl; + continue; + } + + NTPPacket response = processNTPRequest(request); + dumpNTPPacketData(response); + if (sendto(m_NTPSocket, &response, sizeof(response), 0, (struct sockaddr *)&clientAddress, clientLength) <= 0) { + std::cerr << "Error sending NTP response" << std::endl; + } + } + + m_isListening = false; + close(m_NTPSocket); +} + +void *NTPServerWrapper(void *param) { + NTPServer *server = static_cast(param); + + try { + server->NTPServerProcedure(); + } catch (std::exception &e) { + std::cerr << "NTP Server Thread Error: " << e.what() << std::endl; + } + + return nullptr; +} + +void NTPServer::startListening(const std::string &bindAddress, const std::string &NTPServerAddress) { + if (m_isListening) return; + + m_NTPServerAddress = NTPServerAddress; + m_bindAddress = bindAddress; + + if (pthread_create(&m_NTPServerThreadId, nullptr, NTPServerWrapper, this)) { + throw std::runtime_error("Listen Error"); + } +} + +NTPServer::NTPServer() { + m_isListening = false; + m_NTPSocket = -1; +} diff --git a/simulation/server/utils/src/server.cc b/simulation/server/utils/src/server.cc new file mode 100644 index 0000000..990f0f7 --- /dev/null +++ b/simulation/server/utils/src/server.cc @@ -0,0 +1,241 @@ +/************************************************************************************************ + * @file server.cc + * + * @brief Source file defining the Server class + * + * @date 2025-01-04 + * @author Aryan Kashem + ************************************************************************************************/ + +/* Standard library Headers */ +#include + +#include +#include + +/* Inter-component Headers */ +#include + +/* Intra-component Headers */ +#include "client_connection.h" +#include "server.h" + +Server::Server() { + m_serverListening = false; + m_listenPort = -1; + m_listeningSocket = -1; + m_epollFd = -1; + pthread_mutex_init(&m_mutex, nullptr); +} + +Server::~Server() { + pthread_mutex_destroy(&m_mutex); +} + +void Server::listenNewClientsProcedure() { + m_listeningSocket = socket(AF_INET, SOCK_STREAM, IPPROTO_TCP); + + if (m_listeningSocket < 0) { + throw std::runtime_error("Error creating socket for port " + std::to_string(m_listenPort)); + } + + int enable = 1; + if (setsockopt(m_listeningSocket, SOL_SOCKET, SO_REUSEADDR, &enable, sizeof(int)) < 0) { + throw std::runtime_error("Error setting socket option SO_REUSEADDR"); + } + memset((char *)&m_serverAddress, 0U, sizeof(m_serverAddress)); + m_serverAddress.sin_family = AF_INET; + m_serverAddress.sin_port = htons(m_listenPort); + m_serverAddress.sin_addr.s_addr = INADDR_ANY; /* Listen to all addresses */ + + if (bind(m_listeningSocket, (struct sockaddr *)&m_serverAddress, sizeof(m_serverAddress)) < 0) { + throw std::runtime_error("Error binding socket"); + } + + listen(m_listeningSocket, 5); + + while (m_serverListening) { + ClientConnection *client = new ClientConnection(this); + + if (!client->acceptClient(m_listeningSocket)) { + delete client; + break; + } + + /* Add to the EPOLL list */ + struct epoll_event clientEvent; + + /* Edge-triggered input */ + clientEvent.events = EPOLLIN | EPOLLET; + clientEvent.data.ptr = client; + + if (epoll_ctl(m_epollFd, EPOLL_CTL_ADD, client->getSocketFd(), &clientEvent) < 0) { + client->~ClientConnection(); + throw std::runtime_error("Failed to add client connection to EPOLL Interest list"); + break; + } + + if (m_connectCallback) { + m_connectCallback(this, client); + } + + updateClientName(client, client->getClientName()); + } + + m_serverListening = false; +} + +void Server::epollClientsProcedure() { + int numBytes = 0; + int nfds = 0; + char buffer[MAX_CLIENT_READ_SIZE + 1]; + + if (m_epollFd < 0) { + throw std::runtime_error("Invalid epoll FD"); + return; + } + + while (m_serverListening) { + nfds = epoll_wait(m_epollFd, m_epollEvents, MAX_SERVER_EPOLL_EVENTS, -1); + + if (nfds < 0) { + throw std::runtime_error("EPOLL wait failed"); + break; + } + + for (int i = 0; i < nfds; i++) { + ClientConnection *client = static_cast(m_epollEvents[i].data.ptr); + + if (m_epollEvents[i].events & EPOLLIN) { + numBytes = read(client->getSocketFd(), buffer, MAX_CLIENT_READ_SIZE); + + if (numBytes < 0) { + throw std::runtime_error("EPOLL Read failed"); + break; + } + + buffer[numBytes] = '\0'; + std::string msg(buffer, numBytes); + messageReceived(client, msg); + } + } + } + + m_serverListening = false; +} + +void *listenNewClientsWrapper(void *param) { + Server *server = static_cast(param); + + try { + server->listenNewClientsProcedure(); + } catch (std::exception &e) { + std::cerr << "Listen Thread Error: " << e.what() << std::endl; + } + + return nullptr; +} + +void *epollClientsWrapper(void *param) { + Server *server = static_cast(param); + try { + server->epollClientsProcedure(); + } catch (std::exception &e) { + std::cerr << "EPOLL Clients Error: " << e.what() << std::endl; + } + return nullptr; +} + +void Server::listenClients(int port, messageCallback messageCallback, connectCallback connectCallback) { + if (m_serverListening) return; + + m_epollFd = epoll_create1(0U); + if (m_epollFd < 0) { + throw std::runtime_error("Failed to create EPOLL FD"); + } + + m_listenPort = port; + m_messageCallback = messageCallback; + m_connectCallback = connectCallback; + m_serverListening = true; + + if (pthread_create(&m_listenNewClientsId, nullptr, listenNewClientsWrapper, this)) { + throw std::runtime_error("Listen new clients Error"); + } + + if (pthread_create(&m_epollClientsId, nullptr, epollClientsWrapper, this)) { + throw std::runtime_error("EPOLL Clients Error"); + } +} + +void Server::removeClient(ClientConnection *client) { + pthread_mutex_lock(&m_mutex); + if (client) { + std::cerr << "Removed client: " << client->getClientName() << std::endl; + epoll_ctl(m_epollFd, EPOLL_CTL_DEL, client->getSocketFd(), nullptr); + m_connections.erase(client->getClientName()); + client->~ClientConnection(); + } + pthread_mutex_unlock(&m_mutex); +} + +void Server::updateClientName(ClientConnection *client, std::string newName) { + pthread_mutex_lock(&m_mutex); + std::string oldClientName = client->getClientName(); + + if (!oldClientName.empty()) { + m_connections.erase(oldClientName); + } + + client->setClientName(newName); + + /* Handle potential client duplicates */ + std::string clientName = newName; + unsigned int clientDuplicateCount = 1U; + while (m_connections.count(clientName)) { + clientName = newName + " (" + std::to_string(clientDuplicateCount) + ")"; + clientDuplicateCount++; + } + + m_connections[clientName] = client; + client->setClientName(clientName); + pthread_mutex_unlock(&m_mutex); +} + +void Server::messageReceived(ClientConnection *client, std::string &message) { + m_messageCallback(this, client, message); +} + +void Server::sendMessage(ClientConnection *client, const std::string &message) { + client->sendMessage(message); +} + +void Server::broadcastMessage(const std::string &message) { + pthread_mutex_lock(&m_mutex); + for (auto &pair : m_connections) { + if (pair.second->isConnected()) { + pair.second->sendMessage(message); + } + } + pthread_mutex_unlock(&m_mutex); +} + +ClientConnection *Server::getClientByName(std::string &clientName) { + pthread_mutex_lock(&m_mutex); + if (m_connections.count(clientName)) { + pthread_mutex_unlock(&m_mutex); + return m_connections[clientName]; + } + pthread_mutex_unlock(&m_mutex); + return nullptr; +} + +void Server::dumpClientList() { + for (auto &pair : m_connections) { + if (pair.second->isConnected()) { + std::cout << pair.first << std::endl; + } + } +} + +void Server::stop() {}