From e537d7552b28afe4ec4c61d336d0b6f7797c8d42 Mon Sep 17 00:00:00 2001 From: Adrian Del Grosso <10929341+ad3154@users.noreply.github.com> Date: Sun, 10 Sep 2023 10:11:42 -0600 Subject: [PATCH] Add code for version 0.1.0 Initial commit --- README.md | 51 +- library.properties | 12 + src/FlexCAN_T4.hpp | 692 +++ src/FlexCAN_T4.tpp | 1807 +++++++ src/FlexCAN_T4FD.tpp | 833 +++ src/FlexCAN_T4FDTimings.tpp | 569 ++ src/can_NAME.cpp | 161 + src/can_NAME.hpp | 441 ++ src/can_NAME_filter.cpp | 98 + src/can_NAME_filter.hpp | 52 + src/can_address_claim_state_machine.cpp | 376 ++ src/can_address_claim_state_machine.hpp | 108 + src/can_badge.hpp | 40 + src/can_callbacks.cpp | 65 + src/can_callbacks.hpp | 118 + src/can_constants.hpp | 23 + src/can_control_function.cpp | 91 + src/can_control_function.hpp | 98 + src/can_extended_transport_protocol.cpp | 835 +++ src/can_extended_transport_protocol.hpp | 237 + src/can_general_parameter_group_numbers.hpp | 57 + src/can_hardware_abstraction.hpp | 36 + src/can_hardware_interface_single_thread.cpp | 265 + src/can_hardware_interface_single_thread.hpp | 128 + src/can_hardware_plugin.hpp | 47 + src/can_identifier.cpp | 160 + src/can_identifier.hpp | 118 + src/can_internal_control_function.cpp | 64 + src/can_internal_control_function.hpp | 73 + src/can_message.cpp | 185 + src/can_message.hpp | 169 + src/can_message_frame.cpp | 35 + src/can_message_frame.hpp | 40 + src/can_network_configuration.cpp | 60 + src/can_network_configuration.hpp | 86 + src/can_network_manager.cpp | 1001 ++++ src/can_network_manager.hpp | 378 ++ ...arameter_group_number_request_protocol.cpp | 306 ++ ...arameter_group_number_request_protocol.hpp | 168 + src/can_partnered_control_function.cpp | 182 + src/can_partnered_control_function.hpp | 119 + src/can_protocol.cpp | 60 + src/can_protocol.hpp | 95 + src/can_stack_logger.cpp | 86 + src/can_stack_logger.hpp | 175 + src/can_transport_protocol.cpp | 870 ++++ src/can_transport_protocol.hpp | 237 + src/circular_buffer.hpp | 781 +++ src/event_dispatcher.hpp | 124 + src/flex_can_t4_plugin.cpp | 125 + src/flex_can_t4_plugin.hpp | 63 + src/imxrt_flexcan.hpp | 1205 +++++ src/isobus.hpp | 61 + src/isobus_device_descriptor_object_pool.cpp | 1054 ++++ src/isobus_device_descriptor_object_pool.hpp | 191 + src/isobus_diagnostic_protocol.cpp | 1358 +++++ src/isobus_diagnostic_protocol.hpp | 510 ++ src/isobus_functionalities.cpp | 938 ++++ src/isobus_functionalities.hpp | 480 ++ src/isobus_guidance_interface.cpp | 558 ++ src/isobus_guidance_interface.hpp | 405 ++ src/isobus_language_command_interface.cpp | 232 + src/isobus_language_command_interface.hpp | 264 + src/isobus_maintain_power_interface.cpp | 350 ++ src/isobus_maintain_power_interface.hpp | 287 ++ src/isobus_shortcut_button_interface.cpp | 236 + src/isobus_shortcut_button_interface.hpp | 164 + src/isobus_speed_distance_messages.cpp | 896 ++++ src/isobus_speed_distance_messages.hpp | 632 +++ ...obus_standard_data_description_indices.hpp | 548 ++ src/isobus_task_controller_client.cpp | 2067 ++++++++ src/isobus_task_controller_client.hpp | 618 +++ src/isobus_task_controller_client_objects.cpp | 600 +++ src/isobus_task_controller_client_objects.hpp | 518 ++ src/isobus_virtual_terminal_client.cpp | 4589 +++++++++++++++++ src/isobus_virtual_terminal_client.hpp | 1641 ++++++ src/isobus_virtual_terminal_objects.hpp | 81 + src/kinetis_flexcan.hpp | 1141 ++++ src/nmea2000_fast_packet_protocol.cpp | 540 ++ src/nmea2000_fast_packet_protocol.hpp | 218 + src/platform_endianness.cpp | 27 + src/platform_endianness.hpp | 25 + src/processing_flags.cpp | 62 + src/processing_flags.hpp | 35 + src/system_timing.cpp | 80 + src/system_timing.hpp | 33 + src/to_string.hpp | 33 + 87 files changed, 35676 insertions(+), 1 deletion(-) create mode 100644 library.properties create mode 100644 src/FlexCAN_T4.hpp create mode 100644 src/FlexCAN_T4.tpp create mode 100644 src/FlexCAN_T4FD.tpp create mode 100644 src/FlexCAN_T4FDTimings.tpp create mode 100644 src/can_NAME.cpp create mode 100644 src/can_NAME.hpp create mode 100644 src/can_NAME_filter.cpp create mode 100644 src/can_NAME_filter.hpp create mode 100644 src/can_address_claim_state_machine.cpp create mode 100644 src/can_address_claim_state_machine.hpp create mode 100644 src/can_badge.hpp create mode 100644 src/can_callbacks.cpp create mode 100644 src/can_callbacks.hpp create mode 100644 src/can_constants.hpp create mode 100644 src/can_control_function.cpp create mode 100644 src/can_control_function.hpp create mode 100644 src/can_extended_transport_protocol.cpp create mode 100644 src/can_extended_transport_protocol.hpp create mode 100644 src/can_general_parameter_group_numbers.hpp create mode 100644 src/can_hardware_abstraction.hpp create mode 100644 src/can_hardware_interface_single_thread.cpp create mode 100644 src/can_hardware_interface_single_thread.hpp create mode 100644 src/can_hardware_plugin.hpp create mode 100644 src/can_identifier.cpp create mode 100644 src/can_identifier.hpp create mode 100644 src/can_internal_control_function.cpp create mode 100644 src/can_internal_control_function.hpp create mode 100644 src/can_message.cpp create mode 100644 src/can_message.hpp create mode 100644 src/can_message_frame.cpp create mode 100644 src/can_message_frame.hpp create mode 100644 src/can_network_configuration.cpp create mode 100644 src/can_network_configuration.hpp create mode 100644 src/can_network_manager.cpp create mode 100644 src/can_network_manager.hpp create mode 100644 src/can_parameter_group_number_request_protocol.cpp create mode 100644 src/can_parameter_group_number_request_protocol.hpp create mode 100644 src/can_partnered_control_function.cpp create mode 100644 src/can_partnered_control_function.hpp create mode 100644 src/can_protocol.cpp create mode 100644 src/can_protocol.hpp create mode 100644 src/can_stack_logger.cpp create mode 100644 src/can_stack_logger.hpp create mode 100644 src/can_transport_protocol.cpp create mode 100644 src/can_transport_protocol.hpp create mode 100644 src/circular_buffer.hpp create mode 100644 src/event_dispatcher.hpp create mode 100644 src/flex_can_t4_plugin.cpp create mode 100644 src/flex_can_t4_plugin.hpp create mode 100644 src/imxrt_flexcan.hpp create mode 100644 src/isobus.hpp create mode 100644 src/isobus_device_descriptor_object_pool.cpp create mode 100644 src/isobus_device_descriptor_object_pool.hpp create mode 100644 src/isobus_diagnostic_protocol.cpp create mode 100644 src/isobus_diagnostic_protocol.hpp create mode 100644 src/isobus_functionalities.cpp create mode 100644 src/isobus_functionalities.hpp create mode 100644 src/isobus_guidance_interface.cpp create mode 100644 src/isobus_guidance_interface.hpp create mode 100644 src/isobus_language_command_interface.cpp create mode 100644 src/isobus_language_command_interface.hpp create mode 100644 src/isobus_maintain_power_interface.cpp create mode 100644 src/isobus_maintain_power_interface.hpp create mode 100644 src/isobus_shortcut_button_interface.cpp create mode 100644 src/isobus_shortcut_button_interface.hpp create mode 100644 src/isobus_speed_distance_messages.cpp create mode 100644 src/isobus_speed_distance_messages.hpp create mode 100644 src/isobus_standard_data_description_indices.hpp create mode 100644 src/isobus_task_controller_client.cpp create mode 100644 src/isobus_task_controller_client.hpp create mode 100644 src/isobus_task_controller_client_objects.cpp create mode 100644 src/isobus_task_controller_client_objects.hpp create mode 100644 src/isobus_virtual_terminal_client.cpp create mode 100644 src/isobus_virtual_terminal_client.hpp create mode 100644 src/isobus_virtual_terminal_objects.hpp create mode 100644 src/kinetis_flexcan.hpp create mode 100644 src/nmea2000_fast_packet_protocol.cpp create mode 100644 src/nmea2000_fast_packet_protocol.hpp create mode 100644 src/platform_endianness.cpp create mode 100644 src/platform_endianness.hpp create mode 100644 src/processing_flags.cpp create mode 100644 src/processing_flags.hpp create mode 100644 src/system_timing.cpp create mode 100644 src/system_timing.hpp create mode 100644 src/to_string.hpp diff --git a/README.md b/README.md index c96c521..c371c0c 100644 --- a/README.md +++ b/README.md @@ -1,2 +1,51 @@ # AgIsoStack-Arduino -AgIsoStack++ modified for the Arduino Library Manager +AgIsoStack is a free ISOBUS (ISO11783) and SAE J1939 compatible CAN stack that makes communication on off-highway vehicle CAN networks easy. + +Currently this library is compatible with Teensy hardware only. ESP32 support will likely be added at some point, but for now PlatformIO + ESP-IDF is supported for ESP32 platforms via the [main repo](https://github.com/Open-Agriculture/AgIsoStack-plus-plus). + +### Features + +- ISO11783/J1939 Address Claiming +- ISO11783 Universal/Virtual Terminal Client +- ISO11783 Task Controller Client +- NMEA2000 Fast Packet +- J1939 and ISO11783 Diagnostic Messages +- Implement/Machine speed and guidance messaging +- ISOBUS Shortcut Button (ISB) +- ISO11783 Transport Protocol +- ISO11783 Extended Transport Protocol +- Designed to integrate with proprietary messaging as needed + +### Example + +An example ino file for Arduino IDE is located in the main AgIsoStack++ repo. [Starting from here](https://github.com/Open-Agriculture/AgIsoStack-plus-plus/tree/main/examples/arduino_example) is recommended! +The example sketch loads a VT object pool to a virtual terminal, as long as your teensy is connected to an ISO11783 CAN network using the Teensy's CAN 1 pins and compatible CAN transceiver. + +### Troubleshooting + +* My Teensy won't start up after loading this library, or otherwise does nothing + * There might be a bug in the Teensy core, where compiling with non-default optimization causes this issue. + * You can fix this by editing your startup.c file for your Teensy. + * Navigate to your teensy core files located normally at `C:\Users\\AppData\Local\Arduino15\packages\teensy\hardware\avr\\cores\teensy4` + * Open `startup.c` + * Add this line after the `#include` directives: `#pragma GCC optimize ("O2")` +* My Teensy starts fine, but no object pool is loaded! + * Make sure you are using a [proper CAN transceiver](https://www.amazon.com/SN65HVD230-CAN-Board-Communication-Development/dp/B00KM6XMXO). You can't connect CTX1 and CRX1 directly to a CAN bus. + * Make sure you don't have CAN-H and CAN-L reversed. + * Make sure your CAN network is properly terminated. + * Try and view the serial output from the Teensy to see if any errors are shown. + * Make sure your object pool isn't in EXTMEM without first being initialized, otherwise your pool might be all zeros, or worse. +* My program doesn't compile because my Teensy is out of RAM1 space + * Make sure you are compiling with optimizations set to "Smallest Code". +* I don't see Teensy as a board option in Arduino IDE + * Ensure you have followed the steps [outlined here](https://www.pjrc.com/teensy/td_download.html) to add the Teensy boards. +* Something else! + * Open an issue or start a discussion here on GitHub and a maintainer will try to help if they can. + +### Documentation + +View the [precompiled doxygen](https://delgrossoengineering.com/isobus-docs/) or visit our [tutorial site](https://isobus-plus-plus.readthedocs.io/en/latest/). + +Note that the documentation is for the full C++ library, so file paths and other minor differences might exist on Arduino, but they should be very minor. + +You may also want to view the main repo's [examples](https://github.com/Open-Agriculture/AgIsoStack-plus-plus/tree/main/examples). diff --git a/library.properties b/library.properties new file mode 100644 index 0000000..4d6eb7c --- /dev/null +++ b/library.properties @@ -0,0 +1,12 @@ +name=AgIsoStack-plus-plus +version=0.1.0 +license=MIT +author=Adrian Del Grosso +maintainer=Adrian Del Grosso +sentence=A free ISOBUS (ISO11783) and J1939 CAN Stack for Teensy. +paragraph=Includes ISOBUS virtual terminal client, task controller client, and transport layer functionality. Based on the CMake AgIsoStack++ at https://github.com/Open-Agriculture/AgIsoStack-plus-plus. +category=Communication +architectures=teensy +includes=isobus.hpp +url=https://github.com/Open-Agriculture/AgIsoStack-Arduino + diff --git a/src/FlexCAN_T4.hpp b/src/FlexCAN_T4.hpp new file mode 100644 index 0000000..206306c --- /dev/null +++ b/src/FlexCAN_T4.hpp @@ -0,0 +1,692 @@ +/* + MIT License + + Copyright (c) 2018 Antonio Alexander Brewer (tonton81) - https://github.com/tonton81 + + Designed and tested for PJRC Teensy 4.0. + + Forum link : https://forum.pjrc.com/threads/56035-FlexCAN_T4-FlexCAN-for-Teensy-4?highlight=flexcan_t4 + + Thanks goes to skpang, mjs513, and collin for tech/testing support + + Permission is hereby granted, free of charge, to any person obtaining a copy + of this software and associated documentation files (the "Software"), to deal + in the Software without restriction, including without limitation the rights + to use, copy, modify, merge, publish, distribute, sublicense, and / or sell + copies of the Software, and to permit persons to whom the Software is + furnished to do so, subject to the following conditions: + + The above copyright notice and this permission notice shall be included in all + copies or substantial portions of the Software. + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + SOFTWARE. +*/ +#if !defined(_FLEXCAN_T4_H_) +#define _FLEXCAN_T4_H_ + +#include "Arduino.h" +#include "circular_buffer.hpp" +#include "imxrt_flexcan.hpp" + +typedef struct CAN_error_t +{ + char state[30] = "Idle"; + bool BIT1_ERR = 0; + bool BIT0_ERR = 0; + bool ACK_ERR = 0; + bool CRC_ERR = 0; + bool FRM_ERR = 0; + bool STF_ERR = 0; + bool TX_WRN = 0; + bool RX_WRN = 0; + char FLT_CONF[14] = { 0 }; + uint8_t RX_ERR_COUNTER = 0; + uint8_t TX_ERR_COUNTER = 0; + uint32_t ESR1 = 0; + uint16_t ECR = 0; +} CAN_error_t; + +typedef struct CAN_message_t +{ + uint32_t id = 0; // can identifier + uint16_t timestamp = 0; // FlexCAN time when message arrived + uint8_t idhit = 0; // filter that id came from + struct + { + bool extended = 0; // identifier is extended (29-bit) + bool remote = 0; // remote transmission request packet type + bool overrun = 0; // message overrun + bool reserved = 0; + } flags; + uint8_t len = 8; // length of data + uint8_t buf[8] = { 0 }; // data + int8_t mb = 0; // used to identify mailbox reception + uint8_t bus = 0; // used to identify where the message came from when events() is used. + bool seq = 0; // sequential frames +} CAN_message_t; + +typedef struct CANFD_message_t +{ + uint32_t id = 0; // can identifier + uint16_t timestamp = 0; // FlexCAN time when message arrived + uint8_t idhit = 0; // filter that id came from + bool brs = 1; // baud rate switching for data + bool esi = 0; // error status indicator + bool edl = 1; // extended data length (for RX, 0 == CAN2.0, 1 == FD) + struct + { + bool extended = 0; // identifier is extended (29-bit) + bool overrun = 0; // message overrun + bool reserved = 0; + } flags; + uint8_t len = 8; // length of data + uint8_t buf[64] = { 0 }; // data + int8_t mb = 0; // used to identify mailbox reception + uint8_t bus = 0; // used to identify where the message came from when events() is used. + bool seq = 0; // sequential frames +} CANFD_message_t; + +typedef void (*_MB_ptr)(const CAN_message_t &msg); /* mailbox / global callbacks */ +typedef void (*_MBFD_ptr)(const CANFD_message_t &msg); /* mailbox / global callbacks */ + +typedef enum FLEXCAN_PINS +{ + ALT = 0, + DEF = 1, +} FLEXCAN_PINS; + +typedef enum FLEXCAN_MAILBOX +{ + MB0 = 0, + MB1 = 1, + MB2 = 2, + MB3 = 3, + MB4 = 4, + MB5 = 5, + MB6 = 6, + MB7 = 7, + MB8 = 8, + MB9 = 9, + MB10 = 10, + MB11 = 11, + MB12 = 12, + MB13 = 13, + MB14 = 14, + MB15 = 15, + MB16 = 16, + MB17 = 17, + MB18 = 18, + MB19 = 19, + MB20 = 20, + MB21 = 21, + MB22 = 22, + MB23 = 23, + MB24 = 24, + MB25 = 25, + MB26 = 26, + MB27 = 27, + MB28 = 28, + MB29 = 29, + MB30 = 30, + MB31 = 31, + MB32 = 32, + MB33 = 33, + MB34 = 34, + MB35 = 35, + MB36 = 36, + MB37 = 37, + MB38 = 38, + MB39 = 39, + MB40 = 40, + MB41 = 41, + MB42 = 42, + MB43 = 43, + MB44 = 44, + MB45 = 45, + MB46 = 46, + MB47 = 47, + MB48 = 48, + MB49 = 49, + MB50 = 50, + MB51 = 51, + MB52 = 52, + MB53 = 53, + MB54 = 54, + MB55 = 55, + MB56 = 56, + MB57 = 57, + MB58 = 58, + MB59 = 59, + MB60 = 60, + MB61 = 61, + MB62 = 62, + MB63 = 63, + FIFO = 99 +} FLEXCAN_MAILBOX; + +typedef enum FLEXCAN_RXTX +{ + TX, + RX, + LISTEN_ONLY +} FLEXCAN_RXTX; + +typedef enum FLEXCAN_FDRATES +{ + CAN_1M_2M, + CAN_1M_4M, + CAN_1M_6M, + CAN_1M_8M +} FLEXCAN_FDRATES; + +typedef enum FLEXCAN_CLOCK +{ + CLK_OFF, + CLK_8MHz = 8, + CLK_16MHz = 16, + CLK_20MHz = 20, + CLK_24MHz = 24, + CLK_30MHz = 30, + CLK_40MHz = 40, + CLK_60MHz = 60, + CLK_80MHz = 80 +} FLEXCAN_CLOCK; + +typedef struct CANFD_timings_t +{ + double baudrate = 1000000; + double baudrateFD = 2000000; + double propdelay = 190; + double bus_length = 1; + double sample = 75; + FLEXCAN_CLOCK clock = CLK_24MHz; +} CANFD_timings_t; + +typedef enum FLEXCAN_IDE +{ + NONE = 0, + EXT = 1, + RTR = 2, + STD = 3, + INACTIVE +} FLEXCAN_IDE; + +typedef enum FLEXCAN_FLTEN +{ + ACCEPT_ALL = 0, + REJECT_ALL = 1 +} FLEXCAN_FLTEN; + +typedef enum FLEXCAN_FILTER_TABLE +{ + FLEXCAN_MULTI = 1, + FLEXCAN_RANGE = 2, + FLEXCAN_TABLE_B_MULTI = 3, + FLEXCAN_TABLE_B_RANGE = 4, + FLEXCAN_USERMASK = 5 +} FLEXCAN_FILTER_TABLE; + +typedef enum FLEXCAN_FIFOTABLE +{ + A = 0, + B = 1, + C = 2 +} FLEXCAN_FIFOTABLE; + +typedef enum FLEXCAN_RXQUEUE_TABLE +{ + RX_SIZE_2 = (uint16_t)2, + RX_SIZE_4 = (uint16_t)4, + RX_SIZE_8 = (uint16_t)8, + RX_SIZE_16 = (uint16_t)16, + RX_SIZE_32 = (uint16_t)32, + RX_SIZE_64 = (uint16_t)64, + RX_SIZE_128 = (uint16_t)128, + RX_SIZE_256 = (uint16_t)256, + RX_SIZE_512 = (uint16_t)512, + RX_SIZE_1024 = (uint16_t)1024 +} FLEXCAN_RXQUEUE_TABLE; + +typedef enum FLEXCAN_DLC_SIZE +{ + DLC_SIZE_8 = (uint16_t)8, + DLC_SIZE_12 = (uint16_t)12, + DLC_SIZE_16 = (uint16_t)16, + DLC_SIZE_20 = (uint16_t)20, + DLC_SIZE_24 = (uint16_t)24, + DLC_SIZE_32 = (uint16_t)32, + DLC_SIZE_48 = (uint16_t)48, + DLC_SIZE_64 = (uint16_t)64 +} FLEXCAN_DLC_SIZE; + +typedef enum FLEXCAN_RFFN_TABLE +{ + RFFN_8 = (uint8_t)0, + RFFN_16 = (uint8_t)1, + RFFN_24 = (uint8_t)2, + RFFN_32 = (uint8_t)3, + RFFN_40 = (uint8_t)4, + RFFN_48 = (uint8_t)5, + RFFN_56 = (uint8_t)6, + RFFN_64 = (uint8_t)7, + RFFN_72 = (uint8_t)8, + RFFN_80 = (uint8_t)9, + RFFN_88 = (uint8_t)10, + RFFN_96 = (uint8_t)11, + RFFN_104 = (uint8_t)12, + RFFN_112 = (uint8_t)13, + RFFN_120 = (uint8_t)14, + RFFN_128 = (uint8_t)15 +} FLEXCAN_RFFN_TABLE; + +typedef enum FLEXCAN_TXQUEUE_TABLE +{ + TX_SIZE_2 = (uint16_t)2, + TX_SIZE_4 = (uint16_t)4, + TX_SIZE_8 = (uint16_t)8, + TX_SIZE_16 = (uint16_t)16, + TX_SIZE_32 = (uint16_t)32, + TX_SIZE_64 = (uint16_t)64, + TX_SIZE_128 = (uint16_t)128, + TX_SIZE_256 = (uint16_t)256, + TX_SIZE_512 = (uint16_t)512, + TX_SIZE_1024 = (uint16_t)1024 +} FLEXCAN_TXQUEUE_TABLE; + +typedef enum CAN_DEV_TABLE +{ +#if defined(__IMXRT1062__) + CAN0 = (uint32_t)0x0, + CAN1 = (uint32_t)0x401D0000, + CAN2 = (uint32_t)0x401D4000, + CAN3 = (uint32_t)0x401D8000 +#endif +#if defined(__MK20DX256__) || defined(__MK64FX512__) || defined(__MK66FX1M0__) + CAN0 = (uint32_t)0x40024000, + CAN1 = (uint32_t)0x400A4000, + CAN2 = (uint32_t)0x0, + CAN3 = (uint32_t)0x0 +#endif +} CAN_DEV_TABLE; + +#define FCTP_CLASS template +#define FCTP_FUNC template +#define FCTP_OPT FlexCAN_T4<_bus, _rxSize, _txSize> + +#define FCTPFD_CLASS template +#define FCTPFD_FUNC template +#define FCTPFD_OPT FlexCAN_T4FD<_bus, _rxSize, _txSize> + +#define SIZE_LISTENERS 4 + +class CANListener +{ +public: + CANListener() + { + callbacksActive = 0; + } + virtual bool frameHandler(CAN_message_t &frame, int mailbox, uint8_t controller) + { + return false; + } + virtual void txHandler(int mailbox, uint8_t controller) + { + ; + } /* unused in Collin's library */ + void attachMBHandler(uint64_t mailBox) + { + callbacksActive |= (1UL << mailBox); + } + void detachMBHandler(uint64_t mailBox) + { + callbacksActive &= ~(1UL << mailBox); + } + void attachGeneralHandler(void) + { + generalCallbackActive = 1; + } + void detachGeneralHandler(void) + { + generalCallbackActive = 0; + } + uint64_t callbacksActive; // bitfield indicating which callbacks are installed (for object oriented callbacks only) + bool generalCallbackActive = 0; +}; + +class FlexCAN_T4_Base +{ +public: + virtual void flexcan_interrupt() = 0; + virtual void setBaudRate(uint32_t baud = 1000000, FLEXCAN_RXTX listen_only = TX) = 0; + virtual uint64_t events() = 0; + virtual int write(const CANFD_message_t &msg) = 0; + virtual int write(const CAN_message_t &msg) = 0; + virtual bool isFD() = 0; + virtual uint8_t getFirstTxBoxSize() = 0; +}; + +#if defined(__IMXRT1062__) +static FlexCAN_T4_Base *_CAN1 = nullptr; +static FlexCAN_T4_Base *_CAN2 = nullptr; +static FlexCAN_T4_Base *_CAN3 = nullptr; +#endif +#if defined(__MK20DX256__) || defined(__MK64FX512__) || defined(__MK66FX1M0__) +static FlexCAN_T4_Base *_CAN0 = nullptr; +static FlexCAN_T4_Base *_CAN1 = nullptr; +#endif + +extern void ext_outputFD1(const CANFD_message_t &msg); // Interrupt data output, not filtered, for external libraries +extern void ext_outputFD2(const CANFD_message_t &msg); +extern void ext_outputFD3(const CANFD_message_t &msg); + +extern void ext_output1(const CAN_message_t &msg); // Interrupt data output, not filtered, for external libraries +extern void ext_output2(const CAN_message_t &msg); +extern void ext_output3(const CAN_message_t &msg); + +FCTPFD_CLASS class FlexCAN_T4FD : public FlexCAN_T4_Base +{ +public: + FlexCAN_T4FD(); + bool isFD() + { + return 1; + } + void begin(); + void setTX(FLEXCAN_PINS pin = DEF); + void setRX(FLEXCAN_PINS pin = DEF); + void enableFIFO(bool status = 1); + void disableFIFO() + { + enableFIFO(0); + } + int read(CANFD_message_t &msg); + int readMB(CANFD_message_t &msg); + int write(const CANFD_message_t &msg); /* use any available mailbox for transmitting */ + int write(const CAN_message_t &msg) + { + return 0; + } /* to satisfy base class for external pointers */ + int write(FLEXCAN_MAILBOX mb_num, const CANFD_message_t &msg); /* use a single mailbox for transmitting */ + bool setMB(const FLEXCAN_MAILBOX &mb_num, const FLEXCAN_RXTX &mb_rx_tx, const FLEXCAN_IDE &ide = STD); + uint8_t setRegions(uint8_t size); /* 8, 16, 32 or 64 bytes */ + uint8_t setRegions(uint8_t mbdsr0, uint8_t mbdsr1); /* set both regions independantly */ + bool setBaudRateAdvanced(CANFD_timings_t config, uint8_t nominal_choice, uint8_t flexdata_choice, FLEXCAN_RXTX listen_only = TX); + bool setBaudRate(CANFD_timings_t config, FLEXCAN_RXTX listen_only = TX); + void setBaudRate(FLEXCAN_FDRATES input, FLEXCAN_RXTX listen_only = TX); + void enableMBInterrupt(const FLEXCAN_MAILBOX &mb_num, bool status = 1); + void disableMBInterrupt(const FLEXCAN_MAILBOX &mb_num) + { + enableMBInterrupt(mb_num, 0); + } + void enableMBInterrupts(bool status = 1); + void disableMBInterrupts() + { + enableMBInterrupts(0); + } + uint64_t events(); + void onReceive(const FLEXCAN_MAILBOX &mb_num, _MBFD_ptr handler); /* individual mailbox callback function */ + void onReceive(_MBFD_ptr handler); /* global callback function */ + void setMBFilter(FLEXCAN_FLTEN input); /* enable/disable traffic for all MBs (for individual masking) */ + void setMBFilter(FLEXCAN_MAILBOX mb_num, FLEXCAN_FLTEN input); /* set specific MB to accept/deny traffic */ + bool setMBFilter(FLEXCAN_MAILBOX mb_num, uint32_t id1); /* input 1 ID to be filtered */ + bool setMBFilter(FLEXCAN_MAILBOX mb_num, uint32_t id1, uint32_t id2); /* input 2 ID's to be filtered */ + bool setMBFilter(FLEXCAN_MAILBOX mb_num, uint32_t id1, uint32_t id2, uint32_t id3); /* input 3 ID's to be filtered */ + bool setMBFilter(FLEXCAN_MAILBOX mb_num, uint32_t id1, uint32_t id2, uint32_t id3, uint32_t id4); /* input 4 ID's to be filtered */ + bool setMBFilter(FLEXCAN_MAILBOX mb_num, uint32_t id1, uint32_t id2, uint32_t id3, uint32_t id4, uint32_t id5); /* input 5 ID's to be filtered */ + bool setMBFilterRange(FLEXCAN_MAILBOX mb_num, uint32_t id1, uint32_t id2); /* filter a range of ids */ + void mailboxStatus(); + void enhanceFilter(FLEXCAN_MAILBOX mb_num); + void distribute(bool state = 1) + { + distribution = state; + } + void enableDMA(bool state = 1); + void disableDMA() + { + enableDMA(0); + } + uint8_t getFirstTxBoxSize(); + void setMBFilterProcessing(FLEXCAN_MAILBOX mb_num, uint32_t filter_id, uint32_t calculated_mask); + +private: + volatile bool isEventsUsed = 0; + uint64_t readIFLAG() + { + return (((uint64_t)FLEXCANb_IFLAG2(_bus) << 32) | FLEXCANb_IFLAG1(_bus)); + } + uint32_t mailbox_offset(uint8_t mailbox, uint8_t &maxsize); + void writeTxMailbox(uint8_t mb_num, const CANFD_message_t &msg); + bool setBaudRate(CANFD_timings_t config, uint8_t nominal_choice, uint8_t flexdata_choice, FLEXCAN_RXTX listen_only = TX, bool advanced = 0); + int getFirstTxBox(); + uint32_t mb_filter_table[64][7]; + Circular_Buffer rxBuffer; + Circular_Buffer txBuffer; + void FLEXCAN_ExitFreezeMode(); + void FLEXCAN_EnterFreezeMode(); + void reset() + { + softReset(); + } /* reset flexcan controller (needs register restore capabilities...) */ + void flexcan_interrupt(); + void writeIFLAG(uint64_t value); + void writeIFLAGBit(uint8_t mb_num); + uint8_t max_mailboxes(); + uint64_t readIMASK() + { + return (((uint64_t)FLEXCANb_IMASK2(_bus) << 32) | FLEXCANb_IMASK1(_bus)); + } + void frame_distribution(CANFD_message_t &msg); + void setBaudRate(uint32_t baud = 1000000, FLEXCAN_RXTX listen_only = TX) + { + ; + } // unused, CAN2.0 only (needed for base class existance) + void setClock(FLEXCAN_CLOCK clock = CLK_24MHz); + uint32_t getClock(); + void softReset(); + void writeIMASK(uint64_t value); + void writeIMASKBit(uint8_t mb_num, bool set = 1); + uint8_t busNumber; + uint8_t mailbox_reader_increment = 0; + uint32_t nvicIrq = 0; + uint8_t dlc_to_len(uint8_t val); + uint8_t len_to_dlc(uint8_t val); + uint32_t setBaudRateFD(CANFD_timings_t config, uint32_t flexdata_choice, bool advanced); /* internally used */ + void mbCallbacks(const FLEXCAN_MAILBOX &mb_num, const CANFD_message_t &msg); + _MBFD_ptr _mbHandlers[64]; /* individual mailbox handlers */ + _MBFD_ptr _mainHandler; /* global mailbox handler */ + void filter_store(FLEXCAN_FILTER_TABLE type, FLEXCAN_MAILBOX mb_num, uint32_t id_count, uint32_t id1, uint32_t id2, uint32_t id3, uint32_t id4, uint32_t id5, uint32_t mask); + bool filter_match(FLEXCAN_MAILBOX mb_num, uint32_t id); + void struct2queueTx(const CANFD_message_t &msg); + void struct2queueRx(const CANFD_message_t &msg); + bool distribution = 0; +}; + +FCTP_CLASS class FlexCAN_T4 : public FlexCAN_T4_Base +{ +public: + FlexCAN_T4(); + CANListener *listener[SIZE_LISTENERS]; + bool attachObj(CANListener *listener); + bool detachObj(CANListener *listener); + bool isFD() + { + return 0; + } + void begin(); + uint32_t getBaudRate() + { + return currentBitrate; + } + void setTX(FLEXCAN_PINS pin = DEF); + void setRX(FLEXCAN_PINS pin = DEF); + void setBaudRate(uint32_t baud = 1000000, FLEXCAN_RXTX listen_only = TX); + void reset() + { + softReset(); + } /* reset flexcan controller (needs register restore capabilities...) */ + void setMaxMB(uint8_t last); + void enableLoopBack(bool yes = 1); + void enableFIFO(bool status = 1); + void disableFIFO() + { + enableFIFO(0); + } + void enableFIFOInterrupt(bool status = 1); + void disableFIFOInterrupt() + { + enableFIFOInterrupt(0); + } + void mailboxStatus(); + int read(CAN_message_t &msg); + int readMB(CAN_message_t &msg); + int readFIFO(CAN_message_t &msg); + bool setMB(const FLEXCAN_MAILBOX &mb_num, const FLEXCAN_RXTX &mb_rx_tx, const FLEXCAN_IDE &ide = STD); + void enableMBInterrupt(const FLEXCAN_MAILBOX &mb_num, bool status = 1); + void disableMBInterrupt(const FLEXCAN_MAILBOX &mb_num) + { + enableMBInterrupt(mb_num, 0); + } + void enableMBInterrupts(bool status = 1); + void disableMBInterrupts() + { + enableMBInterrupts(0); + } + void setMRP(bool mrp = 1); /* mailbox(1)/fifo(0) priority */ + void setRRS(bool rrs = 1); /* store remote frames */ + void onReceive(const FLEXCAN_MAILBOX &mb_num, _MB_ptr handler); /* individual mailbox callback function */ + void onReceive(_MB_ptr handler); /* global callback function */ + void onTransmit(const FLEXCAN_MAILBOX &mb_num, _MB_ptr handler); /* individual mailbox callback function */ + void onTransmit(_MB_ptr handler); /* global callback function */ + bool setMBUserFilter(FLEXCAN_MAILBOX mb_num, uint32_t id1, uint32_t mask); + bool setMBUserFilter(FLEXCAN_MAILBOX mb_num, uint32_t id1, uint32_t id2, uint32_t mask); + bool setMBUserFilter(FLEXCAN_MAILBOX mb_num, uint32_t id1, uint32_t id2, uint32_t id3, uint32_t mask); + bool setMBUserFilter(FLEXCAN_MAILBOX mb_num, uint32_t id1, uint32_t id2, uint32_t id3, uint32_t id4, uint32_t mask); + bool setMBManualFilter(FLEXCAN_MAILBOX mb_num, uint32_t id1, uint32_t mask); + void setMBFilter(FLEXCAN_FLTEN input); /* enable/disable traffic for all MBs (for individual masking) */ + void setMBFilter(FLEXCAN_MAILBOX mb_num, FLEXCAN_FLTEN input); /* set specific MB to accept/deny traffic */ + bool setMBFilter(FLEXCAN_MAILBOX mb_num, uint32_t id1); /* input 1 ID to be filtered */ + bool setMBFilter(FLEXCAN_MAILBOX mb_num, uint32_t id1, uint32_t id2); /* input 2 ID's to be filtered */ + bool setMBFilter(FLEXCAN_MAILBOX mb_num, uint32_t id1, uint32_t id2, uint32_t id3); /* input 3 ID's to be filtered */ + bool setMBFilter(FLEXCAN_MAILBOX mb_num, uint32_t id1, uint32_t id2, uint32_t id3, uint32_t id4); /* input 4 ID's to be filtered */ + bool setMBFilter(FLEXCAN_MAILBOX mb_num, uint32_t id1, uint32_t id2, uint32_t id3, uint32_t id4, uint32_t id5); /* input 5 ID's to be filtered */ + bool setMBFilterRange(FLEXCAN_MAILBOX mb_num, uint32_t id1, uint32_t id2); /* filter a range of ids */ + int write(const CAN_message_t &msg); /* use any available mailbox for transmitting */ + int write(const CANFD_message_t &msg) + { + return 0; + } /* to satisfy base class for external pointers */ + int write(FLEXCAN_MAILBOX mb_num, const CAN_message_t &msg); /* use a single mailbox for transmitting */ + uint64_t events(); + uint8_t setRFFN(FLEXCAN_RFFN_TABLE rffn = RFFN_8); /* Number Of Rx FIFO Filters (0 == 8 filters, 1 == 16 filters, etc.. */ + uint8_t setRFFN(uint8_t rffn) + { + return setRFFN((FLEXCAN_RFFN_TABLE)constrain(rffn, 0, 15)); + } + void setFIFOFilterTable(FLEXCAN_FIFOTABLE letter); + bool setFIFOUserFilter(uint8_t filter, uint32_t id1, uint32_t mask, const FLEXCAN_IDE &ide, const FLEXCAN_IDE &remote = NONE); + bool setFIFOUserFilter(uint8_t filter, uint32_t id1, uint32_t id2, uint32_t mask, const FLEXCAN_IDE &ide, const FLEXCAN_IDE &remote = NONE); + bool setFIFOUserFilter(uint8_t filter, uint32_t id1, uint32_t id2, uint32_t id3, uint32_t mask, const FLEXCAN_IDE &ide, const FLEXCAN_IDE &remote = NONE); + bool setFIFOUserFilter(uint8_t filter, uint32_t id1, uint32_t id2, uint32_t id3, uint32_t id4, uint32_t mask, const FLEXCAN_IDE &ide, const FLEXCAN_IDE &remote = NONE); + void setFIFOFilter(const FLEXCAN_FLTEN &input); + bool setFIFOManualFilter(uint8_t filter, uint32_t id1, uint32_t mask, const FLEXCAN_IDE &ide, const FLEXCAN_IDE &remote = NONE); + bool setFIFOFilter(uint8_t filter, uint32_t id1, const FLEXCAN_IDE &ide, const FLEXCAN_IDE &remote = NONE); /* single ID per filter */ + bool setFIFOFilter(uint8_t filter, uint32_t id1, uint32_t id2, const FLEXCAN_IDE &ide, const FLEXCAN_IDE &remote = NONE); /* 2 ID's per filter */ + bool setFIFOFilterRange(uint8_t filter, uint32_t id1, uint32_t id2, const FLEXCAN_IDE &ide, const FLEXCAN_IDE &remote = NONE); /* ID range per filter */ + bool setFIFOFilter(uint8_t filter, uint32_t id1, const FLEXCAN_IDE &ide1, const FLEXCAN_IDE &remote1, uint32_t id2, const FLEXCAN_IDE &ide2, const FLEXCAN_IDE &remote2); /* TableB 2 ID / filter */ + bool setFIFOFilter(uint8_t filter, uint32_t id1, uint32_t id2, const FLEXCAN_IDE &ide1, const FLEXCAN_IDE &remote1, uint32_t id3, uint32_t id4, const FLEXCAN_IDE &ide2, const FLEXCAN_IDE &remote2); /* TableB 4 minimum ID / filter */ + bool setFIFOFilterRange(uint8_t filter, uint32_t id1, uint32_t id2, const FLEXCAN_IDE &ide1, const FLEXCAN_IDE &remote1, uint32_t id3, uint32_t id4, const FLEXCAN_IDE &ide2, const FLEXCAN_IDE &remote2); /* TableB dual range based IDs */ + bool struct2queueTx(const CAN_message_t &msg); + void struct2queueRx(const CAN_message_t &msg); +#if defined(__IMXRT1062__) + void setClock(FLEXCAN_CLOCK clock = CLK_24MHz); +#endif + void enhanceFilter(FLEXCAN_MAILBOX mb_num); + void distribute(bool state = 1) + { + distribution = state; + } + void enableDMA(bool state = 1); + void disableDMA() + { + enableDMA(0); + } + uint8_t getFirstTxBoxSize() + { + return 8; + } + void FLEXCAN_ExitFreezeMode(); + void FLEXCAN_EnterFreezeMode(); + bool error(CAN_error_t &error, bool printDetails); + uint32_t getRXQueueCount() + { + return rxBuffer.size(); + } + uint32_t getTXQueueCount() + { + return txBuffer.size(); + } + +private: + void setMBFilterProcessing(FLEXCAN_MAILBOX mb_num, uint32_t filter_id, uint32_t calculated_mask); + void writeTxMailbox(uint8_t mb_num, const CAN_message_t &msg); + uint64_t readIMASK(); // { return (((uint64_t)FLEXCANb_IMASK2(_bus) << 32) | FLEXCANb_IMASK1(_bus)); } + void flexcan_interrupt(); + void flexcanFD_interrupt() + { + ; + } // dummy placeholder to satisfy base class + Circular_Buffer rxBuffer; + Circular_Buffer txBuffer; + Circular_Buffer busESR1; + Circular_Buffer busECR; + void printErrors(const CAN_error_t &error); +#if defined(__IMXRT1062__) + uint32_t getClock(); +#endif + volatile uint32_t fifo_filter_table[32][6]; + volatile uint32_t mb_filter_table[64][6]; + volatile bool fifo_filter_match(uint32_t id); + volatile bool isEventsUsed = 0; + volatile void frame_distribution(CAN_message_t &msg); + void filter_store(FLEXCAN_FILTER_TABLE type, FLEXCAN_MAILBOX mb_num, uint32_t id_count, uint32_t id1, uint32_t id2, uint32_t id3, uint32_t id4, uint32_t id5); + void fifo_filter_store(FLEXCAN_FILTER_TABLE type, uint8_t filter, uint32_t id_count, uint32_t id1, uint32_t id2, uint32_t id3, uint32_t id4, uint32_t id5); + volatile bool filter_match(FLEXCAN_MAILBOX mb_num, uint32_t id); + volatile bool distribution = 0; + uint8_t getNumMailBoxes() + { + return FLEXCANb_MAXMB_SIZE(_bus); + } + uint8_t mailboxOffset(); + void softReset(); + int getFirstTxBox(); + _MB_ptr _mbHandlers[64]; /* individual mailbox handlers */ + _MB_ptr _mainHandler; /* global mailbox handler */ + _MB_ptr _mbTxHandlers[64]; /* individual mailbox tx handlers */ + _MB_ptr _mainTxHandler; /* global mailbox handler */ + uint64_t readIFLAG(); // { return (((uint64_t)FLEXCANb_IFLAG2(_bus) << 32) | FLEXCANb_IFLAG1(_bus)); } + void writeIFLAG(uint64_t value); + void writeIFLAGBit(uint8_t mb_num); + void writeIMASK(uint64_t value); + void writeIMASKBit(uint8_t mb_num, bool set = 1); + uint32_t nvicIrq = 0; + uint32_t currentBitrate = 0UL; + uint8_t mailbox_reader_increment = 0; + uint8_t busNumber; + void mbCallbacks(const FLEXCAN_MAILBOX &mb_num, const CAN_message_t &msg); +}; + +#include "FlexCAN_T4.tpp" + +#if defined(__IMXRT1062__) +#include "FlexCAN_T4FD.tpp" +#include "FlexCAN_T4FDTimings.tpp" +#endif + +#endif diff --git a/src/FlexCAN_T4.tpp b/src/FlexCAN_T4.tpp new file mode 100644 index 0000000..83b2e5d --- /dev/null +++ b/src/FlexCAN_T4.tpp @@ -0,0 +1,1807 @@ +/* + MIT License + + Copyright (c) 2018 Antonio Alexander Brewer (tonton81) - https://github.com/tonton81 + + Designed and tested for PJRC Teensy 4.0. + + Forum link : https://forum.pjrc.com/threads/56035-FlexCAN_T4-FlexCAN-for-Teensy-4?highlight=flexcan_t4 + + Thanks goes to skpang, mjs513, and collin for tech/testing support + + Permission is hereby granted, free of charge, to any person obtaining a copy + of this software and associated documentation files (the "Software"), to deal + in the Software without restriction, including without limitation the rights + to use, copy, modify, merge, publish, distribute, sublicense, and / or sell + copies of the Software, and to permit persons to whom the Software is + furnished to do so, subject to the following conditions: + + The above copyright notice and this permission notice shall be included in all + copies or substantial portions of the Software. + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + SOFTWARE. +*/ +#pragma once +#include +#include "imxrt_flexcan.hpp" +#include "Arduino.h" + +#if defined(__IMXRT1062__) +static void flexcan_isr_can3(); +static void flexcan_isr_can2(); +static void flexcan_isr_can1(); +#endif +#if defined(__MK20DX256__) || defined(__MK64FX512__) +static void flexcan_isr_can0(); +#endif +#if defined(__MK66FX1M0__) +static void flexcan_isr_can0(); +static void flexcan_isr_can1(); +#endif + +FCTP_FUNC FCTP_OPT::FlexCAN_T4() { +#if defined(__IMXRT1062__) + if ( _bus == CAN3 ) _CAN3 = this; + if ( _bus == CAN2 ) _CAN2 = this; + if ( _bus == CAN1 ) _CAN1 = this; +#endif +#if defined(__MK20DX256__) || defined(__MK64FX512__) || defined(__MK66FX1M0__) + if ( _bus == CAN1 ) _CAN1 = this; + if ( _bus == CAN0 ) _CAN0 = this; +#endif + +#if defined(__MK20DX256__) || defined(__MK64FX512__) + static_assert(_bus == CAN0, "Only CAN0 works on Teensy 3.2/3.5"); +#endif +#if defined(__MK66FX1M0__) + static_assert(_bus == CAN0 || _bus == CAN1, "Only CAN0 & CAN1 works on Teensy 3.6"); +#endif +#if defined(__IMXRT1062__) + static_assert(_bus == CAN1 || _bus == CAN2 || _bus == CAN3, "Only CAN1 & CAN2 & CAN3 works on Teensy 4.0/4.1"); +#endif + +} + +#if defined(__IMXRT1062__) +FCTP_FUNC void FCTP_OPT::setClock(FLEXCAN_CLOCK clock) { + if ( clock == CLK_OFF ) CCM_CSCMR2 = (CCM_CSCMR2 & 0xFFFFFC03) | CCM_CSCMR2_CAN_CLK_SEL(3) | CCM_CSCMR2_CAN_CLK_PODF(0); + if ( clock == CLK_8MHz ) CCM_CSCMR2 = (CCM_CSCMR2 & 0xFFFFFC03) | CCM_CSCMR2_CAN_CLK_SEL(2) | CCM_CSCMR2_CAN_CLK_PODF(9); + if ( clock == CLK_16MHz ) CCM_CSCMR2 = (CCM_CSCMR2 & 0xFFFFFC03) | CCM_CSCMR2_CAN_CLK_SEL(2) | CCM_CSCMR2_CAN_CLK_PODF(4); + if ( clock == CLK_24MHz ) CCM_CSCMR2 = (CCM_CSCMR2 & 0xFFFFFC03) | CCM_CSCMR2_CAN_CLK_SEL(1) | CCM_CSCMR2_CAN_CLK_PODF(0); + if ( clock == CLK_20MHz ) CCM_CSCMR2 = (CCM_CSCMR2 & 0xFFFFFC03) | CCM_CSCMR2_CAN_CLK_SEL(2) | CCM_CSCMR2_CAN_CLK_PODF(3); + if ( clock == CLK_30MHz ) CCM_CSCMR2 = (CCM_CSCMR2 & 0xFFFFFC03) | CCM_CSCMR2_CAN_CLK_SEL(0) | CCM_CSCMR2_CAN_CLK_PODF(1); + if ( clock == CLK_40MHz ) CCM_CSCMR2 = (CCM_CSCMR2 & 0xFFFFFC03) | CCM_CSCMR2_CAN_CLK_SEL(2) | CCM_CSCMR2_CAN_CLK_PODF(1); + if ( clock == CLK_60MHz ) CCM_CSCMR2 = (CCM_CSCMR2 & 0xFFFFFC03) | CCM_CSCMR2_CAN_CLK_SEL(0) | CCM_CSCMR2_CAN_CLK_PODF(0); + if ( clock == CLK_80MHz ) CCM_CSCMR2 = (CCM_CSCMR2 & 0xFFFFFC03) | CCM_CSCMR2_CAN_CLK_SEL(2) | CCM_CSCMR2_CAN_CLK_PODF(0); + + if ( _CAN1 ) _CAN1->setBaudRate(currentBitrate, (( FLEXCANb_CTRL1(_bus) & FLEXCAN_CTRL_LOM ) ? LISTEN_ONLY : TX)); + if ( _CAN2 ) _CAN2->setBaudRate(currentBitrate, (( FLEXCANb_CTRL1(_bus) & FLEXCAN_CTRL_LOM ) ? LISTEN_ONLY : TX)); + if ( _CAN3 ) _CAN3->setBaudRate(currentBitrate, (( FLEXCANb_CTRL1(_bus) & FLEXCAN_CTRL_LOM ) ? LISTEN_ONLY : TX)); +} + +FCTP_FUNC uint32_t FCTP_OPT::getClock() { + const uint8_t clocksrc[4] = {60, 24, 80, 0}; + return clocksrc[(CCM_CSCMR2 & 0x300) >> 8]; +} +#endif + +FCTP_FUNC void FCTP_OPT::begin() { + + for (uint8_t i = 0; i < SIZE_LISTENERS; i++) listener[i] = nullptr; + +#if defined(__IMXRT1062__) + if ( !getClock() ) setClock(CLK_24MHz); /* no clock enabled, enable osc clock */ + + if ( _bus == CAN3 ) { + nvicIrq = IRQ_CAN3; + _VectorsRam[16 + nvicIrq] = flexcan_isr_can3; + CCM_CCGR7 |= 0x3C0; + busNumber = 3; + } + if ( _bus == CAN2 ) { + nvicIrq = IRQ_CAN2; + _VectorsRam[16 + nvicIrq] = flexcan_isr_can2; + CCM_CCGR0 |= 0x3C0000; + busNumber = 2; + } + if ( _bus == CAN1 ) { + nvicIrq = IRQ_CAN1; + _VectorsRam[16 + nvicIrq] = flexcan_isr_can1; + CCM_CCGR0 |= 0x3C000; + busNumber = 1; + } +#endif + + + +#if defined(__MK20DX256__) + if ( _bus == CAN0 ) { + nvicIrq = IRQ_CAN_MESSAGE; + _VectorsRam[16 + nvicIrq] = flexcan_isr_can0; + busNumber = 0; + } +#endif + +#if defined(__MK64FX512__) || defined(__MK66FX1M0__) + if ( _bus == CAN0 ) { + nvicIrq = IRQ_CAN0_MESSAGE; + _VectorsRam[16 + nvicIrq] = flexcan_isr_can0; + busNumber = 0; + } +#endif + +#if defined(__MK66FX1M0__) + else if ( _bus == CAN1 ) { + nvicIrq = IRQ_CAN1_MESSAGE; + _VectorsRam[16 + nvicIrq] = flexcan_isr_can1; + busNumber = 1; + } +#endif + +#if defined(__MK20DX256__) || defined(__MK64FX512__) || defined(__MK66FX1M0__) + OSC0_CR |= OSC_ERCLKEN; + if ( _bus == CAN0 ) SIM_SCGC6 |= SIM_SCGC6_FLEXCAN0; +#if defined(__MK66FX1M0__) + else if ( _bus == CAN1 ) SIM_SCGC3 |= SIM_SCGC3_FLEXCAN1; +#endif + FLEXCANb_CTRL1(_bus) &= ~FLEXCAN_CTRL_CLK_SRC; +#endif + + setTX(); setRX(); + + FLEXCANb_MCR(_bus) &= ~FLEXCAN_MCR_MDIS; /* enable module */ + FLEXCAN_EnterFreezeMode(); + FLEXCANb_CTRL1(_bus) |= FLEXCAN_CTRL_LOM; /* listen only mode */ + FLEXCANb_MCR(_bus) |= FLEXCAN_MCR_FRZ; /* enable freeze bit */ + while (FLEXCANb_MCR(_bus) & FLEXCAN_MCR_LPM_ACK); + softReset(); /* reset bus */ + while (!(FLEXCANb_MCR(_bus) & FLEXCAN_MCR_FRZ_ACK)); + FLEXCANb_MCR(_bus) |= FLEXCAN_MCR_SRX_DIS; /* Disable self-reception */ + FLEXCANb_MCR(_bus) |= FLEXCAN_MCR_IRMQ; // individual mailbox masking + FLEXCANb_MCR(_bus) |= FLEXCAN_MCR_AEN; // TX ABORT FEATURE + FLEXCANb_MCR(_bus) |= FLEXCAN_MCR_LPRIO_EN; // TX PRIORITY FEATURE + FLEXCANb_MCR(_bus) |= FLEXCAN_MCR_SLF_WAK; // SELF-WAKE UP FEATURE + FLEXCANb_MCR(_bus) |= FLEXCAN_MCR_WAK_SRC; // WAKE-UP LOW-PASS FILTER + FLEXCANb_MCR(_bus) &= ~0x8800; // disable DMA and FD (valid bits are reserved in legacy controllers) + FLEXCANb_CTRL2(_bus) |= FLEXCAN_CTRL2_RRS | // store remote frames + FLEXCAN_CTRL2_EACEN | /* handles the way filtering works. Library adjusts to whether you use this or not */ + FLEXCAN_CTRL2_MRP; // mailbox > FIFO priority. + FLEXCANb_MCR(_bus) |= FLEXCAN_MCR_WRN_EN; + FLEXCANb_MCR(_bus) |= FLEXCAN_MCR_WAK_MSK; + + disableFIFO(); /* clears all data and layout to legacy mailbox mode */ + FLEXCAN_ExitFreezeMode(); + NVIC_ENABLE_IRQ(nvicIrq); +} + +FCTP_FUNC void FCTP_OPT::enableFIFO(bool status) { + bool frz_flag_negate = !(FLEXCANb_MCR(_bus) & FLEXCAN_MCR_FRZ_ACK); + FLEXCAN_EnterFreezeMode(); + FLEXCANb_MCR(_bus) &= ~FLEXCAN_MCR_FEN; // Disable FIFO if already enabled for cleanup. + writeIMASK(0ULL); // disable all FIFO/MB Interrupts + + for (uint8_t i = 0; i < FLEXCANb_MAXMB_SIZE(_bus); i++ ) { // clear all mailboxes + volatile uint32_t *mbxAddr = &(*(volatile uint32_t*)(_bus + 0x80 + (i * 0x10))); + mbxAddr[0] = mbxAddr[1] = mbxAddr[2] = mbxAddr[3] = 0; // code, id, word0, word1 + FLEXCANb_RXIMR(_bus, i) = 0UL; // CLEAR MAILBOX MASKS (RXIMR) + } + + FLEXCANb_RXMGMASK(_bus) = FLEXCANb_RXFGMASK(_bus) = 0; + writeIFLAG(readIFLAG()); // (all bits reset when written back) + + if ( status ) { + FLEXCANb_MCR(_bus) |= FLEXCAN_MCR_FEN; + for (uint8_t i = mailboxOffset(); i < FLEXCANb_MAXMB_SIZE(_bus); i++) { + FLEXCANb_MBn_CS(_bus,i) = FLEXCAN_MB_CS_CODE(FLEXCAN_MB_CODE_TX_INACTIVE); + enableMBInterrupt((FLEXCAN_MAILBOX)i); /* enable TX interrupt */ + } + } + else { // FIFO disabled default setup of mailboxes, 0-7 RX, 8-15 TX + for (uint8_t i = 0; i < FLEXCANb_MAXMB_SIZE(_bus); i++ ) { // clear all mailboxes + volatile uint32_t *mbxAddr = &(*(volatile uint32_t*)(_bus + 0x80 + (i * 0x10))); + if ( i < (FLEXCANb_MAXMB_SIZE(_bus) / 2) ) { + mbxAddr[0] = FLEXCAN_MB_CS_CODE(FLEXCAN_MB_CODE_RX_EMPTY) | ((i < (FLEXCANb_MAXMB_SIZE(_bus) / 4)) ? 0 : FLEXCAN_MB_CS_IDE | FLEXCAN_MB_CS_SRR); + FLEXCANb_RXIMR(_bus, i) = 0UL | ((FLEXCANb_CTRL2(_bus) & FLEXCAN_CTRL2_EACEN) ? (1UL << 30) : 0); // (RXIMR) + } + else { + mbxAddr[0] = FLEXCAN_MB_CS_CODE(FLEXCAN_MB_CODE_TX_INACTIVE); + enableMBInterrupt((FLEXCAN_MAILBOX)i); /* enable TX interrupt */ + } + } + } + if ( frz_flag_negate ) FLEXCAN_ExitFreezeMode(); +} + +FCTP_FUNC void FCTP_OPT::enableFIFOInterrupt(bool status) { + if ( !(FLEXCANb_MCR(_bus) & FLEXCAN_MCR_FEN) ) return; /* FIFO must be enabled first */ + if ( FLEXCANb_IMASK1(_bus) & FLEXCAN_IMASK1_BUF5M ) return; /* FIFO interrupts already enabled */ + FLEXCANb_IMASK1(_bus) &= ~0xFF; /* disable FIFO interrupt flags */ + if ( status ) FLEXCANb_IMASK1(_bus) |= FLEXCAN_IMASK1_BUF5M; /* enable FIFO interrupt */ +} + +FCTP_FUNC void FCTP_OPT::enableMBInterrupts(bool status) { + FLEXCAN_EnterFreezeMode(); + for ( uint8_t mb_num = mailboxOffset(); mb_num < FLEXCANb_MAXMB_SIZE(_bus); mb_num++ ) { + enableMBInterrupt((FLEXCAN_MAILBOX)mb_num, status); + } + FLEXCAN_ExitFreezeMode(); +} + +FCTP_FUNC void FCTP_OPT::enableMBInterrupt(const FLEXCAN_MAILBOX &mb_num, bool status) { + if ( mb_num < mailboxOffset() ) return; /* mailbox not available */ + if ( status ) writeIMASKBit(mb_num); /* enable mailbox interrupt */ + else if ( (FLEXCAN_get_code(FLEXCANb_MBn_CS(_bus, mb_num)) >> 3) ) writeIMASKBit(mb_num); /* transmit interrupt keeper */ + else writeIMASKBit(mb_num, 0); /* disable mailbox interrupt */ +} + +FCTP_FUNC bool FCTP_OPT::setMB(const FLEXCAN_MAILBOX &mb_num, const FLEXCAN_RXTX &mb_rx_tx, const FLEXCAN_IDE &ide) { + if ( mb_num < mailboxOffset() ) return 0; /* mailbox not available */ + writeIMASKBit(mb_num, 0); /* immediately disable mailbox interrupt */ + FLEXCAN_get_code(FLEXCANb_MBn_CS(_bus, mb_num)); // Reading Control Status atomically locks mailbox (if it is RX mode). + FLEXCANb_MBn_ID(_bus, mb_num) = 0UL; + FLEXCANb_MBn_WORD0(_bus, mb_num) = 0UL; + FLEXCANb_MBn_WORD1(_bus, mb_num) = 0UL; + if ( mb_rx_tx == RX ) { + if ( ide != EXT ) FLEXCANb_MBn_CS(_bus, mb_num) = FLEXCAN_MB_CS_CODE(FLEXCAN_MB_CODE_RX_EMPTY); + else FLEXCANb_MBn_CS(_bus, mb_num) = FLEXCAN_MB_CS_CODE(FLEXCAN_MB_CODE_RX_EMPTY) | FLEXCAN_MB_CS_SRR | FLEXCAN_MB_CS_IDE; + } + if ( mb_rx_tx == TX ) { + FLEXCANb_MBn_CS(_bus, mb_num) = FLEXCAN_MB_CS_CODE(FLEXCAN_MB_CODE_TX_INACTIVE); + writeIMASKBit(mb_num); /* keep transmit interrupt enabled */ + } + if ( ide == INACTIVE ) { + FLEXCANb_MBn_CS(_bus, mb_num) = FLEXCAN_MB_CS_CODE(FLEXCAN_MB_CODE_RX_INACTIVE); + } + (void)FLEXCANb_TIMER(_bus); + writeIFLAGBit(mb_num); /* clear mailbox reception flag */ + mb_filter_table[mb_num][0] = ( ((FLEXCANb_MBn_CS(_bus, mb_num) & 0x600000) ? 1UL : 0UL) << 27); /* extended flag check */ + return 1; +} + +FCTP_FUNC void FCTP_OPT::mailboxStatus() { + if ( FLEXCANb_MCR(_bus) & FLEXCAN_MCR_FEN ) { + Serial.print("FIFO Enabled --> "); ( FLEXCANb_IMASK1(_bus) & FLEXCAN_IFLAG1_BUF5I ) ? Serial.println("Interrupt Enabled") : Serial.println("Interrupt Disabled"); + Serial.print("\tFIFO Filters in use: "); + uint32_t remaining_mailboxes = FLEXCANb_MAXMB_SIZE(_bus) - 6 /* MAXMB - FIFO */ - ((((FLEXCANb_CTRL2(_bus) >> FLEXCAN_CTRL2_RFFN_BIT_NO) & 0xF) + 1) * 2); + if ( FLEXCANb_MAXMB_SIZE(_bus) < (6 + ((((FLEXCANb_CTRL2(_bus) >> FLEXCAN_CTRL2_RFFN_BIT_NO) & 0xF) + 1) * 2))) remaining_mailboxes = 0; + Serial.println(constrain((uint8_t)(FLEXCANb_MAXMB_SIZE(_bus) - remaining_mailboxes), 0, 32)); + Serial.print("\tRemaining Mailboxes: "); + if ( FLEXCANb_MAXMB_SIZE(_bus) < (6 + ((((FLEXCANb_CTRL2(_bus) >> FLEXCAN_CTRL2_RFFN_BIT_NO) & 0xF) + 1) * 2))) remaining_mailboxes = 0; + Serial.println(remaining_mailboxes); // 8 filters per 2 mailboxes + for ( uint8_t i = FLEXCANb_MAXMB_SIZE(_bus) - remaining_mailboxes; i < FLEXCANb_MAXMB_SIZE(_bus); i++ ) { + switch ( FLEXCAN_get_code(FLEXCANb_MBn_CS(_bus, i)) ) { + case 0b0000: { + Serial.print("\t\tMB"); Serial.print(i); Serial.println(" code: RX_INACTIVE"); break; + } + case 0b0100: { + Serial.print("\t\tMB"); Serial.print(i); Serial.print(" code: RX_EMPTY"); + (FLEXCANb_MBn_CS(_bus, i) & FLEXCAN_MB_CS_IDE) ? Serial.println("\t(Extended Frame)") : Serial.println("\t(Standard Frame)"); + break; + } + case 0b0010: { + Serial.print("\t\tMB"); Serial.print(i); Serial.println(" code: RX_FULL"); break; + } + case 0b0110: { + Serial.print("\t\tMB"); Serial.print(i); Serial.println(" code: RX_OVERRUN"); break; + } + case 0b1010: { + Serial.print("\t\tMB"); Serial.print(i); Serial.println(" code: RX_RANSWER"); break; + } + case 0b0001: { + Serial.print("\t\tMB"); Serial.print(i); Serial.println(" code: RX_BUSY"); break; + } + case 0b1000: { + Serial.print("\t\tMB"); Serial.print(i); Serial.println(" code: TX_INACTIVE"); break; + } + case 0b1001: { + Serial.print("\t\tMB"); Serial.print(i); Serial.println(" code: TX_ABORT"); break; + } + case 0b1100: { + Serial.print("\t\tMB"); Serial.print(i); Serial.print(" code: TX_DATA (Transmitting)"); + uint32_t extid = (FLEXCANb_MBn_CS(_bus, i) & FLEXCAN_MB_CS_IDE); + (extid) ? Serial.print("(Extended Frame)") : Serial.print("(Standard Frame)"); + uint32_t dataIn = FLEXCANb_MBn_WORD0(_bus, i); + uint32_t id = (FLEXCANb_MBn_ID(_bus, i) & FLEXCAN_MB_ID_EXT_MASK); + if (!extid) id >>= FLEXCAN_MB_ID_STD_BIT_NO; + Serial.print("(ID: 0x"); Serial.print(id, HEX); Serial.print(")"); + Serial.print("(Payload: "); Serial.print((uint8_t)(dataIn >> 24), HEX); + Serial.print(" "); Serial.print((uint8_t)(dataIn >> 16), HEX); + Serial.print(" "); Serial.print((uint8_t)(dataIn >> 8), HEX); + Serial.print(" "); Serial.print((uint8_t)dataIn, HEX); + dataIn = FLEXCANb_MBn_WORD1(_bus, i); + Serial.print(" "); Serial.print((uint8_t)(dataIn >> 24), HEX); + Serial.print(" "); Serial.print((uint8_t)(dataIn >> 16), HEX); + Serial.print(" "); Serial.print((uint8_t)(dataIn >> 8), HEX); + Serial.print(" "); Serial.print((uint8_t)dataIn, HEX); + Serial.println(")"); + break; + } + case 0b1110: { + Serial.print("\t\tMB"); Serial.print(i); Serial.println(" code: TX_TANSWER"); break; + } + } + } // for loop + return; + } // fifo detected ends here + + Serial.print("FIFO Disabled\n\tMailboxes:\n"); + for ( uint8_t i = 0; i < FLEXCANb_MAXMB_SIZE(_bus); i++ ) { + switch ( FLEXCAN_get_code(FLEXCANb_MBn_CS(_bus, i)) ) { + case 0b0000: { + Serial.print("\t\tMB"); Serial.print(i); Serial.println(" code: RX_INACTIVE"); break; + } + case 0b0100: { + Serial.print("\t\tMB"); Serial.print(i); Serial.print(" code: RX_EMPTY"); + (FLEXCANb_MBn_CS(_bus, i) & FLEXCAN_MB_CS_IDE) ? Serial.println("\t(Extended Frame)") : Serial.println("\t(Standard Frame)"); + break; + } + case 0b0010: { + Serial.print("\t\tMB"); Serial.print(i); Serial.println(" code: RX_FULL"); break; + } + case 0b0110: { + Serial.print("\t\tMB"); Serial.print(i); Serial.println(" code: RX_OVERRUN"); break; + } + case 0b1010: { + Serial.print("\t\tMB"); Serial.print(i); Serial.println(" code: RX_RANSWER"); break; + } + case 0b0001: { + Serial.print("\t\tMB"); Serial.print(i); Serial.println(" code: RX_BUSY"); break; + } + case 0b1000: { + Serial.print("\t\tMB"); Serial.print(i); Serial.println(" code: TX_INACTIVE"); break; + } + case 0b1001: { + Serial.print("\t\tMB"); Serial.print(i); Serial.println(" code: TX_ABORT"); break; + } + case 0b1100: { + Serial.print("\t\tMB"); Serial.print(i); Serial.print(" code: TX_DATA (Transmitting)"); + uint32_t extid = (FLEXCANb_MBn_CS(_bus, i) & FLEXCAN_MB_CS_IDE); + (extid) ? Serial.print("(Extended Frame)") : Serial.print("(Standard Frame)"); + uint32_t dataIn = FLEXCANb_MBn_WORD0(_bus, i); + uint32_t id = (FLEXCANb_MBn_ID(_bus, i) & FLEXCAN_MB_ID_EXT_MASK); + if (!extid) id >>= FLEXCAN_MB_ID_STD_BIT_NO; + Serial.print("(ID: 0x"); Serial.print(id, HEX); Serial.print(")"); + Serial.print("(Payload: "); Serial.print((uint8_t)(dataIn >> 24), HEX); + Serial.print(" "); Serial.print((uint8_t)(dataIn >> 16), HEX); + Serial.print(" "); Serial.print((uint8_t)(dataIn >> 8), HEX); + Serial.print(" "); Serial.print((uint8_t)dataIn, HEX); + dataIn = FLEXCANb_MBn_WORD1(_bus, i); + Serial.print(" "); Serial.print((uint8_t)(dataIn >> 24), HEX); + Serial.print(" "); Serial.print((uint8_t)(dataIn >> 16), HEX); + Serial.print(" "); Serial.print((uint8_t)(dataIn >> 8), HEX); + Serial.print(" "); Serial.print((uint8_t)dataIn, HEX); + Serial.println(")"); + break; + } + case 0b1110: { + Serial.print("\t\tMB"); Serial.print(i); Serial.println(" code: TX_TANSWER"); break; + } + } + } // for loop +} + +FCTP_FUNC uint64_t FCTP_OPT::readIFLAG() { +#if defined(__IMXRT1062__) + return (((uint64_t)FLEXCANb_IFLAG2(_bus) << 32) | FLEXCANb_IFLAG1(_bus)); +#endif + return FLEXCANb_IFLAG1(_bus); +} + +FCTP_FUNC void FCTP_OPT::writeIFLAG(uint64_t value) { +#if defined(__IMXRT1062__) + FLEXCANb_IFLAG2(_bus) = value >> 32; +#endif + FLEXCANb_IFLAG1(_bus) = value; +} + +FCTP_FUNC void FCTP_OPT::writeIFLAGBit(uint8_t mb_num) { // bugfix by bsundahl1 + if ( mb_num < 32 ) FLEXCANb_IFLAG1(_bus) = (1UL << mb_num); + else FLEXCANb_IFLAG2(_bus) = (1UL << (mb_num - 32)); +} + +FCTP_FUNC void FCTP_OPT::writeIMASK(uint64_t value) { +#if defined(__IMXRT1062__) + FLEXCANb_IMASK2(_bus) = value >> 32; +#endif + FLEXCANb_IMASK1(_bus) = value; +} + +FCTP_FUNC uint64_t FCTP_OPT::readIMASK() { +#if defined(__IMXRT1062__) + return (((uint64_t)FLEXCANb_IMASK2(_bus) << 32) | FLEXCANb_IMASK1(_bus)); +#endif + return FLEXCANb_IMASK1(_bus); +} + +FCTP_FUNC void FCTP_OPT::writeIMASKBit(uint8_t mb_num, bool set) { + if ( mb_num < 32 ) (( set ) ? FLEXCANb_IMASK1(_bus) |= (1UL << mb_num) : FLEXCANb_IMASK1(_bus) &= ~(1UL << mb_num)); + else (( set ) ? FLEXCANb_IMASK2(_bus) |= (1UL << (mb_num - 32)) : FLEXCANb_IMASK2(_bus) &= ~(1UL << (mb_num - 32))); +} + +FCTP_FUNC void FCTP_OPT::writeTxMailbox(uint8_t mb_num, const CAN_message_t &msg) { + writeIFLAGBit(mb_num); + uint32_t code = 0; + volatile uint32_t *mbxAddr = &(*(volatile uint32_t*)(_bus + 0x80 + (mb_num * 0x10))); + mbxAddr[0] = FLEXCAN_MB_CS_CODE(FLEXCAN_MB_CODE_TX_INACTIVE); + mbxAddr[1] = (( msg.flags.extended ) ? ( msg.id & FLEXCAN_MB_ID_EXT_MASK ) : FLEXCAN_MB_ID_IDSTD(msg.id)); + if ( msg.flags.remote ) code |= (1UL << 20); + if ( msg.flags.extended ) code |= (3UL << 21); + for ( uint8_t i = 0; i < (8 >> 2); i++ ) mbxAddr[2 + i] = (msg.buf[0 + i * 4] << 24) | (msg.buf[1 + i * 4] << 16) | (msg.buf[2 + i * 4] << 8) | msg.buf[3 + i * 4]; + code |= msg.len << 16; + mbxAddr[0] = code | FLEXCAN_MB_CS_CODE(FLEXCAN_MB_CODE_TX_ONCE); +} + +FCTP_FUNC uint8_t FCTP_OPT::mailboxOffset() { + if ( !(FLEXCANb_MCR(_bus) & FLEXCAN_MCR_FEN ) ) return 0; /* return offset 0 since FIFO is disabled */ + uint32_t remaining_mailboxes = FLEXCANb_MAXMB_SIZE(_bus) - 6 /* MAXMB - FIFO */ - ((((FLEXCANb_CTRL2(_bus) >> FLEXCAN_CTRL2_RFFN_BIT_NO) & 0xF) + 1) * 2); + if ( FLEXCANb_MAXMB_SIZE(_bus) < (6 + ((((FLEXCANb_CTRL2(_bus) >> FLEXCAN_CTRL2_RFFN_BIT_NO) & 0xF) + 1) * 2))) remaining_mailboxes = 0; + return (FLEXCANb_MAXMB_SIZE(_bus) - remaining_mailboxes); /* otherwise return offset MB position after FIFO area */ +} + +FCTP_FUNC void FCTP_OPT::setMaxMB(uint8_t last) { + last = constrain(last,1,64); + last--; + FLEXCAN_EnterFreezeMode(); + bool fifo_was_cleared = FLEXCANb_MCR(_bus) & FLEXCAN_MCR_FEN; + disableFIFO(); + writeIFLAG(readIFLAG()); // (all bits reset when written back) (needed for MAXMB changes) + FLEXCANb_MCR(_bus) &= ~0x7F; // clear current value + FLEXCANb_MCR(_bus) |= last; // set mailbox max + if ( fifo_was_cleared ) enableFIFO(); + FLEXCAN_ExitFreezeMode(); +} + +FCTP_FUNC void FCTP_OPT::softReset() { + FLEXCANb_MCR(_bus) |= FLEXCAN_MCR_SOFT_RST; + while (FLEXCANb_MCR(_bus) & FLEXCAN_MCR_SOFT_RST); +} + +FCTP_FUNC void FCTP_OPT::FLEXCAN_ExitFreezeMode() { + FLEXCANb_MCR(_bus) &= ~FLEXCAN_MCR_HALT; + while (FLEXCANb_MCR(_bus) & FLEXCAN_MCR_FRZ_ACK); +} + +FCTP_FUNC void FCTP_OPT::FLEXCAN_EnterFreezeMode() { + FLEXCANb_MCR(_bus) |= FLEXCAN_MCR_FRZ | FLEXCAN_MCR_HALT; + while (!(FLEXCANb_MCR(_bus) & FLEXCAN_MCR_FRZ_ACK)); +} + +FCTP_FUNC void FCTP_OPT::setBaudRate(uint32_t baud, FLEXCAN_RXTX listen_only) { + currentBitrate = baud; + +#if defined(__IMXRT1062__) + uint32_t clockFreq = getClock() * 1000000; +#else + uint32_t clockFreq = 16000000; +#endif + + uint32_t divisor = 0, bestDivisor = 0, result = clockFreq / baud / (divisor + 1); + int error = baud - (clockFreq / (result * (divisor + 1))), bestError = error; + + bool frz_flag_negate = !(FLEXCANb_MCR(_bus) & FLEXCAN_MCR_FRZ_ACK); + FLEXCAN_EnterFreezeMode(); + + while (result > 5) { + divisor++; + result = clockFreq / baud / (divisor + 1); + if (result <= 25) { + error = baud - (clockFreq / (result * (divisor + 1))); + if (error < 0) error *= -1; + if (error < bestError) { + bestError = error; + bestDivisor = divisor; + } + if ((error == bestError) && (result > 11) && (result < 19)) { + bestError = error; + bestDivisor = divisor; + } + } + } + + divisor = bestDivisor; + result = clockFreq / baud / (divisor + 1); + + if ((result < 5) || (result > 25) || (bestError > 300)) { + if ( frz_flag_negate ) FLEXCAN_ExitFreezeMode(); + return; + } + + result -= 5; // the bitTimingTable is offset by 5 since there was no reason to store bit timings for invalid numbers + uint8_t bitTimingTable[21][3] = { + {0, 0, 1}, //5 + {1, 0, 1}, //6 + {1, 1, 1}, //7 + {2, 1, 1}, //8 + {2, 2, 1}, //9 + {2, 3, 1}, //10 + {2, 3, 2}, //11 + {2, 4, 2}, //12 + {2, 5, 2}, //13 + {2, 5, 3}, //14 + {2, 6, 3}, //15 + {2, 7, 3}, //16 + {2, 7, 4}, //17 + {3, 7, 4}, //18 + {3, 7, 5}, //19 + {4, 7, 5}, //20 + {4, 7, 6}, //21 + {5, 7, 6}, //22 + {6, 7, 6}, //23 + {6, 7, 7}, //24 + {7, 7, 7}, //25 + }, propSeg = bitTimingTable[result][0], pSeg1 = bitTimingTable[result][1], pSeg2 = bitTimingTable[result][2]; + FLEXCANb_CTRL1(_bus) = (FLEXCAN_CTRL_PROPSEG(propSeg) | FLEXCAN_CTRL_RJW(1) | FLEXCAN_CTRL_PSEG1(pSeg1) | + FLEXCAN_CTRL_PSEG2(pSeg2) | FLEXCAN_CTRL_ERR_MSK | FLEXCAN_CTRL_PRESDIV(divisor)); + ( listen_only != LISTEN_ONLY ) ? FLEXCANb_CTRL1(_bus) &= ~FLEXCAN_CTRL_LOM : FLEXCANb_CTRL1(_bus) |= FLEXCAN_CTRL_LOM; /* listen-only mode */ + if ( frz_flag_negate ) FLEXCAN_ExitFreezeMode(); +} + +FCTP_FUNC void FCTP_OPT::setMRP(bool mrp) { /* mailbox priority (1) or FIFO priority (0) */ + FLEXCAN_EnterFreezeMode(); + if ( mrp ) FLEXCANb_CTRL2(_bus) |= FLEXCAN_CTRL2_MRP; + else FLEXCANb_CTRL2(_bus) &= ~FLEXCAN_CTRL2_MRP; + FLEXCAN_ExitFreezeMode(); +} + +FCTP_FUNC void FCTP_OPT::setRRS(bool rrs) { /* store remote frames */ + FLEXCAN_EnterFreezeMode(); + if ( rrs ) FLEXCANb_CTRL2(_bus) |= FLEXCAN_CTRL2_RRS; + else FLEXCANb_CTRL2(_bus) &= ~FLEXCAN_CTRL2_RRS; + FLEXCAN_ExitFreezeMode(); +} + +FCTP_FUNC void FCTP_OPT::setTX(FLEXCAN_PINS pin) { +#if defined(__IMXRT1062__) + if ( _bus == CAN3 ) { + if ( pin == DEF ) { + IOMUXC_SW_MUX_CTL_PAD_GPIO_EMC_36 = 0x19; // pin31 T3B2 + IOMUXC_SW_PAD_CTL_PAD_GPIO_EMC_36 = 0x10B0; // pin31 T3B2 + } + } + if ( _bus == CAN2 ) { + if ( pin == DEF ) { + IOMUXC_SW_MUX_CTL_PAD_GPIO_AD_B0_02 = 0x10; // pin 1 T4B1+B2 + IOMUXC_SW_PAD_CTL_PAD_GPIO_AD_B0_02 = 0x10B0; // pin 1 T4B1+B2 + } + } + if ( _bus == CAN1 ) { + if ( pin == DEF ) { + IOMUXC_SW_MUX_CTL_PAD_GPIO_AD_B1_08 = 0x12; // pin 22 T4B1+B2 + IOMUXC_SW_PAD_CTL_PAD_GPIO_AD_B1_08 = 0x10B0; // pin 22 T4B1+B2 + } + if ( pin == ALT ) { + IOMUXC_SW_MUX_CTL_PAD_GPIO_B0_02 = 0x12; // pin 11 T4B1+B2 + IOMUXC_SW_PAD_CTL_PAD_GPIO_B0_02= 0x10B0; // pin 11 T4B1+B2 + } + } +#endif + +#if defined(__MK20DX256__) + CORE_PIN3_CONFIG = PORT_PCR_MUX(2); +#endif +#if defined(__MK64FX512__) || defined(__MK66FX1M0__) + if ( _bus == CAN0 ) { + static bool init_pins = 1; + if ( init_pins ) { + pin = ( !CORE_PIN3_CONFIG ) ? DEF : ALT; + init_pins = 0; + } + if ( pin == ALT ) { + if ( CORE_PIN3_CONFIG == PORT_PCR_MUX(2) ) CORE_PIN3_CONFIG = 0; + CORE_PIN29_CONFIG = PORT_PCR_MUX(2); + } + else if ( pin == DEF ) { + if ( CORE_PIN29_CONFIG == PORT_PCR_MUX(2) ) CORE_PIN29_CONFIG = 0; + CORE_PIN3_CONFIG = PORT_PCR_MUX(2); + } + } /* Alternative CAN1 pins are not broken out on Teensy 3.6 */ +#endif +#if defined(__MK66FX1M0__) + if ( _bus == CAN1 ) { + CORE_PIN33_CONFIG = PORT_PCR_MUX(2); + } +#endif + +} + +FCTP_FUNC void FCTP_OPT::setRX(FLEXCAN_PINS pin) { +#if defined(__IMXRT1062__) + /* DAISY REGISTER CAN3 + 00 GPIO_EMC_37_ALT9 Selecting Pad: GPIO_EMC_37 for Mode: ALT9 + 01 GPIO_AD_B0_15_ALT8 Selecting Pad: GPIO_AD_B0_15 for Mode: ALT8 + 10 GPIO_AD_B0_11_ALT8 Selecting Pad: GPIO_AD_B0_11 for Mode: ALT8 + */ + /* DAISY REGISTER CAN2 + 00 GPIO_EMC_10_ALT3 Selecting Pad: GPIO_EMC_10 for Mode: ALT3 + 01 GPIO_AD_B0_03_ALT0 Selecting Pad: GPIO_AD_B0_03 for Mode: ALT0 + 10 GPIO_AD_B0_15_ALT6 Selecting Pad: GPIO_AD_B0_15 for Mode: ALT6 + 11 GPIO_B1_09_ALT6 Selecting Pad: GPIO_B1_09 for Mode: ALT6 + */ + /* DAISY REGISTER CAN1 + 00 GPIO_SD_B1_03_ALT4 Selecting Pad: GPIO_SD_B1_03 for Mode: ALT4 + 01 GPIO_EMC_18_ALT3 Selecting Pad: GPIO_EMC_18 for Mode: ALT3 + 10 GPIO_AD_B1_09_ALT2 Selecting Pad: GPIO_AD_B1_09 for Mode: ALT2 + 11 GPIO_B0_03_ALT2 Selecting Pad: GPIO_B0_03 for Mode: ALT2 + */ + if ( _bus == CAN3 ) { + if ( pin == DEF ) { + IOMUXC_CANFD_IPP_IND_CANRX_SELECT_INPUT = 0x00; + IOMUXC_SW_MUX_CTL_PAD_GPIO_EMC_37 = 0x19; // pin30 T3B2 + IOMUXC_SW_PAD_CTL_PAD_GPIO_EMC_37 = 0x10B0; // pin30 T3B2 + } + } + if ( _bus == CAN2 ) { + if ( pin == DEF ) { + IOMUXC_FLEXCAN2_RX_SELECT_INPUT = 0x01; + IOMUXC_SW_MUX_CTL_PAD_GPIO_AD_B0_03 = 0x10; // pin 0 T4B1+B2 + IOMUXC_SW_PAD_CTL_PAD_GPIO_AD_B0_03 = 0x10B0; // pin 0 T4B1+B2 + } + } + if ( _bus == CAN1 ) { + if ( pin == DEF ) { + IOMUXC_FLEXCAN1_RX_SELECT_INPUT = 0x02; + IOMUXC_SW_MUX_CTL_PAD_GPIO_AD_B1_09 = 0x12; // pin 23 T4B1+B2 + IOMUXC_SW_PAD_CTL_PAD_GPIO_AD_B1_09 = 0x10B0; // pin 23 T4B1+B2 + } + if ( pin == ALT ) { + IOMUXC_FLEXCAN1_RX_SELECT_INPUT = 0x03; + IOMUXC_SW_MUX_CTL_PAD_GPIO_B0_03 = 0x12; // pin 13 T4B1+B2 + IOMUXC_SW_PAD_CTL_PAD_GPIO_B0_03 = 0x10B0; // pin 13 T4B1+B2 + } + } +#endif + +#if defined(__MK20DX256__) + CORE_PIN4_CONFIG = PORT_PCR_MUX(2); +#endif +#if defined(__MK64FX512__) || defined(__MK66FX1M0__) + if ( _bus == CAN0 ) { + static bool init_pins = 1; + if ( init_pins ) { + pin = ( !CORE_PIN4_CONFIG ) ? DEF : ALT; + init_pins = 0; + } + if ( pin == ALT ) { + if ( CORE_PIN4_CONFIG == PORT_PCR_MUX(2) ) CORE_PIN4_CONFIG = 0; + CORE_PIN30_CONFIG = PORT_PCR_MUX(2); + } + else if ( pin == DEF ) { + if ( CORE_PIN30_CONFIG == PORT_PCR_MUX(2) ) CORE_PIN30_CONFIG = 0; + CORE_PIN4_CONFIG = PORT_PCR_MUX(2); + } + } /* Alternative CAN1 pins are not broken out on Teensy 3.6 */ +#endif +#if defined(__MK66FX1M0__) + if ( _bus == CAN1 ) { + CORE_PIN34_CONFIG = PORT_PCR_MUX(2); + } +#endif + +} + +FCTP_FUNC bool FCTP_OPT::setMBUserFilter(FLEXCAN_MAILBOX mb_num, uint32_t id1, uint32_t mask) { + if ( mb_num < mailboxOffset() || mb_num >= FLEXCANb_MAXMB_SIZE(_bus) ) return 0; /* mailbox not available */ + if ( (FLEXCAN_get_code(FLEXCANb_MBn_CS(_bus, mb_num)) >> 3) ) return 0; /* exit on TX mailbox */ + setMBFilterProcessing(mb_num,id1,mask); + filter_store(FLEXCAN_USERMASK, mb_num, 1, id1, 0, 0, 0, mask); + return 1; +} + +FCTP_FUNC bool FCTP_OPT::setMBUserFilter(FLEXCAN_MAILBOX mb_num, uint32_t id1, uint32_t id2, uint32_t mask) { + if ( mb_num < mailboxOffset() || mb_num >= FLEXCANb_MAXMB_SIZE(_bus) ) return 0; /* mailbox not available */ + if ( (FLEXCAN_get_code(FLEXCANb_MBn_CS(_bus, mb_num)) >> 3) ) return 0; /* exit on TX mailbox */ + uint32_t new_mask = ( !(FLEXCANb_MBn_CS(_bus, mb_num) & FLEXCAN_MB_CS_IDE) ) ? FLEXCAN_MB_ID_IDSTD((((id1 | id2) ^ (id1 & id2)) ^ 0x7FF) & mask) : ((((id1 | id2) ^ (id1 & id2)) ^ 0x1FFFFFFF) & mask); + setMBFilterProcessing(mb_num,id1,new_mask); + filter_store(FLEXCAN_USERMASK, mb_num, 2, id1, id2, 0, 0, mask); + return 1; +} + +FCTP_FUNC bool FCTP_OPT::setMBUserFilter(FLEXCAN_MAILBOX mb_num, uint32_t id1, uint32_t id2, uint32_t id3, uint32_t mask) { + if ( mb_num < mailboxOffset() || mb_num >= FLEXCANb_MAXMB_SIZE(_bus) ) return 0; /* mailbox not available */ + if ( (FLEXCAN_get_code(FLEXCANb_MBn_CS(_bus, mb_num)) >> 3) ) return 0; /* exit on TX mailbox */ + uint32_t new_mask = ( !(FLEXCANb_MBn_CS(_bus, mb_num) & FLEXCAN_MB_CS_IDE) ) ? FLEXCAN_MB_ID_IDSTD((((id1 | id2 | id3) ^ (id1 & id2 & id3)) ^ 0x7FF) & mask) : ((((id1 | id2 | id3) ^ (id1 & id2 & id3)) ^ 0x1FFFFFFF) & mask); + setMBFilterProcessing(mb_num,id1,new_mask); + filter_store(FLEXCAN_USERMASK, mb_num, 3, id1, id2, id3, 0, mask); + return 1; +} + +FCTP_FUNC bool FCTP_OPT::setMBUserFilter(FLEXCAN_MAILBOX mb_num, uint32_t id1, uint32_t id2, uint32_t id3, uint32_t id4, uint32_t mask) { + if ( mb_num < mailboxOffset() || mb_num >= FLEXCANb_MAXMB_SIZE(_bus) ) return 0; /* mailbox not available */ + if ( (FLEXCAN_get_code(FLEXCANb_MBn_CS(_bus, mb_num)) >> 3) ) return 0; /* exit on TX mailbox */ + uint32_t new_mask = ( !(FLEXCANb_MBn_CS(_bus, mb_num) & FLEXCAN_MB_CS_IDE) ) ? FLEXCAN_MB_ID_IDSTD((((id1 | id2 | id3 | id4) ^ (id1 & id2 & id3 & id4)) ^ 0x7FF) & mask) : ((((id1 | id2 | id3 | id4) ^ (id1 & id2 & id3 & id4)) ^ 0x1FFFFFFF) & mask); + setMBFilterProcessing(mb_num,id1,new_mask); + filter_store(FLEXCAN_USERMASK, mb_num, 4, id1, id2, id3, id4, mask); + return 1; +} + +FCTP_FUNC bool FCTP_OPT::setFIFOUserFilter(uint8_t filter, uint32_t id1, uint32_t mask, const FLEXCAN_IDE &ide, const FLEXCAN_IDE &remote) { + if ( !(FLEXCANb_MCR(_bus) & FLEXCAN_MCR_FEN )) return 0; /* FIFO not enabled. */ + uint8_t max_fifo_filters = (((FLEXCANb_CTRL2(_bus) >> FLEXCAN_CTRL2_RFFN_BIT_NO) & 0xF) + 1) * 8; // 8->128 + if ( filter >= max_fifo_filters ) return 0; + bool frz_flag_negate = !(FLEXCANb_MCR(_bus) & FLEXCAN_MCR_FRZ_ACK); + FLEXCAN_EnterFreezeMode(); + /* WE ONLY USE TABLE A FOR CUSTOM FILTERS, AS TABLE B+ (PARTIAL IDS) LEAST SIGNIFICANT BITS ARE IGNORED */ + /* ##################################### TABLE A ###################################### */ + if ( ((FLEXCANb_MCR(_bus) & FLEXCAN_MCR_IDAM_MASK) >> FLEXCAN_MCR_IDAM_BIT_NO) == 0 ) { +// uint32_t new_mask = mask << (( ide == EXT ) ? 1 : 19) | 0xC0000001; + uint32_t new_mask = ( ide != EXT ) ? ((((id1) ^ (id1)) ^ 0x7FF) << 19 ) | 0xC0000001 : (((((id1) ^ (id1)) ^ 0x1FFFFFFF) & mask) << 1 ) | 0xC0000001; + FLEXCANb_IDFLT_TAB(_bus, filter) = ((ide == EXT ? 1 : 0) << 30) | ((remote == RTR ? 1 : 0) << 31) | + ((ide == EXT ? ((id1 & FLEXCAN_MB_ID_EXT_MASK) << 1) : (FLEXCAN_MB_ID_IDSTD(id1) << 1))); + if ( filter < constrain(mailboxOffset(), 0, 32) ) FLEXCANb_RXIMR(_bus, filter) = new_mask; + FLEXCANb_RXFGMASK(_bus) = 0x3FFFFFFF; /* enforce it for blocks 32->127, single IDs */ + + fifo_filter_table[filter][0] = ( ((ide == EXT) ? 1UL : 0UL) << 16); /* extended flag check */ + fifo_filter_store(FLEXCAN_USERMASK, filter, 1, id1, 0, 0, 0, mask); + } + /* #################################################################################### */ + if ( frz_flag_negate ) FLEXCAN_ExitFreezeMode(); + return 1; +} + +FCTP_FUNC bool FCTP_OPT::setFIFOUserFilter(uint8_t filter, uint32_t id1, uint32_t id2, uint32_t mask, const FLEXCAN_IDE &ide, const FLEXCAN_IDE &remote) { + if ( !(FLEXCANb_MCR(_bus) & FLEXCAN_MCR_FEN )) return 0; /* FIFO not enabled. */ + uint8_t max_fifo_filters = (((FLEXCANb_CTRL2(_bus) >> FLEXCAN_CTRL2_RFFN_BIT_NO) & 0xF) + 1) * 8; // 8->128 + if ( filter >= max_fifo_filters ) return 0; + bool frz_flag_negate = !(FLEXCANb_MCR(_bus) & FLEXCAN_MCR_FRZ_ACK); + FLEXCAN_EnterFreezeMode(); + /* WE ONLY USE TABLE A FOR CUSTOM FILTERS, AS TABLE B+ (PARTIAL IDS) LEAST SIGNIFICANT BITS ARE IGNORED */ + /* ##################################### TABLE A ###################################### */ + if ( ((FLEXCANb_MCR(_bus) & FLEXCAN_MCR_IDAM_MASK) >> FLEXCAN_MCR_IDAM_BIT_NO) == 0 ) { + uint32_t new_mask = ( ide != EXT ) ? ((((id1 | id2) ^ (id1 & id2)) ^ 0x7FF) << 19 ) | 0xC0000001 : (((((id1 | id2) ^ (id1 & id2)) ^ 0x1FFFFFFF) & mask) << 1 ) | 0xC0000001; + FLEXCANb_IDFLT_TAB(_bus, filter) = ((ide == EXT ? 1 : 0) << 30) | ((remote == RTR ? 1 : 0) << 31) | + ((ide == EXT ? ((id1 & FLEXCAN_MB_ID_EXT_MASK) << 1) : (FLEXCAN_MB_ID_IDSTD(id1) << 1))); + if ( filter < constrain(mailboxOffset(), 0, 32) ) FLEXCANb_RXIMR(_bus, filter) = new_mask; + FLEXCANb_RXFGMASK(_bus) = 0x3FFFFFFF; /* enforce it for blocks 32->127, single IDs */ + + fifo_filter_table[filter][0] = ( ((ide == EXT) ? 1UL : 0UL) << 16); /* extended flag check */ + fifo_filter_store(FLEXCAN_USERMASK, filter, 2, id1, id2, 0, 0, mask); + } + /* #################################################################################### */ + if ( frz_flag_negate ) FLEXCAN_ExitFreezeMode(); + return 1; +} + +FCTP_FUNC bool FCTP_OPT::setFIFOUserFilter(uint8_t filter, uint32_t id1, uint32_t id2, uint32_t id3, uint32_t mask, const FLEXCAN_IDE &ide, const FLEXCAN_IDE &remote) { + if ( !(FLEXCANb_MCR(_bus) & FLEXCAN_MCR_FEN )) return 0; /* FIFO not enabled. */ + uint8_t max_fifo_filters = (((FLEXCANb_CTRL2(_bus) >> FLEXCAN_CTRL2_RFFN_BIT_NO) & 0xF) + 1) * 8; // 8->128 + if ( filter >= max_fifo_filters ) return 0; + bool frz_flag_negate = !(FLEXCANb_MCR(_bus) & FLEXCAN_MCR_FRZ_ACK); + FLEXCAN_EnterFreezeMode(); + /* WE ONLY USE TABLE A FOR CUSTOM FILTERS, AS TABLE B+ (PARTIAL IDS) LEAST SIGNIFICANT BITS ARE IGNORED */ + /* ##################################### TABLE A ###################################### */ + if ( ((FLEXCANb_MCR(_bus) & FLEXCAN_MCR_IDAM_MASK) >> FLEXCAN_MCR_IDAM_BIT_NO) == 0 ) { + uint32_t new_mask = ( ide != EXT ) ? ((((id1 | id2 | id3) ^ (id1 & id2 & id3)) ^ 0x7FF) << 19 ) | 0xC0000001 : (((((id1 | id2 | id3) ^ (id1 & id2 & id3)) ^ 0x1FFFFFFF) & mask) << 1 ) | 0xC0000001; + FLEXCANb_IDFLT_TAB(_bus, filter) = ((ide == EXT ? 1 : 0) << 30) | ((remote == RTR ? 1 : 0) << 31) | + ((ide == EXT ? ((id1 & FLEXCAN_MB_ID_EXT_MASK) << 1) : (FLEXCAN_MB_ID_IDSTD(id1) << 1))); + if ( filter < constrain(mailboxOffset(), 0, 32) ) FLEXCANb_RXIMR(_bus, filter) = new_mask; + FLEXCANb_RXFGMASK(_bus) = 0x3FFFFFFF; /* enforce it for blocks 32->127, single IDs */ + + fifo_filter_table[filter][0] = ( ((ide == EXT) ? 1UL : 0UL) << 16); /* extended flag check */ + fifo_filter_store(FLEXCAN_USERMASK, filter, 3, id1, id2, id3, 0, mask); + } + /* #################################################################################### */ + if ( frz_flag_negate ) FLEXCAN_ExitFreezeMode(); + return 1; +} + +FCTP_FUNC bool FCTP_OPT::setFIFOUserFilter(uint8_t filter, uint32_t id1, uint32_t id2, uint32_t id3, uint32_t id4, uint32_t mask, const FLEXCAN_IDE &ide, const FLEXCAN_IDE &remote) { + if ( !(FLEXCANb_MCR(_bus) & FLEXCAN_MCR_FEN )) return 0; /* FIFO not enabled. */ + uint8_t max_fifo_filters = (((FLEXCANb_CTRL2(_bus) >> FLEXCAN_CTRL2_RFFN_BIT_NO) & 0xF) + 1) * 8; // 8->128 + if ( filter >= max_fifo_filters ) return 0; + bool frz_flag_negate = !(FLEXCANb_MCR(_bus) & FLEXCAN_MCR_FRZ_ACK); + FLEXCAN_EnterFreezeMode(); + /* WE ONLY USE TABLE A FOR CUSTOM FILTERS, AS TABLE B+ (PARTIAL IDS) LEAST SIGNIFICANT BITS ARE IGNORED */ + /* ##################################### TABLE A ###################################### */ + if ( ((FLEXCANb_MCR(_bus) & FLEXCAN_MCR_IDAM_MASK) >> FLEXCAN_MCR_IDAM_BIT_NO) == 0 ) { + uint32_t new_mask = ( ide != EXT ) ? ((((id1 | id2 | id3 | id4) ^ (id1 & id2 & id3 & id4)) ^ 0x7FF) << 19 ) | 0xC0000001 : (((((id1 | id2 | id3 | id4) ^ (id1 & id2 & id3 & id4)) ^ 0x1FFFFFFF) & mask) << 1 ) | 0xC0000001; + FLEXCANb_IDFLT_TAB(_bus, filter) = ((ide == EXT ? 1 : 0) << 30) | ((remote == RTR ? 1 : 0) << 31) | + ((ide == EXT ? ((id1 & FLEXCAN_MB_ID_EXT_MASK) << 1) : (FLEXCAN_MB_ID_IDSTD(id1) << 1))); + if ( filter < constrain(mailboxOffset(), 0, 32) ) FLEXCANb_RXIMR(_bus, filter) = new_mask; + FLEXCANb_RXFGMASK(_bus) = 0x3FFFFFFF; /* enforce it for blocks 32->127, single IDs */ + + fifo_filter_table[filter][0] = ( ((ide == EXT) ? 1UL : 0UL) << 16); /* extended flag check */ + fifo_filter_store(FLEXCAN_USERMASK, filter, 4, id1, id2, id3, id4, mask); + } + /* #################################################################################### */ + if ( frz_flag_negate ) FLEXCAN_ExitFreezeMode(); + return 1; +} + +FCTP_FUNC void FCTP_OPT::setMBFilterProcessing(FLEXCAN_MAILBOX mb_num, uint32_t filter_id, uint32_t calculated_mask) { + bool frz_flag_negate = !(FLEXCANb_MCR(_bus) & FLEXCAN_MCR_FRZ_ACK); + FLEXCAN_EnterFreezeMode(); + FLEXCANb_RXIMR(_bus, mb_num) = calculated_mask | ((FLEXCANb_CTRL2(_bus) & FLEXCAN_CTRL2_EACEN) ? (1UL << 30) : 0); + FLEXCANb_MBn_ID(_bus, mb_num) = ((!(FLEXCANb_MBn_CS(_bus, mb_num) & FLEXCAN_MB_CS_IDE)) ? FLEXCAN_MB_ID_IDSTD(filter_id) : (filter_id & 0x1FFFFFFF)); + if ( frz_flag_negate ) FLEXCAN_ExitFreezeMode(); +} + +FCTP_FUNC void FCTP_OPT::setMBFilter(FLEXCAN_FLTEN input) { + bool frz_flag_negate = !(FLEXCANb_MCR(_bus) & FLEXCAN_MCR_FRZ_ACK); + FLEXCAN_EnterFreezeMode(); + for (uint8_t i = mailboxOffset(); i < FLEXCANb_MAXMB_SIZE(_bus); i++) { + if ( (FLEXCAN_get_code(FLEXCANb_MBn_CS(_bus, i)) >> 3) ) continue; /* skip TX mailboxes */ + if ( input == ACCEPT_ALL ) FLEXCANb_RXIMR(_bus, i) = 0UL | ((FLEXCANb_CTRL2(_bus) & FLEXCAN_CTRL2_EACEN) ? (1UL << 30) : 0); // (RXIMR) + if ( input == REJECT_ALL ) FLEXCANb_RXIMR(_bus, i) = ~0UL; // (RXIMR) + FLEXCANb_MBn_ID(_bus, i) = ~0UL; + mb_filter_table[i][0] = ( ((FLEXCANb_MBn_CS(_bus, i) & 0x600000) ? 1UL : 0UL) << 27); /* extended flag check */ + } + if ( frz_flag_negate ) FLEXCAN_ExitFreezeMode(); +} + +FCTP_FUNC void FCTP_OPT::setMBFilter(FLEXCAN_MAILBOX mb_num, FLEXCAN_FLTEN input) { + if ( mb_num < mailboxOffset() || mb_num >= FLEXCANb_MAXMB_SIZE(_bus) ) return; /* mailbox not available */ + if ( (FLEXCAN_get_code(FLEXCANb_MBn_CS(_bus, mb_num)) >> 3) ) return; /* exit on TX mailbox */ + bool frz_flag_negate = !(FLEXCANb_MCR(_bus) & FLEXCAN_MCR_FRZ_ACK); + FLEXCAN_EnterFreezeMode(); + if ( input == ACCEPT_ALL ) FLEXCANb_RXIMR(_bus, mb_num) = 0UL | ((FLEXCANb_CTRL2(_bus) & FLEXCAN_CTRL2_EACEN) ? (1UL << 30) : 0); // (RXIMR) + if ( input == REJECT_ALL ) FLEXCANb_RXIMR(_bus, mb_num) = ~0UL; // (RXIMR) + FLEXCANb_MBn_ID(_bus, mb_num) = 0UL; + mb_filter_table[mb_num][0] = ( ((FLEXCANb_MBn_CS(_bus, mb_num) & 0x600000) ? 1UL : 0UL) << 27); /* extended flag check */ + if ( frz_flag_negate ) FLEXCAN_ExitFreezeMode(); +} + +FCTP_FUNC bool FCTP_OPT::setMBManualFilter(FLEXCAN_MAILBOX mb_num, uint32_t id1, uint32_t mask) { + if ( mb_num < mailboxOffset() || mb_num >= FLEXCANb_MAXMB_SIZE(_bus) ) return 0; /* mailbox not available */ + if ( (FLEXCAN_get_code(FLEXCANb_MBn_CS(_bus, mb_num)) >> 3) ) return 0; /* exit on TX mailbox */ + if (!(FLEXCANb_MBn_CS(_bus, mb_num) & FLEXCAN_MB_CS_IDE)) { + mask = mask << 18; /* shift mask by 18 for 11 bit filters */ + } + setMBFilterProcessing(mb_num,id1,mask); + filter_store(FLEXCAN_MULTI, mb_num, 1, id1, 0, 0, 0, 0); + return 1; +} + +FCTP_FUNC bool FCTP_OPT::setMBFilter(FLEXCAN_MAILBOX mb_num, uint32_t id1) { + if ( mb_num < mailboxOffset() || mb_num >= FLEXCANb_MAXMB_SIZE(_bus) ) return 0; /* mailbox not available */ + if ( (FLEXCAN_get_code(FLEXCANb_MBn_CS(_bus, mb_num)) >> 3) ) return 0; /* exit on TX mailbox */ + uint32_t mask = ( !(FLEXCANb_MBn_CS(_bus, mb_num) & FLEXCAN_MB_CS_IDE) ) ? FLEXCAN_MB_ID_IDSTD(((id1) ^ (id1)) ^ 0x7FF) : ((((id1) ^ (id1)) ^ 0x1FFFFFFF) & 0x1FFFFFFF); + setMBFilterProcessing(mb_num,id1,mask); + filter_store(FLEXCAN_MULTI, mb_num, 1, id1, 0, 0, 0, 0); + return 1; +} + +FCTP_FUNC bool FCTP_OPT::setMBFilter(FLEXCAN_MAILBOX mb_num, uint32_t id1, uint32_t id2) { + if ( mb_num < mailboxOffset() || mb_num >= FLEXCANb_MAXMB_SIZE(_bus) ) return 0; /* mailbox not available */ + if ( (FLEXCAN_get_code(FLEXCANb_MBn_CS(_bus, mb_num)) >> 3) ) return 0; /* exit on TX mailbox */ + uint32_t mask = ( !(FLEXCANb_MBn_CS(_bus, mb_num) & FLEXCAN_MB_CS_IDE) ) ? FLEXCAN_MB_ID_IDSTD(((id1 | id2) ^ (id1 & id2)) ^ 0x7FF) : ((((id1 | id2) ^ (id1 & id2)) ^ 0x1FFFFFFF) & 0x1FFFFFFF); + setMBFilterProcessing(mb_num,id1,mask); + filter_store(FLEXCAN_MULTI, mb_num, 2, id1, id2, 0, 0, 0); + return 1; +} + +FCTP_FUNC bool FCTP_OPT::setMBFilter(FLEXCAN_MAILBOX mb_num, uint32_t id1, uint32_t id2, uint32_t id3) { + if ( mb_num < mailboxOffset() || mb_num >= FLEXCANb_MAXMB_SIZE(_bus) ) return 0; /* mailbox not available */ + if ( (FLEXCAN_get_code(FLEXCANb_MBn_CS(_bus, mb_num)) >> 3) ) return 0; /* exit on TX mailbox */ + uint32_t mask = ( !(FLEXCANb_MBn_CS(_bus, mb_num) & FLEXCAN_MB_CS_IDE) ) ? FLEXCAN_MB_ID_IDSTD(((id1 | id2 | id3) ^ (id1 & id2 & id3)) ^ 0x7FF) : ((((id1 | id2 | id3) ^ (id1 & id2 & id3)) ^ 0x1FFFFFFF) & 0x1FFFFFFF); + setMBFilterProcessing(mb_num,id1,mask); + filter_store(FLEXCAN_MULTI, mb_num, 3, id1, id2, id3, 0, 0); + return 1; +} + +FCTP_FUNC bool FCTP_OPT::setMBFilter(FLEXCAN_MAILBOX mb_num, uint32_t id1, uint32_t id2, uint32_t id3, uint32_t id4) { + if ( mb_num < mailboxOffset() || mb_num >= FLEXCANb_MAXMB_SIZE(_bus) ) return 0; /* mailbox not available */ + if ( (FLEXCAN_get_code(FLEXCANb_MBn_CS(_bus, mb_num)) >> 3) ) return 0; /* exit on TX mailbox */ + uint32_t mask = ( !(FLEXCANb_MBn_CS(_bus, mb_num) & FLEXCAN_MB_CS_IDE) ) ? FLEXCAN_MB_ID_IDSTD(((id1 | id2 | id3 | id4) ^ (id1 & id2 & id3 & id4)) ^ 0x7FF) : ((((id1 | id2 | id3 | id4) ^ (id1 & id2 & id3 & id4)) ^ 0x1FFFFFFF) & 0x1FFFFFFF); + setMBFilterProcessing(mb_num,id1,mask); + filter_store(FLEXCAN_MULTI, mb_num, 4, id1, id2, id3, id4, 0); + return 1; +} + +FCTP_FUNC bool FCTP_OPT::setMBFilter(FLEXCAN_MAILBOX mb_num, uint32_t id1, uint32_t id2, uint32_t id3, uint32_t id4, uint32_t id5) { + if ( mb_num < mailboxOffset() || mb_num >= FLEXCANb_MAXMB_SIZE(_bus) ) return 0; /* mailbox not available */ + if ( (FLEXCAN_get_code(FLEXCANb_MBn_CS(_bus, mb_num)) >> 3) ) return 0; /* exit on TX mailbox */ + uint32_t mask = ( !(FLEXCANb_MBn_CS(_bus, mb_num) & FLEXCAN_MB_CS_IDE) ) ? FLEXCAN_MB_ID_IDSTD(((id1 | id2 | id3 | id4 | id5) ^ (id1 & id2 & id3 & id4 & id5)) ^ 0x7FF) : ((((id1 | id2 | id3 | id4 | id5) ^ (id1 & id2 & id3 & id4 & id5)) ^ 0x1FFFFFFF) & 0x1FFFFFFF); + setMBFilterProcessing(mb_num,id1,mask); + filter_store(FLEXCAN_MULTI, mb_num, 5, id1, id2, id3, id4, id5); + return 1; +} + +FCTP_FUNC bool FCTP_OPT::setMBFilterRange(FLEXCAN_MAILBOX mb_num, uint32_t id1, uint32_t id2) { + if ( mb_num < mailboxOffset() || mb_num >= FLEXCANb_MAXMB_SIZE(_bus) ) return 0; /* mailbox not available */ + if ( (FLEXCAN_get_code(FLEXCANb_MBn_CS(_bus, mb_num)) >> 3) ) return 0; /* exit on TX mailbox */ + if ( id1 > id2 || ((id2 > id1) && (id2-id1>1000)) || !id1 || !id2 ) return 0; /* don't play around... */ + uint32_t stage1 = id1, stage2 = id1; + for ( uint32_t i = id1 + 1; i <= id2; i++ ) { + stage1 |= i; stage2 &= i; + } + uint32_t mask = ( !(FLEXCANb_MBn_CS(_bus, mb_num) & FLEXCAN_MB_CS_IDE) ) ? FLEXCAN_MB_ID_IDSTD( (stage1 ^ stage2) ^ 0x1FFFFFFF ) : (( (stage1 ^ stage2) ^ 0x1FFFFFFF ) & 0x1FFFFFFF); + setMBFilterProcessing(mb_num,id1,mask); + filter_store(FLEXCAN_RANGE, mb_num, 2, id1, id2, 0, 0, 0); + return 1; +} + +FCTP_FUNC int FCTP_OPT::readFIFO(CAN_message_t &msg) { + //delayMicroseconds(150); + if ( !(FLEXCANb_MCR(_bus) & FLEXCAN_MCR_FEN) ) return 0; /* FIFO is disabled */ + if ( !(FLEXCANb_MCR(_bus) & (1UL << 15)) ) { /* if DMA is not enabled, check interrupt flag, else continue. */ + if ( FLEXCANb_IMASK1(_bus) & FLEXCAN_IMASK1_BUF5M ) return 0; /* FIFO interrupt enabled, polling blocked */ + } + if ( FLEXCANb_IFLAG1(_bus) & FLEXCAN_IFLAG1_BUF5I ) { /* message available */ + volatile uint32_t *mbxAddr = &(*(volatile uint32_t*)(_bus + 0x80)); + uint32_t code = mbxAddr[0]; + msg.len = (code & 0xF0000) >> 16; + msg.flags.remote = (bool)(code & (1UL << 20)); + msg.flags.extended = (bool)(code & (1UL << 21)); + msg.timestamp = code & 0xFFFF; + msg.id = (mbxAddr[1] & 0x1FFFFFFF) >> ((msg.flags.extended) ? 0 : 18); + uint32_t data0 = mbxAddr[2]; + for ( int8_t d = 0; d < 4 ; d++ ) msg.buf[3 - d] = (uint8_t)(data0 >> (8 * d)); + uint32_t data1 = mbxAddr[3]; + for ( int8_t d = 0; d < 4 ; d++ ) msg.buf[7 - d] = (uint8_t)(data1 >> (8 * d)); + msg.bus = busNumber; + msg.idhit = code >> 23; + msg.mb = FIFO; /* store the mailbox the message came from (for callback reference) */ + if ( !(FLEXCANb_MCR(_bus) & (1UL << 15)) ) writeIFLAGBit(5); /* clear FIFO bit only, NOT FOR DMA USE! */ + frame_distribution(msg); + if ( fifo_filter_match(msg.id) ) return 1; + } + return 0; /* message not available */ +} + +FCTP_FUNC int FCTP_OPT::getFirstTxBox() { + for (uint8_t i = mailboxOffset(); i < FLEXCANb_MAXMB_SIZE(_bus); i++) { + if ( (FLEXCAN_get_code(FLEXCANb_MBn_CS(_bus, i)) >> 3) ) return i; // if TX + } + return -1; +} + +FCTP_FUNC int FCTP_OPT::read(CAN_message_t &msg) { + bool _random = random(0, 2); + if ( ( !_random ) && ( FLEXCANb_MCR(_bus) & FLEXCAN_MCR_FEN ) && + !( FLEXCANb_IMASK1(_bus) & FLEXCAN_IMASK1_BUF5M ) && + ( FLEXCANb_IFLAG1(_bus) & FLEXCAN_IFLAG1_BUF5I ) ) return readFIFO(msg); + return readMB(msg); +} + +FCTP_FUNC int FCTP_OPT::readMB(CAN_message_t &msg) { + uint64_t iflag = 0; + for ( uint8_t cycle_limit = 3, mailboxes = mailboxOffset(); mailbox_reader_increment <= FLEXCANb_MAXMB_SIZE(_bus); ++mailbox_reader_increment ) { + iflag = readIFLAG(); + if ( iflag && (mailbox_reader_increment >= (64 - __builtin_clzll(iflag))) ) { /* break from MSB's if unset, add 1 to prevent undefined behaviour in clz for 0 check */ + mailbox_reader_increment = mailboxOffset(); + if ( !--cycle_limit ) return 0; + } + if ( FLEXCANb_MCR(_bus) & FLEXCAN_MCR_FEN ) { /* FIFO is enabled, get only remaining RX (if any) */ + if ( mailbox_reader_increment < mailboxes ) mailbox_reader_increment = mailboxes - 1; /* go back to position end of fifo+filter region */ + } + if ( mailbox_reader_increment >= FLEXCANb_MAXMB_SIZE(_bus) ) { + mailbox_reader_increment = mailboxOffset(); + if ( !--cycle_limit ) return 0; + } + volatile uint32_t *mbxAddr = &(*(volatile uint32_t*)(_bus + 0x80 + (mailbox_reader_increment * 0x10))); + if ((readIMASK() & (1ULL << mailbox_reader_increment))) continue; /* don't read interrupt enabled mailboxes */ + uint32_t code = mbxAddr[0]; + if ( (FLEXCAN_get_code(code) >> 3) ) continue; /* skip TX mailboxes */ + //if (!(code & 0x600000) && !(iflag & (1ULL << mailbox_reader_increment))) continue; /* don't read unflagged mailboxes, errata: extended mailboxes iflags do not work in poll mode, must check CS field */ + if ( ( FLEXCAN_get_code(code) == FLEXCAN_MB_CODE_RX_FULL ) || + ( FLEXCAN_get_code(code) == FLEXCAN_MB_CODE_RX_OVERRUN ) ) { + msg.flags.remote = (bool)(code & (1UL << 20)); + msg.flags.extended = (bool)(code & (1UL << 21)); + msg.id = (mbxAddr[1] & 0x1FFFFFFF) >> ((msg.flags.extended) ? 0 : 18); + if ( FLEXCAN_get_code(code) == FLEXCAN_MB_CODE_RX_OVERRUN ) msg.flags.overrun = 1; + msg.len = (code & 0xF0000) >> 16; + msg.mb = mailbox_reader_increment++; + msg.timestamp = code & 0xFFFF; + msg.bus = busNumber; + for ( uint8_t i = 0; i < (8 >> 2); i++ ) for ( int8_t d = 0; d < 4 ; d++ ) msg.buf[(4 * i) + 3 - d] = (uint8_t)(mbxAddr[2 + i] >> (8 * d)); + mbxAddr[0] = FLEXCAN_MB_CS_CODE(FLEXCAN_MB_CODE_RX_EMPTY) | ((msg.flags.extended) ? (FLEXCAN_MB_CS_SRR | FLEXCAN_MB_CS_IDE) : 0); + (void)FLEXCANb_TIMER(_bus); + writeIFLAGBit(msg.mb); + frame_distribution(msg); + if ( filter_match((FLEXCAN_MAILBOX)msg.mb, msg.id) ) return 1; + } + } + return 0; /* no messages available */ +} + +FCTP_FUNC bool FCTP_OPT::struct2queueTx(const CAN_message_t &msg) { + if (FLEXCANb_ESR1(_bus) & 0x20) return -2; + if ( txBuffer.size() == txBuffer.capacity() ) return 0; /* no queues available */ + uint8_t buf[sizeof(CAN_message_t)]; + memmove(buf, &msg, sizeof(msg)); + txBuffer.push_back(buf, sizeof(CAN_message_t)); + return -1; /* transmit entry failed, no mailboxes available, queued */ +} + +FCTP_FUNC int FCTP_OPT::write(FLEXCAN_MAILBOX mb_num, const CAN_message_t &msg) { + if ( mb_num < mailboxOffset() ) return 0; /* FIFO doesn't transmit */ + volatile uint32_t *mbxAddr = &(*(volatile uint32_t*)(_bus + 0x80 + (mb_num * 0x10))); + if ( !((FLEXCAN_get_code(mbxAddr[0])) >> 3) ) return 0; /* not a transmit mailbox */ + if ( msg.seq ) { + int first_tx_mb = getFirstTxBox(); + if ( FLEXCAN_get_code(FLEXCANb_MBn_CS(_bus, first_tx_mb)) == FLEXCAN_MB_CODE_TX_INACTIVE ) { + writeTxMailbox(first_tx_mb, msg); + return 1; /* transmit entry accepted */ + } + else { + CAN_message_t msg_copy = msg; + msg_copy.mb = first_tx_mb; + return struct2queueTx(msg_copy); /* queue if no mailboxes found */ + } + } + if ( FLEXCAN_get_code(mbxAddr[0]) == FLEXCAN_MB_CODE_TX_INACTIVE ) { + writeTxMailbox(mb_num, msg); + return 1; + } + CAN_message_t msg_copy = msg; + msg_copy.mb = mb_num; + return struct2queueTx(msg_copy); /* queue if no mailboxes found */ +} + +FCTP_FUNC int FCTP_OPT::write(const CAN_message_t &msg) { + if ( msg.seq ) { + int first_tx_mb = getFirstTxBox(); + if ( FLEXCAN_get_code(FLEXCANb_MBn_CS(_bus, first_tx_mb)) == FLEXCAN_MB_CODE_TX_INACTIVE ) { + writeTxMailbox(first_tx_mb, msg); + return 1; /* transmit entry accepted */ + } + else { + CAN_message_t msg_copy = msg; + msg_copy.mb = first_tx_mb; + return struct2queueTx(msg_copy); /* queue if no mailboxes found */ + } + } + for (uint8_t i = mailboxOffset(); i < FLEXCANb_MAXMB_SIZE(_bus); i++) { + if ( FLEXCAN_get_code(FLEXCANb_MBn_CS(_bus, i)) == FLEXCAN_MB_CODE_TX_INACTIVE ) { + writeTxMailbox(i, msg); + return 1; /* transmit entry accepted */ + } + } + CAN_message_t msg_copy = msg; + msg_copy.mb = -1; + return struct2queueTx(msg_copy); /* queue if no mailboxes found */ +} + +FCTP_FUNC void FCTP_OPT::onReceive(const FLEXCAN_MAILBOX &mb_num, _MB_ptr handler) { + if ( FIFO == mb_num ) { + _mbHandlers[0] = handler; + return; + } + _mbHandlers[mb_num] = handler; +} + +FCTP_FUNC void FCTP_OPT::onReceive(_MB_ptr handler) { + _mainHandler = handler; +} + +FCTP_FUNC void FCTP_OPT::onTransmit(const FLEXCAN_MAILBOX &mb_num, _MB_ptr handler) { + if ( FIFO == mb_num ) { + _mbTxHandlers[0] = handler; + return; + } + _mbTxHandlers[mb_num] = handler; +} + +FCTP_FUNC void FCTP_OPT::onTransmit(_MB_ptr handler) { + _mainTxHandler = handler; +} + +FCTP_FUNC uint64_t FCTP_OPT::events() { + if ( !isEventsUsed ) isEventsUsed = 1; + if ( rxBuffer.size() ) { + CAN_message_t frame; + uint8_t buf[sizeof(CAN_message_t)]; + rxBuffer.pop_front(buf, sizeof(CAN_message_t)); + memmove(&frame, buf, sizeof(frame)); + mbCallbacks((FLEXCAN_MAILBOX)frame.mb, frame); + } + NVIC_DISABLE_IRQ(nvicIrq); + if ( txBuffer.size() ) { + CAN_message_t frame; + uint8_t buf[sizeof(CAN_message_t)]; + txBuffer.peek_front(buf, sizeof(CAN_message_t)); + memmove(&frame, buf, sizeof(frame)); + if ( frame.mb == -1 ) { + for (uint8_t i = mailboxOffset(); i < FLEXCANb_MAXMB_SIZE(_bus); i++) { + if ( FLEXCAN_get_code(FLEXCANb_MBn_CS(_bus, i)) == FLEXCAN_MB_CODE_TX_INACTIVE ) { + //Serial.print("DBG NORM: "); Serial.println(frame.mb); + writeTxMailbox(i, frame); + txBuffer.pop_front(); + } + } + } + else if ( FLEXCAN_get_code(FLEXCANb_MBn_CS(_bus, frame.mb)) == FLEXCAN_MB_CODE_TX_INACTIVE ) { + //Serial.print("DBG SEQ: "); Serial.println(frame.mb); + writeTxMailbox(frame.mb, frame); + txBuffer.pop_front(); + } + } + NVIC_ENABLE_IRQ(nvicIrq); + return (uint64_t)(rxBuffer.size() << 12) | txBuffer.size(); +} + +#if defined(__IMXRT1062__) +static void flexcan_isr_can1() { + if ( _CAN1 ) _CAN1->flexcan_interrupt(); +} + +static void flexcan_isr_can2() { + if ( _CAN2 ) _CAN2->flexcan_interrupt(); +} + +static void flexcan_isr_can3() { + if ( _CAN3 ) _CAN3->flexcan_interrupt(); +} +#endif +#if defined(__MK20DX256__) || defined(__MK64FX512__) +static void flexcan_isr_can0() { + if ( _CAN0 ) _CAN0->flexcan_interrupt(); +} +#endif +#if defined(__MK66FX1M0__) +static void flexcan_isr_can0() { + if ( _CAN0 ) _CAN0->flexcan_interrupt(); +} +static void flexcan_isr_can1() { + if ( _CAN1 ) _CAN1->flexcan_interrupt(); +} +#endif + + +FCTP_FUNC void FCTP_OPT::mbCallbacks(const FLEXCAN_MAILBOX &mb_num, const CAN_message_t &msg) { + if ( mb_num == FIFO ) { + if ( _mbHandlers[0] ) _mbHandlers[0](msg); + if ( _mainHandler ) _mainHandler(msg); + return; + } + if ( _mbHandlers[mb_num] ) _mbHandlers[mb_num](msg); + if ( _mainHandler ) _mainHandler(msg); +} + +FCTP_FUNC void FCTP_OPT::struct2queueRx(const CAN_message_t &msg) { + CANListener *thisListener; + CAN_message_t cl = msg; + for (uint8_t listenerPos = 0; listenerPos < SIZE_LISTENERS; listenerPos++) { + thisListener = listener[listenerPos]; + if (thisListener != nullptr) { + if (thisListener->callbacksActive & (1UL << cl.mb)) thisListener->frameHandler (cl, cl.mb, cl.bus); + if (thisListener->generalCallbackActive) thisListener->frameHandler (cl, -1, cl.bus); + } + } + if ( !isEventsUsed ) { + mbCallbacks((FLEXCAN_MAILBOX)msg.mb, msg); + return; + } + uint8_t buf[sizeof(CAN_message_t)]; + memmove(buf, &msg, sizeof(msg)); + rxBuffer.push_back(buf, sizeof(CAN_message_t)); +} + +FCTP_FUNC void FCTP_OPT::flexcan_interrupt() { + CAN_message_t msg; // setup a temporary storage buffer + uint64_t imask = readIMASK(), iflag = readIFLAG(); + + if ( !(FLEXCANb_MCR(_bus) & (1UL << 15)) ) { /* if DMA is disabled, ONLY THEN you can handle FIFO in ISR */ + if ( (FLEXCANb_MCR(_bus) & FLEXCAN_MCR_FEN) && (imask & FLEXCAN_IMASK1_BUF5M) && (iflag & FLEXCAN_IFLAG1_BUF5I) ) { /* FIFO is enabled, capture frames if triggered */ + volatile uint32_t *mbxAddr = &(*(volatile uint32_t*)(_bus + 0x80 + (0 * 0x10))); + uint32_t code = mbxAddr[0]; + msg.len = (code & 0xF0000) >> 16; + msg.flags.remote = (bool)(code & (1UL << 20)); + msg.flags.extended = (bool)(code & (1UL << 21)); + msg.timestamp = code & 0xFFFF; + msg.id = (mbxAddr[1] & 0x1FFFFFFF) >> ((msg.flags.extended) ? 0 : 18); + msg.idhit = code >> 23; + for ( uint8_t i = 0; i < (8 >> 2); i++ ) for ( int8_t d = 0; d < 4 ; d++ ) msg.buf[(4 * i) + 3 - d] = (uint8_t)(mbxAddr[2 + i] >> (8 * d)); + msg.bus = busNumber; + msg.mb = FIFO; /* store the mailbox the message came from (for callback reference) */ + (void)FLEXCANb_TIMER(_bus); + writeIFLAGBit(5); /* clear FIFO bit only! */ + if ( iflag & FLEXCAN_IFLAG1_BUF6I ) writeIFLAGBit(6); /* clear FIFO bit only! */ + if ( iflag & FLEXCAN_IFLAG1_BUF7I ) writeIFLAGBit(7); /* clear FIFO bit only! */ + frame_distribution(msg); + ext_output1(msg); + ext_output2(msg); + ext_output3(msg); + if (fifo_filter_match(msg.id)) struct2queueRx(msg); + } + } + + uint8_t exit_point = 64 - __builtin_clzll(iflag | 1); /* break from MSB's if unset, add 1 to prevent undefined behaviour in clz for 0 check */ + for ( uint8_t mb_num = mailboxOffset(); mb_num < FLEXCANb_MAXMB_SIZE(_bus); mb_num++ ) { + if ( mb_num >= exit_point ) break; /* early exit from higher unflagged mailboxes */ + if (!(imask & (1ULL << mb_num))) continue; /* don't read non-interrupt mailboxes */ + if (!(iflag & (1ULL << mb_num))) continue; /* don't read unflagged mailboxes */ + volatile uint32_t *mbxAddr = &(*(volatile uint32_t*)(_bus + 0x80 + (mb_num * 0x10))); + uint32_t code = mbxAddr[0]; + if ( ( FLEXCAN_get_code(code) == FLEXCAN_MB_CODE_RX_FULL ) || + ( FLEXCAN_get_code(code) == FLEXCAN_MB_CODE_RX_OVERRUN ) ) { + msg.flags.extended = (bool)(code & (1UL << 21)); + msg.id = (mbxAddr[1] & 0x1FFFFFFF) >> ((msg.flags.extended) ? 0 : 18); + if ( FLEXCAN_get_code(code) == FLEXCAN_MB_CODE_RX_OVERRUN ) msg.flags.overrun = 1; + msg.len = (code & 0xF0000) >> 16; + msg.mb = mb_num; + msg.timestamp = code & 0xFFFF; + msg.bus = busNumber; + for ( uint8_t i = 0; i < (8 >> 2); i++ ) for ( int8_t d = 0; d < 4 ; d++ ) msg.buf[(4 * i) + 3 - d] = (uint8_t)(mbxAddr[2 + i] >> (8 * d)); + mbxAddr[0] = FLEXCAN_MB_CS_CODE(FLEXCAN_MB_CODE_RX_EMPTY) | ((msg.flags.extended) ? (FLEXCAN_MB_CS_SRR | FLEXCAN_MB_CS_IDE) : 0); + (void)FLEXCANb_TIMER(_bus); + writeIFLAGBit(mb_num); + if ( filter_match((FLEXCAN_MAILBOX)mb_num, msg.id) ) struct2queueRx(msg); /* store frame in queue */ + frame_distribution(msg); + ext_output1(msg); + ext_output2(msg); + ext_output3(msg); + } + + else if ( FLEXCAN_get_code(code) == FLEXCAN_MB_CODE_RX_EMPTY ) { + /* there are no flags for EMPTY reception boxes, however, when sending remote + frames, the mailboxes switch to RX_EMPTY and trigger the flag */ + if (!(iflag & (1ULL << mb_num))) continue; /* only process the flagged RX_EMPTY mailboxes */ + + msg.flags.extended = (bool)(code & (1UL << 21)); + msg.id = (mbxAddr[1] & 0x1FFFFFFF) >> ((msg.flags.extended) ? 0 : 18); + if ( FLEXCAN_get_code(code) == FLEXCAN_MB_CODE_RX_OVERRUN ) msg.flags.overrun = 1; + msg.len = (code & 0xF0000) >> 16; + msg.mb = mb_num; + msg.timestamp = code & 0xFFFF; + msg.bus = busNumber; + for ( uint8_t i = 0; i < (8 >> 2); i++ ) for ( int8_t d = 0; d < 4 ; d++ ) msg.buf[(4 * i) + 3 - d] = (uint8_t)(mbxAddr[2 + i] >> (8 * d)); + if ( mb_num == FIFO ) { + if ( _mbTxHandlers[0] ) _mbTxHandlers[0](msg); + if ( _mainTxHandler ) _mainTxHandler(msg); + } + else { + if ( _mbTxHandlers[mb_num] ) _mbTxHandlers[mb_num](msg); + if ( _mainTxHandler ) _mainTxHandler(msg); + } + + if ( txBuffer.size() ) { + CAN_message_t frame; + uint8_t buf[sizeof(CAN_message_t)]; + txBuffer.peek_front(buf, sizeof(CAN_message_t)); + memmove(&frame, buf, sizeof(frame)); + if ( frame.mb == -1 ) { + writeTxMailbox(mb_num, frame); + txBuffer.pop_front(); + } + else if ( frame.mb == mb_num ) { + writeTxMailbox(frame.mb, frame); + txBuffer.pop_front(); + } + } + else { + writeIFLAGBit(mb_num); /* just clear IFLAG if no TX queues exist */ + mbxAddr[0] = FLEXCAN_MB_CS_CODE(FLEXCAN_MB_CODE_TX_INACTIVE); /* set it back to a TX mailbox */ + } + } + + else if ( FLEXCAN_get_code(code) == FLEXCAN_MB_CODE_TX_INACTIVE ) { + msg.flags.extended = (bool)(code & (1UL << 21)); + msg.id = (mbxAddr[1] & 0x1FFFFFFF) >> ((msg.flags.extended) ? 0 : 18); + if ( FLEXCAN_get_code(code) == FLEXCAN_MB_CODE_RX_OVERRUN ) msg.flags.overrun = 1; + msg.len = (code & 0xF0000) >> 16; + msg.mb = mb_num; + msg.timestamp = code & 0xFFFF; + msg.bus = busNumber; + for ( uint8_t i = 0; i < (8 >> 2); i++ ) for ( int8_t d = 0; d < 4 ; d++ ) msg.buf[(4 * i) + 3 - d] = (uint8_t)(mbxAddr[2 + i] >> (8 * d)); + if ( mb_num == FIFO ) { + if ( _mbTxHandlers[0] ) _mbTxHandlers[0](msg); + if ( _mainTxHandler ) _mainTxHandler(msg); + } + else { + if ( _mbTxHandlers[mb_num] ) _mbTxHandlers[mb_num](msg); + if ( _mainTxHandler ) _mainTxHandler(msg); + } + + if ( txBuffer.size() ) { + CAN_message_t frame; + uint8_t buf[sizeof(CAN_message_t)]; + txBuffer.peek_front(buf, sizeof(CAN_message_t)); + memmove(&frame, buf, sizeof(frame)); + if ( frame.mb == -1 ) { + writeTxMailbox(mb_num, frame); + txBuffer.pop_front(); + } + else if ( frame.mb == mb_num ) { + writeTxMailbox(frame.mb, frame); + txBuffer.pop_front(); + } + } + else { + writeIFLAGBit(mb_num); /* just clear IFLAG if no TX queues exist */ + } + } + } + + uint32_t esr1 = FLEXCANb_ESR1(_bus); + static uint32_t last_esr1 = 0; + if ( (last_esr1 & 0x7FFBF) != (esr1 & 0x7FFBF) ) { + if ( busESR1.size() < busESR1.capacity() ) { + busESR1.write(esr1); + busECR.write(FLEXCANb_ECR(_bus)); + last_esr1 = esr1; + } + } + FLEXCANb_ESR1(_bus) |= esr1; + + asm volatile ("dsb"); +} + +FCTP_FUNC bool FCTP_OPT::error(CAN_error_t &error, bool printDetails) { + if ( !busESR1.size() ) return 0; + NVIC_DISABLE_IRQ(nvicIrq); + error.ESR1 = busESR1.read(); + error.ECR = busECR.read(); + + if ( (error.ESR1 & 0x400C8) == 0x40080 ) strncpy((char*)error.state, "Idle", (sizeof(error.state) - 1)); + else if ( (error.ESR1 & 0x400C8) == 0x0 ) strncpy((char*)error.state, "Not synchronized to CAN bus", (sizeof(error.state) - 1)); + else if ( (error.ESR1 & 0x400C8) == 0x40040 ) strncpy((char*)error.state, "Transmitting", (sizeof(error.state) - 1)); + else if ( (error.ESR1 & 0x400C8) == 0x40008 ) strncpy((char*)error.state, "Receiving", (sizeof(error.state) - 1)); + + error.BIT1_ERR = (error.ESR1 & (1UL << 15)) ? 1 : 0; + error.BIT0_ERR = (error.ESR1 & (1UL << 14)) ? 1 : 0; + error.ACK_ERR = (error.ESR1 & (1UL << 13)) ? 1 : 0; + error.CRC_ERR = (error.ESR1 & (1UL << 12)) ? 1 : 0; + error.FRM_ERR = (error.ESR1 & (1UL << 11)) ? 1 : 0; + error.STF_ERR = (error.ESR1 & (1UL << 10)) ? 1 : 0; + error.TX_WRN = (error.ESR1 & (1UL << 9)) ? 1 : 0; + error.RX_WRN = (error.ESR1 & (1UL << 8)) ? 1 : 0; + + if ( (error.ESR1 & 0x30) == 0x0 ) strncpy((char*)error.FLT_CONF, "Error Active", (sizeof(error.FLT_CONF) - 1)); + else if ( (error.ESR1 & 0x30) == 0x1 ) strncpy((char*)error.FLT_CONF, "Error Passive", (sizeof(error.FLT_CONF) - 1)); + else strncpy((char*)error.FLT_CONF, "Bus off", (sizeof(error.FLT_CONF) - 1)); + + error.RX_ERR_COUNTER = (uint8_t)(error.ECR >> 8); + error.TX_ERR_COUNTER = (uint8_t)error.ECR; + + if ( printDetails ) printErrors(error); + NVIC_ENABLE_IRQ(nvicIrq); + return 1; +} + +FCTP_FUNC void FCTP_OPT::printErrors(const CAN_error_t &error) { + Serial.print("FlexCAN State: "); Serial.print((char*)error.state); + if ( error.BIT1_ERR ) Serial.print(", BIT1_ERR"); + if ( error.BIT0_ERR ) Serial.print(", BIT0_ERR"); + if ( error.ACK_ERR ) Serial.print(", ACK_ERR"); + if ( error.CRC_ERR ) Serial.print(", CRC_ERR"); + if ( error.FRM_ERR ) Serial.print(", FRM_ERR"); + if ( error.STF_ERR ) Serial.print(", STF_ERR"); + if ( error.RX_WRN ) Serial.printf(", RX_WRN: %d", error.RX_ERR_COUNTER); + if ( error.TX_WRN ) Serial.printf(", TX_WRN: %d", error.TX_ERR_COUNTER); + Serial.printf(", FLT_CONF: %s\n", (char*)error.FLT_CONF); +} + +FCTP_FUNC void FCTP_OPT::enableDMA(bool state) { /* only CAN3 supports this on 1062, untested */ + bool frz_flag_negate = !(FLEXCANb_MCR(_bus) & FLEXCAN_MCR_FRZ_ACK); + FLEXCAN_EnterFreezeMode(); + ( !state ) ? FLEXCANb_MCR(_bus) &= ~0x8000 : FLEXCANb_MCR(_bus) |= 0x8000; + if ( frz_flag_negate ) FLEXCAN_ExitFreezeMode(); +} + +FCTP_FUNC uint8_t FCTP_OPT::setRFFN(FLEXCAN_RFFN_TABLE rffn) { + bool frz_flag_negate = !(FLEXCANb_MCR(_bus) & FLEXCAN_MCR_FRZ_ACK); + FLEXCAN_EnterFreezeMode(); + FLEXCAN_set_rffn(FLEXCANb_CTRL2(_bus), rffn); + if ( frz_flag_negate ) FLEXCAN_ExitFreezeMode(); + uint32_t remaining_mailboxes = FLEXCANb_MAXMB_SIZE(_bus) - 6 /* MAXMB - FIFO */ - ((((FLEXCANb_CTRL2(_bus) >> FLEXCAN_CTRL2_RFFN_BIT_NO) & 0xF) + 1) * 2); + if ( FLEXCANb_MAXMB_SIZE(_bus) < (6 + ((((FLEXCANb_CTRL2(_bus) >> FLEXCAN_CTRL2_RFFN_BIT_NO) & 0xF) + 1) * 2))) remaining_mailboxes = 0; + return constrain((uint8_t)(FLEXCANb_MAXMB_SIZE(_bus) - remaining_mailboxes), 0, 32); +} + +FCTP_FUNC void FCTP_OPT::setFIFOFilter(const FLEXCAN_FLTEN &input) { + if ( !(FLEXCANb_MCR(_bus) & FLEXCAN_MCR_FEN )) return; /* FIFO not enabled. */ + uint8_t max_fifo_filters = (((FLEXCANb_CTRL2(_bus) >> FLEXCAN_CTRL2_RFFN_BIT_NO) & 0xF) + 1) * 8; // 8->128 + bool frz_flag_negate = !(FLEXCANb_MCR(_bus) & FLEXCAN_MCR_FRZ_ACK); + FLEXCAN_EnterFreezeMode(); + for (uint8_t i = 0; i < max_fifo_filters; i++) { /* block all ID's so filtering could be applied. */ + if ( input == REJECT_ALL ) { + if ( ((FLEXCANb_MCR(_bus) & FLEXCAN_MCR_IDAM_MASK) >> FLEXCAN_MCR_IDAM_BIT_NO) == 0 ) { /* If Table A is chosen for FIFO */ + FLEXCANb_IDFLT_TAB(_bus, i) = 0xFFFFFFFF; /* reset id */ + /* individual masks (RXIMR) will just cover Rx FIFO filters in 0-31 range, and filters 32-127 + will use RXFGMASK. */ + if ( i < constrain(mailboxOffset(), 0, 32) ) FLEXCANb_RXIMR(_bus, i) = 0x3FFFFFFF; // (RXIMR) /* block all id's (0-31) */ + FLEXCANb_RXFGMASK(_bus) = 0x3FFFFFFF; + } + else if ( ((FLEXCANb_MCR(_bus) & FLEXCAN_MCR_IDAM_MASK) >> FLEXCAN_MCR_IDAM_BIT_NO) == 1 ) { /* If Table B is chosen for FIFO */ + FLEXCANb_IDFLT_TAB(_bus, i) = 0xFFFFFFFF; /* reset id */ + if ( i < constrain(mailboxOffset(), 0, 32) ) FLEXCANb_RXIMR(_bus, i) = 0x7FFF7FFF; // (RXIMR) /* block all id's (0-31) */ + FLEXCANb_RXFGMASK(_bus) = 0x7FFF7FFF; + } + else if ( ((FLEXCANb_MCR(_bus) & FLEXCAN_MCR_IDAM_MASK) >> FLEXCAN_MCR_IDAM_BIT_NO) == 2 ) { /* If Table C is chosen for FIFO */ + /* TO BE DONE */ //FLEXCANb_IDFLT_TAB(_bus, i) = 0x6E6E6E6E; /* reset id */ + //FLEXCANb_RXIMR(_bus, i) = 0xFFFFFFFF; // (RXIMR) /* block all id's */ + } + } + else if ( input == ACCEPT_ALL ) { + FLEXCANb_IDFLT_TAB(_bus, i) = 0; /* reset id */ + if ( i < constrain(mailboxOffset(), 0, 32) ) FLEXCANb_RXIMR(_bus, i) = 0; // (RXIMR) /* allow all id's */ + FLEXCANb_RXFGMASK(_bus) = 0; /* for masks above IDF 0->31, global is used for rest) */ + } + } + if ( frz_flag_negate ) FLEXCAN_ExitFreezeMode(); +} + +FCTP_FUNC bool FCTP_OPT::setFIFOManualFilter(uint8_t filter, uint32_t id1, uint32_t mask, const FLEXCAN_IDE &ide, const FLEXCAN_IDE &remote) { + if ( !(FLEXCANb_MCR(_bus) & FLEXCAN_MCR_FEN )) return 0; /* FIFO not enabled. */ + uint8_t max_fifo_filters = (((FLEXCANb_CTRL2(_bus) >> FLEXCAN_CTRL2_RFFN_BIT_NO) & 0xF) + 1) * 8; // 8->128 + if ( filter >= max_fifo_filters ) return 0; + bool frz_flag_negate = !(FLEXCANb_MCR(_bus) & FLEXCAN_MCR_FRZ_ACK); + FLEXCAN_EnterFreezeMode(); + /* ##################################### TABLE A ###################################### */ + if ( ((FLEXCANb_MCR(_bus) & FLEXCAN_MCR_IDAM_MASK) >> FLEXCAN_MCR_IDAM_BIT_NO) == 0 ) { + if (ide != EXT) { + mask = mask << 19 | 0xC0000001; + } else { + mask = mask << 1 | 0xC0000001; + } + FLEXCANb_IDFLT_TAB(_bus, filter) = ((ide == EXT ? 1 : 0) << 30) | ((remote == RTR ? 1 : 0) << 31) | + ((ide == EXT ? ((id1 & FLEXCAN_MB_ID_EXT_MASK) << 1) : (FLEXCAN_MB_ID_IDSTD(id1) << 1))); + if ( filter < constrain(mailboxOffset(), 0, 32) ) FLEXCANb_RXIMR(_bus, filter) = mask;// | ((filter < (max_fifo_filters / 2)) ? 0 : (1UL << 30)); // (RXIMR) + FLEXCANb_RXFGMASK(_bus) = mask;//0x3FFFFFFF; /* enforce it for blocks 32->127, single IDs */ + fifo_filter_table[filter][0] = ( ((ide == EXT) ? 1UL : 0UL) << 16); /* extended flag check */ + fifo_filter_store(FLEXCAN_MULTI, filter, 1, id1, 0, 0, 0, 0); + } + /* #################################################################################### */ + /* ##################################### TABLE B ###################################### */ + if ( ((FLEXCANb_MCR(_bus) & FLEXCAN_MCR_IDAM_MASK) >> FLEXCAN_MCR_IDAM_BIT_NO) == 1 ) { + return false; + } + + /* #################################################################################### */ + if ( frz_flag_negate ) FLEXCAN_ExitFreezeMode(); + return 1; +} + +FCTP_FUNC bool FCTP_OPT::setFIFOFilter(uint8_t filter, uint32_t id1, const FLEXCAN_IDE &ide, const FLEXCAN_IDE &remote) { + if ( !(FLEXCANb_MCR(_bus) & FLEXCAN_MCR_FEN )) return 0; /* FIFO not enabled. */ + uint8_t max_fifo_filters = (((FLEXCANb_CTRL2(_bus) >> FLEXCAN_CTRL2_RFFN_BIT_NO) & 0xF) + 1) * 8; // 8->128 + if ( filter >= max_fifo_filters ) return 0; + bool frz_flag_negate = !(FLEXCANb_MCR(_bus) & FLEXCAN_MCR_FRZ_ACK); + FLEXCAN_EnterFreezeMode(); + /* ##################################### TABLE A ###################################### */ + if ( ((FLEXCANb_MCR(_bus) & FLEXCAN_MCR_IDAM_MASK) >> FLEXCAN_MCR_IDAM_BIT_NO) == 0 ) { + uint32_t mask = ( ide != EXT ) ? ((((id1) ^ (id1)) ^ 0x7FF) << 19 ) | 0xC0000001 : ((((id1) ^ (id1)) ^ 0x1FFFFFFF) << 1 ) | 0xC0000001; + FLEXCANb_IDFLT_TAB(_bus, filter) = ((ide == EXT ? 1 : 0) << 30) | ((remote == RTR ? 1 : 0) << 31) | + ((ide == EXT ? ((id1 & FLEXCAN_MB_ID_EXT_MASK) << 1) : (FLEXCAN_MB_ID_IDSTD(id1) << 1))); + if ( filter < constrain(mailboxOffset(), 0, 32) ) FLEXCANb_RXIMR(_bus, filter) = mask;// | ((filter < (max_fifo_filters / 2)) ? 0 : (1UL << 30)); // (RXIMR) + FLEXCANb_RXFGMASK(_bus) = 0x3FFFFFFF; /* enforce it for blocks 32->127, single IDs */ + + fifo_filter_table[filter][0] = ( ((ide == EXT) ? 1UL : 0UL) << 16); /* extended flag check */ + fifo_filter_store(FLEXCAN_MULTI, filter, 1, id1, 0, 0, 0, 0); + } + /* #################################################################################### */ + /* ##################################### TABLE B ###################################### */ + if ( ((FLEXCANb_MCR(_bus) & FLEXCAN_MCR_IDAM_MASK) >> FLEXCAN_MCR_IDAM_BIT_NO) == 1 ) { + FLEXCANb_IDFLT_TAB(_bus, filter) = ((ide == EXT ? 1 : 0) << 30) | ((ide == EXT ? 1 : 0) << 14) | /* STD IDs / EXT IDs */ + ((remote == RTR ? 1 : 0) << 31) | ((remote == RTR ? 1 : 0) << 15) | /* remote frames */ + (ide == EXT ? ((id1 >> (29 - 14)) << 16) : ((id1 & 0x7FF) << 19)) | /* first ID is EXT or STD? */ + (ide == EXT ? ((id1 >> (29 - 14)) << 0) : ((id1 & 0x7FF) << 3)) ; /* second ID is EXT or STD? */ + uint32_t mask = ( ide != EXT ) ? ((((id1) ^ (id1)) ^ 0x7FF) << 19 ) | ((remote == RTR)?(1UL<<31):0UL) : ((((id1) ^ (id1)) ^ 0x1FFFFFFF) << 16 ) | ((remote == RTR)?(1UL<<31):0UL); + mask |= (( ide != EXT ) ? ((((id1) ^ (id1)) ^ 0x7FF) << 3 ) | ((remote == RTR)?(1UL<<15):0UL) : ((((id1) ^ (id1)) ^ 0x1FFFFFFF) << 0 ) | ((remote == RTR)?(1UL<<15):0UL) ) & 0xFFFF; + mask |= (1UL<<30) | (1UL<<14); + if ( filter < constrain(mailboxOffset(), 0, 32) ) FLEXCANb_RXIMR(_bus, filter) = mask; // (RXIMR) + FLEXCANb_RXFGMASK(_bus) = 0x7FFF7FFF; /* enforce it for blocks 32->127, single STD IDs / EXT partial matches */ + } + + /* #################################################################################### */ + if ( frz_flag_negate ) FLEXCAN_ExitFreezeMode(); + return 1; +} + +FCTP_FUNC void FCTP_OPT::fifo_filter_store(FLEXCAN_FILTER_TABLE type, uint8_t filter, uint32_t id_count, uint32_t id1, uint32_t id2, uint32_t id3, uint32_t id4, uint32_t id5) { + fifo_filter_table[filter][0] = (fifo_filter_table[filter][0] & 0xF0000) | filter; // first 7 bits reserved for fifo filter + fifo_filter_table[filter][0] |= (id_count << 7); // we store the quantity of ids after the fifo filter count + /* bit 16-19: extended ids */ + /* bit 28: filter enabled */ + fifo_filter_table[filter][0] |= (type << 29); // we reserve 3 upper bits for type + fifo_filter_table[filter][1] = id1; // id1 + fifo_filter_table[filter][2] = id2; // id2 + fifo_filter_table[filter][3] = id3; // id3 + fifo_filter_table[filter][4] = id4; // id4 + fifo_filter_table[filter][5] = id5; // id5 +} + +FCTP_FUNC void FCTP_OPT::enhanceFilter(FLEXCAN_MAILBOX mb_num) { + if ( mb_num == FIFO ) fifo_filter_table[0][0] |= (1UL << 28); /* enable fifo enhancement */ + else mb_filter_table[mb_num][0] |= (1UL << 28); /* enable mb enhancement */ +} + +FCTP_FUNC volatile bool FCTP_OPT::fifo_filter_match(uint32_t id) { + if ( !(fifo_filter_table[0][0] & 0x10000000) ) return 1; + uint8_t max_fifo_filters = (((FLEXCANb_CTRL2(_bus) >> FLEXCAN_CTRL2_RFFN_BIT_NO) & 0xF) + 1) * 8; // 8->128 + for (uint8_t mb_num = 0; mb_num < max_fifo_filters; mb_num++) { /* check fifo filters */ + if ( (fifo_filter_table[mb_num][0] >> 29) == FLEXCAN_MULTI ) { + for ( uint8_t i = 0; i < ((fifo_filter_table[mb_num][0] & 0x380) >> 7); i++) if ( id == fifo_filter_table[mb_num][i+1] ) return 1; + } + else if ( (fifo_filter_table[mb_num][0] >> 29) == FLEXCAN_RANGE ) { + if ( id >= fifo_filter_table[mb_num][1] && id <= fifo_filter_table[mb_num][2] ) return 1; + } + else if ( (fifo_filter_table[mb_num][0] >> 29) == FLEXCAN_USERMASK ) { + for ( uint8_t i = 1; i < ((fifo_filter_table[mb_num][0] & 0x380) >> 7) + 1; i++) { + if ( (id & fifo_filter_table[mb_num][5]) == (fifo_filter_table[mb_num][i] & fifo_filter_table[mb_num][5]) ) return 1; + } + } + } + return 0; +} + +FCTP_FUNC volatile bool FCTP_OPT::filter_match(FLEXCAN_MAILBOX mb_num, uint32_t id) { + if ( !(mb_filter_table[mb_num][0] & 0x10000000) ) return 1; + if ( (mb_filter_table[mb_num][0] >> 29) == FLEXCAN_MULTI ) { + for ( uint8_t i = 0; i < ((mb_filter_table[mb_num][0] & 0x380) >> 7); i++) if ( id == mb_filter_table[mb_num][i+1] ) return 1; + } + else if ( (mb_filter_table[mb_num][0] >> 29) == FLEXCAN_RANGE ) { + if ( id >= mb_filter_table[mb_num][1] && id <= mb_filter_table[mb_num][2] ) return 1; + } + else if ( (mb_filter_table[mb_num][0] >> 29) == FLEXCAN_USERMASK ) { + for ( uint8_t i = 1; i < ((mb_filter_table[mb_num][0] & 0x380) >> 7) + 1; i++) { + if ( (id & mb_filter_table[mb_num][5]) == (mb_filter_table[mb_num][i] & mb_filter_table[mb_num][5]) ) return 1; + } + } + return 0; +} + +FCTP_FUNC bool FCTP_OPT::setFIFOFilter(uint8_t filter, uint32_t id1, uint32_t id2, const FLEXCAN_IDE &ide, const FLEXCAN_IDE &remote) { + if ( !(FLEXCANb_MCR(_bus) & FLEXCAN_MCR_FEN )) return 0; /* FIFO not enabled. */ + if ( filter > 31 ) return 0; /* multi-id & ranges are not allowed */ + uint8_t max_fifo_filters = (((FLEXCANb_CTRL2(_bus) >> FLEXCAN_CTRL2_RFFN_BIT_NO) & 0xF) + 1) * 8; // 8->128 + if ( filter >= max_fifo_filters ) return 0; + bool frz_flag_negate = !(FLEXCANb_MCR(_bus) & FLEXCAN_MCR_FRZ_ACK); + FLEXCAN_EnterFreezeMode(); + /* ##################################### TABLE A ###################################### */ + if ( ((FLEXCANb_MCR(_bus) & FLEXCAN_MCR_IDAM_MASK) >> FLEXCAN_MCR_IDAM_BIT_NO) == 0 ) { + uint32_t mask = ( ide != EXT ) ? ((((id1 | id2) ^ (id1 & id2)) ^ 0x7FF) << 19 ) | 0xC0000001 : ((((id1 | id2) ^ (id1 & id2)) ^ 0x1FFFFFFF) << 1 ) | 0xC0000001; + FLEXCANb_RXIMR(_bus, filter) = mask; // (RXIMR) + FLEXCANb_IDFLT_TAB(_bus, filter) = ((ide == EXT ? 1 : 0) << 30) | ((remote == RTR ? 1 : 0) << 31) | + ((ide == EXT ? ((id1 & FLEXCAN_MB_ID_EXT_MASK) << 1) : (FLEXCAN_MB_ID_IDSTD(id1) << 1))); + + fifo_filter_table[filter][0] = ( ((ide == EXT) ? 1UL : 0UL) << 16); /* extended flag check */ + fifo_filter_store(FLEXCAN_MULTI, filter, 2, id1, id2, 0, 0, 0); + } + /* #################################################################################### */ + /* ##################################### TABLE B ###################################### */ + if ( ((FLEXCANb_MCR(_bus) & FLEXCAN_MCR_IDAM_MASK) >> FLEXCAN_MCR_IDAM_BIT_NO) == 1 ) { + FLEXCANb_IDFLT_TAB(_bus, filter) = ((ide == EXT ? 1 : 0) << 30) | ((ide == EXT ? 1 : 0) << 14) | /* STD IDs / EXT IDs */ + ((remote == RTR ? 1 : 0) << 31) | ((remote == RTR ? 1 : 0) << 15) | /* remote frames */ + (ide == EXT ? ((id1 >> (29 - 14)) << 16) : ((id1 & 0x7FF) << 19)) | /* first ID is EXT or STD? */ + (ide == EXT ? ((id2 >> (29 - 14)) << 0) : ((id2 & 0x7FF) << 3)) ; /* second ID is EXT or STD? */ + uint32_t mask = ( ide != EXT ) ? ((((id1) ^ (id1)) ^ 0x7FF) << 19 ) | ((remote == RTR)?(1UL<<31):0UL) : ((((id1) ^ (id1)) ^ 0x1FFFFFFF) << 16 ) | ((remote == RTR)?(1UL<<31):0UL); + mask |= (( ide != EXT ) ? ((((id2) ^ (id2)) ^ 0x7FF) << 3 ) | ((remote == RTR)?(1UL<<15):0UL) : ((((id2) ^ (id2)) ^ 0x1FFFFFFF) << 0 ) | ((remote == RTR)?(1UL<<15):0UL) ) & 0xFFFF; + mask |= (1UL<<30) | (1UL<<14); + FLEXCANb_RXIMR(_bus, filter) = mask; // (RXIMR) + } + /* #################################################################################### */ + if ( frz_flag_negate ) FLEXCAN_ExitFreezeMode(); + return 1; +} + +FCTP_FUNC bool FCTP_OPT::setFIFOFilterRange(uint8_t filter, uint32_t id1, uint32_t id2, const FLEXCAN_IDE &ide, const FLEXCAN_IDE &remote) { + if ( !(FLEXCANb_MCR(_bus) & FLEXCAN_MCR_FEN )) return 0; /* FIFO not enabled. */ + if ( filter > 31 ) return 0; /* multi-id & ranges are not allowed */ + uint8_t max_fifo_filters = (((FLEXCANb_CTRL2(_bus) >> FLEXCAN_CTRL2_RFFN_BIT_NO) & 0xF) + 1) * 8; // 8->128 + if ( filter >= max_fifo_filters ) return 0; + if ( id1 > id2 || ((id2 > id1) && (id2 - id1 > 1000)) || !id1 || !id2 ) return 0; /* don't play around... */ + bool frz_flag_negate = !(FLEXCANb_MCR(_bus) & FLEXCAN_MCR_FRZ_ACK); + FLEXCAN_EnterFreezeMode(); + /* ##################################### TABLE A ###################################### */ + if ( ((FLEXCANb_MCR(_bus) & FLEXCAN_MCR_IDAM_MASK) >> FLEXCAN_MCR_IDAM_BIT_NO) == 0 ) { + uint32_t stage1 = id1, stage2 = id1; + for ( uint32_t i = id1 + 1; i <= id2; i++ ) { + stage1 |= i; stage2 &= i; + } + uint32_t mask = ( ide != EXT ) ? (((stage1 ^ stage2) ^ 0x7FF) << 19) | 0xC0000001 : (((stage1 ^ stage2) ^ 0x1FFFFFFF) << 1) | 0xC0000001; + FLEXCANb_RXIMR(_bus, filter) = mask; // (RXIMR) + FLEXCANb_IDFLT_TAB(_bus, filter) = ((ide == EXT ? 1 : 0) << 30) | ((remote == RTR ? 1 : 0) << 31) | + ((ide == EXT ? ((id1 & FLEXCAN_MB_ID_EXT_MASK) << 1) : (FLEXCAN_MB_ID_IDSTD(id1) << 1))); + + fifo_filter_table[filter][0] = ( ((ide == EXT) ? 1UL : 0UL) << 16); /* extended flag check */ + fifo_filter_store(FLEXCAN_RANGE, filter, 2, id1, id2, 0, 0, 0); + } + /* #################################################################################### */ + if ( frz_flag_negate ) FLEXCAN_ExitFreezeMode(); + return 1; +} + +FCTP_FUNC bool FCTP_OPT::setFIFOFilter(uint8_t filter, uint32_t id1, const FLEXCAN_IDE &ide1, const FLEXCAN_IDE &remote1, uint32_t id2, const FLEXCAN_IDE &ide2, const FLEXCAN_IDE &remote2) { + if ( !(FLEXCANb_MCR(_bus) & FLEXCAN_MCR_FEN )) return 0; /* FIFO not enabled. */ + if ( filter > 31 ) return 0; /* multi-id & ranges are not allowed */ + uint8_t max_fifo_filters = (((FLEXCANb_CTRL2(_bus) >> FLEXCAN_CTRL2_RFFN_BIT_NO) & 0xF) + 1) * 8; // 8->128 + if ( filter >= max_fifo_filters ) return 0; + bool frz_flag_negate = !(FLEXCANb_MCR(_bus) & FLEXCAN_MCR_FRZ_ACK); + FLEXCAN_EnterFreezeMode(); + /* ##################################### TABLE B ###################################### */ + if ( ((FLEXCANb_MCR(_bus) & FLEXCAN_MCR_IDAM_MASK) >> FLEXCAN_MCR_IDAM_BIT_NO) == 1 ) { + FLEXCANb_IDFLT_TAB(_bus, filter) = ((ide1 == EXT ? 1 : 0) << 30) | ((ide2 == EXT ? 1 : 0) << 14) | /* STD IDs / EXT IDs */ + ((remote1 == RTR ? 1 : 0) << 31) | ((remote2 == RTR ? 1 : 0) << 15) | /* remote frames */ + (ide1 == EXT ? ((id1 >> (29 - 14)) << 16) : ((id1 & 0x7FF) << 19)) | /* first ID is EXT or STD? */ + (ide2 == EXT ? ((id2 >> (29 - 14)) << 0) : ((id2 & 0x7FF) << 3)) ; /* second ID is EXT or STD? */ + uint32_t mask = ( ide1 != EXT ) ? ((((id1) ^ (id1)) ^ 0x7FF) << 19 ) | ((remote1 == RTR)?(1UL<<31):0UL) : ((((id1) ^ (id1)) ^ 0x1FFFFFFF) << 16 ) | ((remote1 == RTR)?(1UL<<31):0UL); + mask |= (( ide2 != EXT ) ? ((((id2) ^ (id2)) ^ 0x7FF) << 3 ) | ((remote2 == RTR)?(1UL<<15):0UL) : ((((id2) ^ (id2)) ^ 0x1FFFFFFF) << 0 ) | ((remote2 == RTR)?(1UL<<15):0UL) ) & 0xFFFF; + mask |= (1UL<<30) | (1UL<<14); + FLEXCANb_RXIMR(_bus, filter) = mask; // (RXIMR) + } + /* #################################################################################### */ + if ( frz_flag_negate ) FLEXCAN_ExitFreezeMode(); + return 1; +} + +FCTP_FUNC bool FCTP_OPT::setFIFOFilter(uint8_t filter, uint32_t id1, uint32_t id2, const FLEXCAN_IDE &ide1, const FLEXCAN_IDE &remote1, uint32_t id3, uint32_t id4, const FLEXCAN_IDE &ide2, const FLEXCAN_IDE &remote2) { + if ( !(FLEXCANb_MCR(_bus) & FLEXCAN_MCR_FEN )) return 0; /* FIFO not enabled. */ + if ( filter > 31 ) return 0; /* multi-id & ranges are not allowed */ + uint8_t max_fifo_filters = (((FLEXCANb_CTRL2(_bus) >> FLEXCAN_CTRL2_RFFN_BIT_NO) & 0xF) + 1) * 8; // 8->128 + if ( filter >= max_fifo_filters ) return 0; + bool frz_flag_negate = !(FLEXCANb_MCR(_bus) & FLEXCAN_MCR_FRZ_ACK); + FLEXCAN_EnterFreezeMode(); + /* ##################################### TABLE B ###################################### */ + if ( ((FLEXCANb_MCR(_bus) & FLEXCAN_MCR_IDAM_MASK) >> FLEXCAN_MCR_IDAM_BIT_NO) == 1 ) { + uint32_t mask = ( ide1 != EXT ) ? ((((id1 | id2) ^ (id1 & id2)) ^ 0x7FF) << 19 ) | ((remote1 == RTR)?(1UL<<31):0UL) : ((((id1 | id2) ^ (id1 & id2)) ^ 0x1FFFFFFF) << 16 ) | ((remote1 == RTR)?(1UL<<31):0UL); + mask |= (( ide2 != EXT ) ? ((((id3 | id4) ^ (id3 & id4)) ^ 0x7FF) << 3 ) | ((remote2 == RTR)?(1UL<<15):0UL) : ((((id3 | id4) ^ (id3 & id4)) ^ 0x1FFFFFFF) << 0 ) | ((remote2 == RTR)?(1UL<<15):0UL) ) & 0xFFFF; + mask |= (1UL<<30) | (1UL<<14); + FLEXCANb_IDFLT_TAB(_bus, filter) = ((ide1 == EXT ? 1 : 0) << 30) | ((ide2 == EXT ? 1 : 0) << 14) | /* STD IDs / EXT IDs */ + ((remote1 == RTR ? 1 : 0) << 31) | ((remote2 == RTR ? 1 : 0) << 15) | /* remote frames */ + (ide1 == EXT ? ((id1 >> (29 - 14)) << 16) : ((id1 & 0x7FF) << 19)) | /* first ID is EXT or STD? */ + (ide2 == EXT ? ((id3 >> (29 - 14)) << 0 ) : ((id3 & 0x7FF) << 3 )) ; /* second ID is EXT or STD? */ + FLEXCANb_RXIMR(_bus, filter) = mask; // (RXIMR) + } + /* #################################################################################### */ + if ( frz_flag_negate ) FLEXCAN_ExitFreezeMode(); + return 1; +} + +FCTP_FUNC bool FCTP_OPT::setFIFOFilterRange(uint8_t filter, uint32_t id1, uint32_t id2, const FLEXCAN_IDE &ide1, const FLEXCAN_IDE &remote1, uint32_t id3, uint32_t id4, const FLEXCAN_IDE &ide2, const FLEXCAN_IDE &remote2) { + if ( !(FLEXCANb_MCR(_bus) & FLEXCAN_MCR_FEN )) return 0; /* FIFO not enabled. */ + if ( filter > 31 ) return 0; /* multi-id & ranges are not allowed */ + uint8_t max_fifo_filters = (((FLEXCANb_CTRL2(_bus) >> FLEXCAN_CTRL2_RFFN_BIT_NO) & 0xF) + 1) * 8; // 8->128 + if ( filter >= max_fifo_filters ) return 0; + if ( id1 > id2 || ((id2 > id1) && (id2 - id1 > 1000)) || !id1 || !id2 ) return 0; /* don't play around... */ + if ( id3 > id4 || ((id4 > id3) && (id4 - id3 > 1000)) || !id3 || !id4 ) return 0; /* don't play around... */ + bool frz_flag_negate = !(FLEXCANb_MCR(_bus) & FLEXCAN_MCR_FRZ_ACK); + FLEXCAN_EnterFreezeMode(); + /* ##################################### TABLE B ###################################### */ + if ( ((FLEXCANb_MCR(_bus) & FLEXCAN_MCR_IDAM_MASK) >> FLEXCAN_MCR_IDAM_BIT_NO) == 1 ) { + uint32_t stage1 = id1, stage2 = id1; + for ( uint32_t i = id1 + 1; i <= id2; i++ ) { + stage1 |= i; stage2 &= i; + } + uint32_t mask = ( ide1 != EXT ) ? ((((stage1 | stage2) ^ (stage1 & stage2)) ^ 0x7FF) << 19 ) | ((remote1 == RTR)?(1UL<<31):0UL) : ((((stage1 | stage2) ^ (stage1 & stage2)) ^ 0x1FFFFFFF) << 16 ) | ((remote1 == RTR)?(1UL<<31):0UL); + stage1 = stage2 = id3; + for ( uint32_t i = id3 + 1; i <= id4; i++ ) { + stage1 |= i; stage2 &= i; + } + mask |= (( ide2 != EXT ) ? ((((stage1 | stage2) ^ (stage1 & stage2)) ^ 0x7FF) << 3 ) | ((remote2 == RTR)?(1UL<<15):0UL) : ((((stage1 | stage2) ^ (stage1 & stage2)) ^ 0x1FFFFFFF) << 0 ) | ((remote2 == RTR)?(1UL<<15):0UL) ) & 0xFFFF; + mask |= (1UL<<30) | (1UL<<14); + FLEXCANb_IDFLT_TAB(_bus, filter) = ((ide1 == EXT ? 1 : 0) << 30) | ((ide2 == EXT ? 1 : 0) << 14) | /* STD IDs / EXT IDs */ + ((remote1 == RTR ? 1 : 0) << 31) | ((remote2 == RTR ? 1 : 0) << 15) | /* remote frames */ + (ide1 == EXT ? ((id1 >> (29 - 14)) << 16) : ((id1 & 0x7FF) << 19)) | /* first ID is EXT or STD? */ + (ide2 == EXT ? ((id3 >> (29 - 14)) << 0 ) : ((id3 & 0x7FF) << 3 )) ; /* second ID is EXT or STD? */ + FLEXCANb_RXIMR(_bus, filter) = mask; // (RXIMR) + } + /* #################################################################################### */ + if ( frz_flag_negate ) FLEXCAN_ExitFreezeMode(); + return 1; +} + +FCTP_FUNC void FCTP_OPT::setFIFOFilterTable(FLEXCAN_FIFOTABLE letter) { + bool frz_flag_negate = !(FLEXCANb_MCR(_bus) & FLEXCAN_MCR_FRZ_ACK); + FLEXCAN_EnterFreezeMode(); + FLEXCANb_MCR(_bus) = (FLEXCANb_MCR(_bus) & 0xFFFFFCFF) | FLEXCAN_MCR_IDAM(letter); + if ( ((FLEXCANb_MCR(_bus) & FLEXCAN_MCR_IDAM_MASK) >> FLEXCAN_MCR_IDAM_BIT_NO) == 0 ) FLEXCANb_RXFGMASK(_bus) = 0x3FFFFFFF; + else if ( ((FLEXCANb_MCR(_bus) & FLEXCAN_MCR_IDAM_MASK) >> FLEXCAN_MCR_IDAM_BIT_NO) == 1 ) FLEXCANb_RXFGMASK(_bus) = 0x7FFF7FFF; + if ( frz_flag_negate ) FLEXCAN_ExitFreezeMode(); +} + +FCTP_FUNC void FCTP_OPT::filter_store(FLEXCAN_FILTER_TABLE type, FLEXCAN_MAILBOX mb_num, uint32_t id_count, uint32_t id1, uint32_t id2, uint32_t id3, uint32_t id4, uint32_t id5) { + mb_filter_table[mb_num][0] = mb_num; // first 7 bits reserved for MB + mb_filter_table[mb_num][0] |= (id_count << 7); // we store the quantity of ids after the mailboxes + /* bit 28: filter enabled */ + mb_filter_table[mb_num][0] |= (type << 29); // we reserve 3 upper bits for type + volatile uint32_t *mbxAddr = &(*(volatile uint32_t*)(_bus + 0x80 + (mb_num * 0x10))); + mb_filter_table[mb_num][0] |= ( ((mbxAddr[0] & 0x600000) ? 1UL : 0UL) << 27); /* extended flag check */ + mb_filter_table[mb_num][1] = id1; // id1 + mb_filter_table[mb_num][2] = id2; // id2 + mb_filter_table[mb_num][3] = id3; // id3 + mb_filter_table[mb_num][4] = id4; // id4 + mb_filter_table[mb_num][5] = id5; // id5 +} + +FCTP_FUNC volatile void FCTP_OPT::frame_distribution(CAN_message_t &msg) { + if ( !distribution ) return; /* distribution not enabled */ + CAN_message_t frame = msg; + + if ( FLEXCANb_MCR(_bus) & FLEXCAN_MCR_FEN ) { + uint8_t max_fifo_filters = (((FLEXCANb_CTRL2(_bus) >> FLEXCAN_CTRL2_RFFN_BIT_NO) & 0xF) + 1) * 8; // 8->128 + for (uint8_t i = 0; i < max_fifo_filters; i++) { /* check fifo filters */ + if ( msg.mb == FIFO ) break; // don't distribute to fifo if fifo was the source + if ( !(fifo_filter_table[i][0] & 0xE0000000) ) continue; // skip unset filters + + if ( (fifo_filter_table[i][0] >> 29) == FLEXCAN_MULTI ) { + if ( (bool)(fifo_filter_table[i][0] & (1UL << 16)) != msg.flags.extended ) continue; /* extended flag check */ + for ( uint8_t p = 0; p < ((fifo_filter_table[i][0] & 0x380) >> 7); p++) { + if ( frame.id == fifo_filter_table[i][p+1] ) { + frame.mb = FIFO; + struct2queueRx(frame); + } + } + } + else if ( (fifo_filter_table[i][0] >> 29) == FLEXCAN_RANGE ) { + if ( (bool)(fifo_filter_table[i][0] & (1UL << 16)) != msg.flags.extended ) continue; /* extended flag check */ + if ( frame.id >= fifo_filter_table[i][1] && frame.id <= fifo_filter_table[i][2] ) { + frame.mb = FIFO; + struct2queueRx(frame); + } + } + else if ( (fifo_filter_table[i][0] >> 29) == FLEXCAN_USERMASK ) { + for ( uint8_t p = 1; p < ((fifo_filter_table[i][0] & 0x380) >> 7) + 1; p++) { + if ( (frame.id & fifo_filter_table[i][5]) == (fifo_filter_table[i][p] & fifo_filter_table[i][5]) ) { + frame.mb = FIFO; + frame.idhit = i; + struct2queueRx(frame); + } + } + } + } /* end of fifo scan */ + } /* end of fifo checking */ + + frame.idhit = 0; + + for ( uint8_t i = mailboxOffset(); i < FLEXCANb_MAXMB_SIZE(_bus); i++ ) { + if ( msg.mb == i ) continue; // don't distribute to same mailbox + if ( !(mb_filter_table[i][0] & 0xE0000000) ) continue; // skip unset filters + if ( (bool)(mb_filter_table[i][0] & (1UL << 27)) != msg.flags.extended ) continue; /* extended flag check */ + if ( (mb_filter_table[i][0] >> 29) == FLEXCAN_MULTI ) { + for ( uint8_t p = 0; p < ((mb_filter_table[i][0] & 0x380) >> 7); p++) { + if ( frame.id == mb_filter_table[i][p+1] ) { + frame.mb = i; + struct2queueRx(frame); + } + } + } + else if ( (mb_filter_table[i][0] >> 29) == FLEXCAN_RANGE ) { + if ( frame.id >= mb_filter_table[i][1] && frame.id <= mb_filter_table[i][2] ) { + frame.mb = i; + struct2queueRx(frame); + } + } + else if ( (mb_filter_table[i][0] >> 29) == FLEXCAN_USERMASK ) { + if ( filter_match((FLEXCAN_MAILBOX)i, frame.id) ) { + frame.mb = i; + struct2queueRx(frame); + } + } + } /* end of mb scan */ +} + +FCTP_FUNC void FCTP_OPT::enableLoopBack(bool yes) { + FLEXCAN_EnterFreezeMode(); + if ( yes ) { + FLEXCANb_MCR(_bus) &= ~(1UL << 17); + FLEXCANb_CTRL1(_bus) |= (1UL << 12); + } + else { + FLEXCANb_MCR(_bus) |= (1UL << 17); + FLEXCANb_CTRL1(_bus) &= ~(1UL << 12); + } + FLEXCAN_ExitFreezeMode(); +} + +FCTP_FUNC bool FCTP_OPT::attachObj (CANListener *listener) { + for (uint8_t i = 0; i < SIZE_LISTENERS; i++) { + if (this->listener[i] == nullptr) { + this->listener[i] = listener; + listener->callbacksActive = 0; + return true; + } + } + return false; +} + +FCTP_FUNC bool FCTP_OPT::detachObj (CANListener *listener) { + for (uint8_t i = 0; i < SIZE_LISTENERS; i++) { + if (this->listener[i] == listener) { + this->listener[i] = nullptr; + return true; + } + } + return false; +} + +extern void __attribute__((weak)) ext_output1(const CAN_message_t &msg); +extern void __attribute__((weak)) ext_output2(const CAN_message_t &msg); +extern void __attribute__((weak)) ext_output3(const CAN_message_t &msg); diff --git a/src/FlexCAN_T4FD.tpp b/src/FlexCAN_T4FD.tpp new file mode 100644 index 0000000..cb80cfc --- /dev/null +++ b/src/FlexCAN_T4FD.tpp @@ -0,0 +1,833 @@ +/* + MIT License + + Copyright (c) 2018 Antonio Alexander Brewer (tonton81) - https://github.com/tonton81 + + Designed and tested for PJRC Teensy 4.0. + + Forum link : https://forum.pjrc.com/threads/56035-FlexCAN_T4-FlexCAN-for-Teensy-4?highlight=flexcan_t4 + + Thanks goes to skpang, mjs513, and collin for tech/testing support + + Permission is hereby granted, free of charge, to any person obtaining a copy + of this software and associated documentation files (the "Software"), to deal + in the Software without restriction, including without limitation the rights + to use, copy, modify, merge, publish, distribute, sublicense, and / or sell + copies of the Software, and to permit persons to whom the Software is + furnished to do so, subject to the following conditions: + + The above copyright notice and this permission notice shall be included in all + copies or substantial portions of the Software. + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + SOFTWARE. +*/ +#include +#include "imxrt_flexcan.hpp" +#include "Arduino.h" + +static void flexcan_isr_can3fd(); + +FCTPFD_FUNC FCTPFD_OPT::FlexCAN_T4FD() { + if ( _bus == CAN3 ) _CAN3 = this; +} + +FCTPFD_FUNC void FCTPFD_OPT::setClock(FLEXCAN_CLOCK clock) { + if ( clock == CLK_OFF ) CCM_CSCMR2 = (CCM_CSCMR2 & 0xFFFFFC03) | CCM_CSCMR2_CAN_CLK_SEL(3) | CCM_CSCMR2_CAN_CLK_PODF(0); + if ( clock == CLK_8MHz ) CCM_CSCMR2 = (CCM_CSCMR2 & 0xFFFFFC03) | CCM_CSCMR2_CAN_CLK_SEL(2) | CCM_CSCMR2_CAN_CLK_PODF(9); + if ( clock == CLK_16MHz ) CCM_CSCMR2 = (CCM_CSCMR2 & 0xFFFFFC03) | CCM_CSCMR2_CAN_CLK_SEL(2) | CCM_CSCMR2_CAN_CLK_PODF(4); + if ( clock == CLK_24MHz ) CCM_CSCMR2 = (CCM_CSCMR2 & 0xFFFFFC03) | CCM_CSCMR2_CAN_CLK_SEL(1) | CCM_CSCMR2_CAN_CLK_PODF(0); + if ( clock == CLK_20MHz ) CCM_CSCMR2 = (CCM_CSCMR2 & 0xFFFFFC03) | CCM_CSCMR2_CAN_CLK_SEL(2) | CCM_CSCMR2_CAN_CLK_PODF(3); + if ( clock == CLK_30MHz ) CCM_CSCMR2 = (CCM_CSCMR2 & 0xFFFFFC03) | CCM_CSCMR2_CAN_CLK_SEL(0) | CCM_CSCMR2_CAN_CLK_PODF(1); + if ( clock == CLK_40MHz ) CCM_CSCMR2 = (CCM_CSCMR2 & 0xFFFFFC03) | CCM_CSCMR2_CAN_CLK_SEL(2) | CCM_CSCMR2_CAN_CLK_PODF(1); + if ( clock == CLK_60MHz ) CCM_CSCMR2 = (CCM_CSCMR2 & 0xFFFFFC03) | CCM_CSCMR2_CAN_CLK_SEL(0) | CCM_CSCMR2_CAN_CLK_PODF(0); + if ( clock == CLK_80MHz ) CCM_CSCMR2 = (CCM_CSCMR2 & 0xFFFFFC03) | CCM_CSCMR2_CAN_CLK_SEL(2) | CCM_CSCMR2_CAN_CLK_PODF(0); +} + +FCTPFD_FUNC uint32_t FCTPFD_OPT::getClock() { + const uint8_t clocksrc[4] = {60, 24, 80, 0}; + return clocksrc[(CCM_CSCMR2 & 0x300) >> 8]; +} + +FCTPFD_FUNC void FCTPFD_OPT::begin() { + if ( !getClock() ) setClock(CLK_24MHz); /* no clock enabled, enable osc clock */ + CCM_CCGR0 |= CCM_CCGR0_LPUART3(CCM_CCGR_ON); /* hardware bug, FD is unable to operate without an LPUART clock online */ + if ( _bus == CAN3 ) { + nvicIrq = IRQ_CAN3; + _VectorsRam[16 + nvicIrq] = flexcan_isr_can3fd; + CCM_CCGR7 |= 0x3C0; + busNumber = 3; + } + + setTX(); setRX(); + + FLEXCANb_MCR(_bus) &= ~FLEXCAN_MCR_MDIS; /* enable module */ + FLEXCAN_EnterFreezeMode(); + FLEXCANb_CTRL1(_bus) = FLEXCAN_CTRL_LOM /*| (1UL << 5)*/; /* listen only mode, TSYN */ + FLEXCANb_MCR(_bus) |= FLEXCAN_MCR_FRZ; /* enable freeze bit */ + while (FLEXCANb_MCR(_bus) & FLEXCAN_MCR_LPM_ACK); + softReset(); /* reset bus */ + while (!(FLEXCANb_MCR(_bus) & FLEXCAN_MCR_FRZ_ACK)); + FLEXCANb_MCR(_bus) &= ~0x8000; // disable DMA + FLEXCANb_MCR(_bus) |= (1UL << 16) | (1UL << 11) | (1UL << 17) | 0x3F; // IRMQ, FDEN, SRXDIS, 64MBs + FLEXCANb_MCR(_bus) |= FLEXCAN_MCR_AEN; // TX ABORT FEATURE + FLEXCANb_MCR(_bus) |= FLEXCAN_MCR_LPRIO_EN; // TX PRIORITY FEATURE + FLEXCANb_CTRL2(_bus) |= FLEXCAN_CTRL2_RRS | // store remote frames + FLEXCAN_CTRL2_EACEN | /* handles the way filtering works. Library adjusts to whether you use this or not */ + FLEXCAN_CTRL2_MRP | // mailbox > FIFO priority. + FLEXCAN_CTRL2_ISOCANFDEN; + FLEXCANb_MCR(_bus) |= FLEXCAN_MCR_WRN_EN; + FLEXCANb_MCR(_bus) |= FLEXCAN_MCR_WAK_MSK; + FLEXCANb_FDCTRL(_bus) = 0x80008300; // MBDSR0-MBDSR1 = 0; 32MBs each as default, TDCEN, FDRATE(BRS) + disableFIFO(); + FLEXCAN_ExitFreezeMode(); + NVIC_ENABLE_IRQ(nvicIrq); +} + +FCTPFD_FUNC uint8_t FCTPFD_OPT::setRegions(uint8_t size) { + FLEXCAN_EnterFreezeMode(); + FLEXCANb_FDCTRL(_bus) &= ~0x1B0000; /* clear both memory region bits */ + if ( constrain(size, 8, 64) == 8 ) { + size = 64; + } + else if ( constrain(size, 16, 64) == 16 ) { + FLEXCANb_FDCTRL(_bus) |= 0x90000; + size = 42; + } + else if ( constrain(size, 32, 64) == 32 ) { + FLEXCANb_FDCTRL(_bus) |= 0x120000; + size = 24; + } + else { + FLEXCANb_FDCTRL(_bus) |= 0x1B0000; + size = 14; + } + disableFIFO(); + FLEXCAN_ExitFreezeMode(); + return size; +} + +FCTPFD_FUNC uint8_t FCTPFD_OPT::setRegions(uint8_t mbdsr0, uint8_t mbdsr1) { + FLEXCAN_EnterFreezeMode(); + FLEXCANb_FDCTRL(_bus) &= ~0x1B0000; /* clear both memory region bits */ + if ( constrain(mbdsr0, 8, 64) == 8 ) { + mbdsr0 = 32; + } + else if ( constrain(mbdsr0, 16, 64) == 16 ) { + FLEXCANb_FDCTRL(_bus) |= 0x10000; + mbdsr0 = 21; + } + else if ( constrain(mbdsr0, 32, 64) == 32 ) { + FLEXCANb_FDCTRL(_bus) |= 0x20000; + mbdsr0 = 12; + } + else { + FLEXCANb_FDCTRL(_bus) |= 0x30000; + mbdsr0 = 7; + } + if ( constrain(mbdsr1, 8, 64) == 8 ) { + mbdsr1 = 32; + } + else if ( constrain(mbdsr1, 16, 64) == 16 ) { + FLEXCANb_FDCTRL(_bus) |= 0x80000; + mbdsr1 = 21; + } + else if ( constrain(mbdsr1, 32, 64) == 32 ) { + FLEXCANb_FDCTRL(_bus) |= 0x100000; + mbdsr1 = 12; + } + else { + FLEXCANb_FDCTRL(_bus) |= 0x180000; + mbdsr1 = 7; + } + disableFIFO(); + FLEXCAN_ExitFreezeMode(); + return mbdsr0 + mbdsr1; +} + +FCTPFD_FUNC uint32_t FCTPFD_OPT::mailbox_offset(uint8_t mailbox, uint8_t &maxsize) { + const uint8_t data_size[4] = { 8, 16, 32, 64 }; + const uint8_t mbx_total[4] = { 32, 21, 12, 7 }; + const uint8_t mbx_shift[4] = { 0x10, 0x18, 0x28, 0x48 }; + uint8_t region0 = (FLEXCANb_FDCTRL(_bus) & (3UL << 16)) >> 16; + uint8_t region1 = (FLEXCANb_FDCTRL(_bus) & (3UL << 19)) >> 19; + + if ( mailbox < mbx_total[region0] ) { + maxsize = data_size[region0]; + return _bus + 0x80 + (mbx_shift[region0] * mailbox); + } + else if ( mailbox < (mbx_total[region0] + mbx_total[region1]) ) { + maxsize = data_size[region1]; + return _bus + 0x280 + (mbx_shift[region1] * (mailbox - mbx_total[region0])); + } + return _bus + 0x80; +} + +FCTPFD_FUNC int FCTPFD_OPT::getFirstTxBox() { + for (uint8_t i = 0, mbsize = 0; i < max_mailboxes(); i++) { + volatile uint32_t *mbxAddr = &(*(volatile uint32_t*)(mailbox_offset(i, mbsize))); + if ( (FLEXCAN_get_code(mbxAddr[0]) >> 3) ) return i; // if TX + } + return -1; +} + +FCTPFD_FUNC uint8_t FCTPFD_OPT::getFirstTxBoxSize() { + uint8_t mbsize = 0; + mailbox_offset(getFirstTxBox(), mbsize); + return mbsize; +} + +FCTPFD_FUNC void FCTPFD_OPT::setBaudRate(FLEXCAN_FDRATES input, FLEXCAN_RXTX listen_only) { + bool frz_flag_negate = !(FLEXCANb_MCR(_bus) & FLEXCAN_MCR_FRZ_ACK); + FLEXCAN_EnterFreezeMode(); + FLEXCANb_FDCTRL(_bus) = (FLEXCANb_FDCTRL(_bus) & 0xFFFF60FF); /* clear TDC values */ + FLEXCANb_CBT(_bus) &= ~(1UL << 31); /* clear BTE bit to edit CTRL1 register */ + ( listen_only != LISTEN_ONLY ) ? FLEXCANb_CTRL1(_bus) &= ~FLEXCAN_CTRL_LOM : FLEXCANb_CTRL1(_bus) |= FLEXCAN_CTRL_LOM; + if ( input == CAN_1M_2M ) { /* based on 24MHz and 70% sample point */ + setClock(CLK_24MHz); + FLEXCANb_FDCTRL(_bus) |= (0x801B8300 & 0x9F00); + FLEXCANb_FDCBT(_bus) = 0x31423; + FLEXCANb_CBT(_bus) = 0x800624A6; + } + if ( input == CAN_1M_4M ) { /* based on 24MHz and 70% sample point */ + setClock(CLK_24MHz); + FLEXCANb_FDCTRL(_bus) |= (0x80008300 & 0x9F00); + FLEXCANb_FDCBT(_bus) = 0x10421; + FLEXCANb_CBT(_bus) = 0x800624A6; + } + if ( input == CAN_1M_6M ) { /* based on 30MHz and 70% sample point */ + setClock(CLK_30MHz); + FLEXCANb_FDCTRL(_bus) |= (0x80008300 & 0x9F00); + FLEXCANb_FDCBT(_bus) = 0x401; + FLEXCANb_CBT(_bus) = 0x80082CE8; + } + if ( input == CAN_1M_8M ) { /* based on 40MHz and 70% sample point */ + setClock(CLK_40MHz); + FLEXCANb_FDCTRL(_bus) |= (0x80008300 & 0x9F00); + FLEXCANb_FDCBT(_bus) = 0x401; + FLEXCANb_CBT(_bus) = 0x800B3D4B; + } + if ( frz_flag_negate ) FLEXCAN_ExitFreezeMode(); +} + +FCTPFD_FUNC uint8_t FCTPFD_OPT::max_mailboxes() { + uint8_t mb_count = 0; + uint8_t block0 = (FLEXCANb_FDCTRL(_bus) & (3UL << 16)) >> 16; + uint8_t block1 = (FLEXCANb_FDCTRL(_bus) & (3UL << 19)) >> 19; + + const uint8_t sizes[4] = {32, 21, 12, 7}; + mb_count = sizes[block0] + sizes[block1]; + + if ( mb_count > FLEXCANb_MAXMB_SIZE(_bus) ) return FLEXCANb_MAXMB_SIZE(_bus); + return mb_count; +} + +FCTPFD_FUNC int FCTPFD_OPT::write(FLEXCAN_MAILBOX mb_num, const CANFD_message_t &msg) { + uint8_t mbsize = 0; + volatile uint32_t *mbxAddr = &(*(volatile uint32_t*)(mailbox_offset(mb_num, mbsize))); + + if ( !((FLEXCAN_get_code(mbxAddr[0])) >> 3) ) return 0; /* not a transmit mailbox */ + if ( msg.seq && FLEXCAN_get_code(mbxAddr[0]) != FLEXCAN_MB_CODE_TX_INACTIVE ) return 0; /* non blocking resend sequential frames */ + uint32_t timeout = millis(); + while ( FLEXCAN_get_code(mbxAddr[0]) != FLEXCAN_MB_CODE_TX_INACTIVE ) { + if ( millis() - timeout > 100 ) return 0; + } + writeTxMailbox(mb_num, msg); + return 1; // transmit entry accepted // +} + +FCTPFD_FUNC void FCTPFD_OPT::writeTxMailbox(uint8_t mb_num, const CANFD_message_t &frame) { + CANFD_message_t msg = frame; + writeIFLAGBit(mb_num); + uint8_t mbsize = 0; + uint32_t code = 0; + volatile uint32_t *mbxAddr = &(*(volatile uint32_t*)(mailbox_offset(mb_num, mbsize))); + mbxAddr[0] = FLEXCAN_MB_CS_CODE(FLEXCAN_MB_CODE_TX_INACTIVE); + mbxAddr[1] = (( msg.flags.extended ) ? ( msg.id & FLEXCAN_MB_ID_EXT_MASK ) : FLEXCAN_MB_ID_IDSTD(msg.id)); + if ( msg.flags.extended ) code |= (3UL << 21); + for ( uint8_t i = 0; i < (mbsize >> 2); i++ ) mbxAddr[2 + i] = (msg.buf[0 + i * 4] << 24) | (msg.buf[1 + i * 4] << 16) | (msg.buf[2 + i * 4] << 8) | msg.buf[3 + i * 4]; + if ( msg.len > mbsize ) msg.len = mbsize; + code |= len_to_dlc(msg.len) << 16; + if ( msg.brs ) code |= (1UL << 30); // BRS + if ( msg.edl ) code |= (1UL << 31); // EDL + mbxAddr[0] = code | FLEXCAN_MB_CS_CODE(FLEXCAN_MB_CODE_TX_ONCE); +} + +FCTPFD_FUNC uint8_t FCTPFD_OPT::len_to_dlc(uint8_t val) { + if ( val <= 8 ); + else if ( val <= 12 ) val = 9; + else if ( val <= 16 ) val = 10; + else if ( val <= 20 ) val = 11; + else if ( val <= 24 ) val = 12; + else if ( val <= 32 ) val = 13; + else if ( val <= 48 ) val = 14; + else if ( val <= 64 ) val = 15; + return val; +} + +FCTPFD_FUNC bool FCTPFD_OPT::setMB(const FLEXCAN_MAILBOX &mb_num, const FLEXCAN_RXTX &mb_rx_tx, const FLEXCAN_IDE &ide) { + if ( mb_num >= max_mailboxes() ) return 0; + writeIMASKBit(mb_num, 0); /* immediately disable mailbox interrupt */ + FLEXCAN_EnterFreezeMode(); + FLEXCANb_RXIMR(_bus, mb_num) = 0UL | ((FLEXCANb_CTRL2(_bus) & FLEXCAN_CTRL2_EACEN) ? (1UL << 30) : 0); // CLEAR MAILBOX MASK + FLEXCAN_ExitFreezeMode(); + uint8_t mbsize = 0; + volatile uint32_t *mbxAddr = &(*(volatile uint32_t*)(mailbox_offset(mb_num , mbsize))); + FLEXCAN_get_code(mbxAddr[0]); // Reading Control Status atomically locks mailbox (if it is RX mode). + for ( uint8_t i = 0; i < (mbsize >> 2); i++ ) mbxAddr[2 + i] = 0x0; /* clear mailbox data */ + if ( ide == INACTIVE ) mbxAddr[0] = FLEXCAN_MB_CS_CODE(FLEXCAN_MB_CODE_RX_INACTIVE); + else if ( mb_rx_tx == RX ) mbxAddr[0] = FLEXCAN_MB_CS_CODE(FLEXCAN_MB_CODE_RX_EMPTY) | ((ide != EXT) ? 0 : FLEXCAN_MB_CS_SRR | FLEXCAN_MB_CS_IDE); + else if ( mb_rx_tx == TX ) mbxAddr[0] = FLEXCAN_MB_CS_CODE(FLEXCAN_MB_CODE_TX_INACTIVE); + mbxAddr[1] = 0; // clear ID + FLEXCANb_TIMER(_bus); /* reading timer unlocks individual mailbox */ + writeIFLAGBit(mb_num); /* clear mailbox flag */ + mb_filter_table[mb_num][0] = ( ((mbxAddr[0] & 0x600000) ? 1UL : 0UL) << 27); /* extended flag check */ + return 1; +} + +FCTPFD_FUNC uint8_t FCTPFD_OPT::dlc_to_len(uint8_t val) { + const uint8_t dlcTolen[16] = {0, 1, 2, 3, 4, 5, 6, 7, 8, 12, 16, 20, 24, 32, 48, 64}; + return dlcTolen[val]; +} + +FCTPFD_FUNC void FCTPFD_OPT::FLEXCAN_ExitFreezeMode() { + FLEXCANb_MCR(_bus) &= ~FLEXCAN_MCR_HALT; + while (FLEXCANb_MCR(_bus) & FLEXCAN_MCR_FRZ_ACK); +} + +FCTPFD_FUNC void FCTPFD_OPT::FLEXCAN_EnterFreezeMode() { + FLEXCANb_MCR(_bus) |= FLEXCAN_MCR_FRZ | FLEXCAN_MCR_HALT; + while (!(FLEXCANb_MCR(_bus) & FLEXCAN_MCR_FRZ_ACK)); +} + +FCTPFD_FUNC void FCTPFD_OPT::softReset() { + FLEXCANb_MCR(_bus) |= FLEXCAN_MCR_SOFT_RST; + while (FLEXCANb_MCR(_bus) & FLEXCAN_MCR_SOFT_RST); +} + +FCTPFD_FUNC void FCTPFD_OPT::setTX(FLEXCAN_PINS pin) { + if ( _bus == CAN3 ) { + if ( pin == DEF ) { + IOMUXC_SW_MUX_CTL_PAD_GPIO_EMC_36 = 0x19; // pin31 T3B2 + IOMUXC_SW_PAD_CTL_PAD_GPIO_EMC_36 = 0x10B0; // pin31 T3B2 + } + } + if ( _bus == CAN2 ) { + if ( pin == DEF ) { + IOMUXC_SW_MUX_CTL_PAD_GPIO_AD_B0_02 = 0x10; // pin 1 T4B1+B2 + IOMUXC_SW_PAD_CTL_PAD_GPIO_AD_B0_02 = 0x10B0; // pin 1 T4B1+B2 + } + } + if ( _bus == CAN1 ) { + if ( pin == DEF ) { + IOMUXC_SW_MUX_CTL_PAD_GPIO_AD_B1_08 = 0x12; // pin 22 T4B1+B2 + IOMUXC_SW_PAD_CTL_PAD_GPIO_AD_B1_08 = 0x10B0; // pin 22 T4B1+B2 + } + } +} + +FCTPFD_FUNC void FCTPFD_OPT::setRX(FLEXCAN_PINS pin) { + /* DAISY REGISTER CAN3 + 00 GPIO_EMC_37_ALT9  Selecting Pad: GPIO_EMC_37 for Mode: ALT9 + 01 GPIO_AD_B0_15_ALT8  Selecting Pad: GPIO_AD_B0_15 for Mode: ALT8 + 10 GPIO_AD_B0_11_ALT8  Selecting Pad: GPIO_AD_B0_11 for Mode: ALT8 + */ + /* DAISY REGISTER CAN2 + 00 GPIO_EMC_10_ALT3  Selecting Pad: GPIO_EMC_10 for Mode: ALT3 + 01 GPIO_AD_B0_03_ALT0  Selecting Pad: GPIO_AD_B0_03 for Mode: ALT0 + 10 GPIO_AD_B0_15_ALT6  Selecting Pad: GPIO_AD_B0_15 for Mode: ALT6 + 11 GPIO_B1_09_ALT6  Selecting Pad: GPIO_B1_09 for Mode: ALT6 + */ + /* DAISY REGISTER CAN1 + 00 GPIO_SD_B1_03_ALT4  Selecting Pad: GPIO_SD_B1_03 for Mode: ALT4 + 01 GPIO_EMC_18_ALT3  Selecting Pad: GPIO_EMC_18 for Mode: ALT3 + 10 GPIO_AD_B1_09_ALT2  Selecting Pad: GPIO_AD_B1_09 for Mode: ALT2 + 11 GPIO_B0_03_ALT2  Selecting Pad: GPIO_B0_03 for Mode: ALT2 + */ + if ( _bus == CAN3 ) { + if ( pin == DEF ) { + IOMUXC_CANFD_IPP_IND_CANRX_SELECT_INPUT = 0x00; + IOMUXC_SW_MUX_CTL_PAD_GPIO_EMC_37 = 0x19; // pin30 T3B2 + IOMUXC_SW_PAD_CTL_PAD_GPIO_EMC_37 = 0x10B0; // pin30 T3B2 + } + } + if ( _bus == CAN2 ) { + if ( pin == DEF ) { + IOMUXC_FLEXCAN2_RX_SELECT_INPUT = 0x01; + IOMUXC_SW_MUX_CTL_PAD_GPIO_AD_B0_03 = 0x10; // pin 0 T4B1+B2 + IOMUXC_SW_PAD_CTL_PAD_GPIO_AD_B0_03 = 0x10B0; // pin 0 T4B1+B2 + } + } + if ( _bus == CAN1 ) { + if ( pin == DEF ) { + IOMUXC_FLEXCAN1_RX_SELECT_INPUT = 0x02; + IOMUXC_SW_MUX_CTL_PAD_GPIO_AD_B1_09 = 0x12; // pin 23 T4B1+B2 + IOMUXC_SW_PAD_CTL_PAD_GPIO_AD_B1_09 = 0x10B0; // pin 23 T4B1+B2 + } + } +} + +FCTPFD_FUNC void FCTPFD_OPT::enableDMA(bool state) { /* only CAN3 supports this on 1062, untested */ + bool frz_flag_negate = !(FLEXCANb_MCR(_bus) & FLEXCAN_MCR_FRZ_ACK); + FLEXCAN_EnterFreezeMode(); + ( !state ) ? FLEXCANb_MCR(_bus) &= ~0x8000 : FLEXCANb_MCR(_bus) |= 0x8000; + if ( frz_flag_negate ) FLEXCAN_ExitFreezeMode(); +} + +FCTPFD_FUNC void FCTPFD_OPT::writeIFLAG(uint64_t value) { + FLEXCANb_IFLAG2(_bus) = value >> 32; + FLEXCANb_IFLAG1(_bus) = value; +} + +FCTPFD_FUNC void FCTPFD_OPT::writeIFLAGBit(uint8_t mb_num) { + if ( mb_num < 32 ) FLEXCANb_IFLAG1(_bus) |= (1UL << mb_num); + else FLEXCANb_IFLAG2(_bus) |= (1UL << (mb_num - 32)); +} + +FCTPFD_FUNC void FCTPFD_OPT::writeIMASK(uint64_t value) { + FLEXCANb_IMASK2(_bus) = value >> 32; + FLEXCANb_IMASK1(_bus) = value; +} + +FCTPFD_FUNC void FCTPFD_OPT::writeIMASKBit(uint8_t mb_num, bool set) { + if ( mb_num < 32 ) (( set ) ? FLEXCANb_IMASK1(_bus) |= (1UL << mb_num) : FLEXCANb_IMASK1(_bus) &= ~(1UL << mb_num)); + else (( set ) ? FLEXCANb_IMASK2(_bus) |= (1UL << (mb_num - 32)) : FLEXCANb_IMASK2(_bus) &= ~(1UL << (mb_num - 32))); +} + +static void flexcan_isr_can3fd() { + if ( _CAN3 ) _CAN3->flexcan_interrupt(); +} + +FCTPFD_FUNC void FCTPFD_OPT::enableMBInterrupts(bool status) { + FLEXCAN_EnterFreezeMode(); + for ( uint8_t mb_num = 0, mbsize = 0; mb_num < max_mailboxes(); mb_num++ ) { + volatile uint32_t *mbxAddr = &(*(volatile uint32_t*)(mailbox_offset(mb_num, mbsize))); + if ( (FLEXCAN_get_code(mbxAddr[0]) >> 3) ) continue; // skip TX mailboxes + enableMBInterrupt((FLEXCAN_MAILBOX)mb_num, status); + } + FLEXCAN_ExitFreezeMode(); +} + +FCTPFD_FUNC void FCTPFD_OPT::enableMBInterrupt(const FLEXCAN_MAILBOX &mb_num, bool status) { + FLEXCAN_EnterFreezeMode(); + if ( status ) writeIMASKBit(mb_num); /* enable mailbox interrupt */ + else writeIMASKBit(mb_num, 0); /* disable mailbox interrupt */ + FLEXCAN_ExitFreezeMode(); +} + +FCTPFD_FUNC void FCTPFD_OPT::onReceive(const FLEXCAN_MAILBOX &mb_num, _MBFD_ptr handler) { + if ( FIFO == mb_num ) { + _mbHandlers[0] = handler; + return; + } + _mbHandlers[mb_num] = handler; +} + +FCTPFD_FUNC void FCTPFD_OPT::onReceive(_MBFD_ptr handler) { + _mainHandler = handler; +} + +FCTPFD_FUNC void FCTPFD_OPT::mbCallbacks(const FLEXCAN_MAILBOX &mb_num, const CANFD_message_t &msg) { + if ( mb_num == FIFO ) { + if ( _mbHandlers[0] ) _mbHandlers[0](msg); + return; + } + if ( _mbHandlers[mb_num] ) _mbHandlers[mb_num](msg); + if ( _mainHandler ) _mainHandler(msg); +} + +FCTPFD_FUNC void FCTPFD_OPT::flexcan_interrupt() { + CANFD_message_t msg; // setup a temporary storage buffer + uint64_t imask = readIMASK(), iflag = readIFLAG(); + + for ( uint8_t mb_num = 0, mbsize = 0; mb_num < max_mailboxes(); mb_num++ ) { + if (!(imask & (1ULL << mb_num))) continue; /* don't read non-interrupt mailboxes */ + if (!(iflag & (1ULL << mb_num))) continue; /* don't read unflagged mailboxes */ + volatile uint32_t *mbxAddr = &(*(volatile uint32_t*)(mailbox_offset(mb_num, mbsize))); + uint32_t code = mbxAddr[0]; + if ( ( FLEXCAN_get_code(code) == FLEXCAN_MB_CODE_RX_FULL ) || + ( FLEXCAN_get_code(code) == FLEXCAN_MB_CODE_RX_OVERRUN ) ) { + msg.flags.extended = (bool)(code & (1UL << 21)); + msg.edl = (bool)(code & (1UL << 31)); + msg.brs = (bool)(code & (1UL << 30)); + msg.esi = (bool)(code & (1UL << 29)); + msg.id = (mbxAddr[1] & 0x1FFFFFFF) >> ((msg.flags.extended) ? 0 : 18); + if ( FLEXCAN_get_code(code) == FLEXCAN_MB_CODE_RX_OVERRUN ) msg.flags.overrun = 1; + msg.len = dlc_to_len((code & 0xF0000) >> 16); + msg.mb = mb_num; + msg.timestamp = code & 0xFFFF; + msg.bus = busNumber; + for ( uint8_t i = 0; i < (mbsize >> 2); i++ ) for ( int8_t d = 0; d < 4 ; d++ ) msg.buf[(4 * i) + 3 - d] = (uint8_t)(mbxAddr[2 + i] >> (8 * d)); + mbxAddr[0] = FLEXCAN_MB_CS_CODE(FLEXCAN_MB_CODE_RX_EMPTY) | ((msg.flags.extended) ? (FLEXCAN_MB_CS_SRR | FLEXCAN_MB_CS_IDE) : 0); + (void)FLEXCANb_TIMER(_bus); + writeIFLAGBit(mb_num); + if ( filter_match((FLEXCAN_MAILBOX)mb_num, msg.id) ) struct2queueRx(msg); /* store frame in queue */ + frame_distribution(msg); + ext_outputFD1(msg); + ext_outputFD2(msg); + ext_outputFD3(msg); + } + } + FLEXCANb_ESR1(_bus) = FLEXCANb_ESR1(_bus); +} + +FCTPFD_FUNC void FCTPFD_OPT::struct2queueRx(const CANFD_message_t &msg) { + if ( !isEventsUsed ) { + mbCallbacks((FLEXCAN_MAILBOX)msg.mb, msg); + return; + } + uint8_t buf[sizeof(CANFD_message_t)]; + memmove(buf, &msg, sizeof(msg)); + rxBuffer.push_back(buf, sizeof(CANFD_message_t)); +} + +FCTPFD_FUNC void FCTPFD_OPT::struct2queueTx(const CANFD_message_t &msg) { + uint8_t buf[sizeof(CANFD_message_t)]; + memmove(buf, &msg, sizeof(msg)); + txBuffer.push_back(buf, sizeof(CANFD_message_t)); +} + +FCTPFD_FUNC int FCTPFD_OPT::write(const CANFD_message_t &msg) { + if ( msg.seq ) { + if ( !write((FLEXCAN_MAILBOX)getFirstTxBox(), msg) ) struct2queueTx(msg); + return 1; + } + for (uint8_t i = 0, mbsize = 0; i < max_mailboxes(); i++) { + if (readIMASK() & (1ULL << i)) continue; /* don't write interrupt enabled mailboxes */ + volatile uint32_t *mbxAddr = &(*(volatile uint32_t*)(mailbox_offset(i, mbsize))); + if ( FLEXCAN_get_code(mbxAddr[0]) == FLEXCAN_MB_CODE_TX_INACTIVE ) { + writeTxMailbox(i, msg); + return 1; /* transmit entry accepted */ + } + } + return 0; /* transmit entry failed, no mailboxes or queues available */ +} + +FCTPFD_FUNC uint64_t FCTPFD_OPT::events() { + if ( !isEventsUsed ) isEventsUsed = 1; + if ( rxBuffer.size() ) { + CANFD_message_t frame; + uint8_t buf[sizeof(CANFD_message_t)]; + rxBuffer.pop_front(buf, sizeof(CANFD_message_t)); + memmove(&frame, buf, sizeof(frame)); + if ( _mbHandlers[frame.mb] ) _mbHandlers[frame.mb](frame); + if ( _mainHandler ) _mainHandler(frame); + } + if ( txBuffer.size() ) { + CANFD_message_t frame; + uint8_t buf[sizeof(CANFD_message_t)]; + txBuffer.peek_front(buf, sizeof(CANFD_message_t)); + memmove(&frame, buf, sizeof(frame)); + if ( write((FLEXCAN_MAILBOX)getFirstTxBox(), frame) ) txBuffer.pop_front(); + } + return (uint64_t)(rxBuffer.size() << 12) | txBuffer.size(); +} + +FCTPFD_FUNC int FCTPFD_OPT::read(CANFD_message_t &msg) { + return readMB(msg); +} + +FCTPFD_FUNC int FCTPFD_OPT::readMB(CANFD_message_t &msg) { + uint8_t cycle_limit = 3; + for ( uint8_t mbsize = 0; mailbox_reader_increment <= max_mailboxes(); ++mailbox_reader_increment ) { + if ( mailbox_reader_increment >= max_mailboxes() ) { + mailbox_reader_increment = 0; + if ( !--cycle_limit ) return 0; + } + volatile uint32_t *mbxAddr = &(*(volatile uint32_t*)(mailbox_offset(mailbox_reader_increment , mbsize))); + if ((readIMASK() & (1ULL << mailbox_reader_increment))) continue; /* don't read interrupt enabled mailboxes */ + uint32_t code = mbxAddr[0]; + if ( (FLEXCAN_get_code(code) >> 3) ) continue; /* skip TX mailboxes */ + // if (!(code & 0x600000) && !(readIFLAG() & (1ULL << mailbox_reader_increment))) continue; /* don't read unflagged mailboxes, errata: extended mailboxes iflags do not work in poll mode, must check CS field */ + if ( ( FLEXCAN_get_code(code) == FLEXCAN_MB_CODE_RX_FULL ) || + ( FLEXCAN_get_code(code) == FLEXCAN_MB_CODE_RX_OVERRUN ) ) { + msg.flags.extended = (bool)(code & (1UL << 21)); + msg.edl = (bool)(code & (1UL << 31)); + msg.brs = (bool)(code & (1UL << 30)); + msg.esi = (bool)(code & (1UL << 29)); + msg.id = (mbxAddr[1] & 0x1FFFFFFF) >> ((msg.flags.extended) ? 0 : 18); + if ( FLEXCAN_get_code(code) == FLEXCAN_MB_CODE_RX_OVERRUN ) msg.flags.overrun = 1; + msg.len = dlc_to_len((code & 0xF0000) >> 16); + msg.mb = mailbox_reader_increment++; + msg.timestamp = code & 0xFFFF; + msg.bus = busNumber; + for ( uint8_t i = 0; i < (mbsize >> 2); i++ ) for ( int8_t d = 0; d < 4 ; d++ ) msg.buf[(4 * i) + 3 - d] = (uint8_t)(mbxAddr[2 + i] >> (8 * d)); + mbxAddr[0] = FLEXCAN_MB_CS_CODE(FLEXCAN_MB_CODE_RX_EMPTY) | ((msg.flags.extended) ? (FLEXCAN_MB_CS_SRR | FLEXCAN_MB_CS_IDE) : 0); + (void)FLEXCANb_TIMER(_bus); /* Read free-running timer to unlock Rx Message Buffer. */ + writeIFLAGBit(msg.mb); + frame_distribution(msg); + if ( filter_match((FLEXCAN_MAILBOX)msg.mb, msg.id) ) return 1; + } + } + return 0; /* no messages available */ +} + +FCTPFD_FUNC void FCTPFD_OPT::enableFIFO(bool status) { + bool frz_flag_negate = !(FLEXCANb_MCR(_bus) & FLEXCAN_MCR_FRZ_ACK); + FLEXCAN_EnterFreezeMode(); + writeIMASK(0ULL); // disable all FIFO/MB Interrupts + FLEXCANb_RXMGMASK(_bus) = FLEXCANb_RXFGMASK(_bus) = 0; + writeIFLAG(readIFLAG()); // (all bits reset when written back) + FLEXCANb_MCR(_bus) &= ~0x20008000; /* we disable DMA, Legacy FIFO (we never use this in FD mode!) */ + for (uint8_t i = 0; i < max_mailboxes(); i++ ) FLEXCANb_RXIMR(_bus, i) = 0 | ((FLEXCANb_CTRL2(_bus) & FLEXCAN_CTRL2_EACEN) ? (1UL << 30) : 0); // CLEAR MAILBOX MASKS (RXIMR) + for (uint8_t i = 0, mbsize = 0; i < max_mailboxes(); i++) { /* clear valid MB codes from memory regions */ + volatile uint32_t *mbxAddr = &(*(volatile uint32_t*)(mailbox_offset(i, mbsize))); + mbxAddr[0] = mbxAddr[1] = 0; + } + + if ( status ) { /* enable FIFO */ + // TODO: FD FIFO, ### WARNING: NXP claims, after being tested to fail despite documented and advertised, that the 1062 does NOT support FD FIFO mode!!! + } + else { // FIFO disabled default setup of mailboxes, 0-7 RX, 8-15 TX + for (uint8_t i = 0, mbsize = 0; i < max_mailboxes(); i++ ) { // clear all mailboxes + volatile uint32_t *mbxAddr = &(*(volatile uint32_t*)(mailbox_offset(i, mbsize))); + if ( i < max_mailboxes() / 2 ) { + mbxAddr[0] = FLEXCAN_MB_CS_CODE(FLEXCAN_MB_CODE_RX_EMPTY) | ((i < (max_mailboxes() / 4)) ? 0 : FLEXCAN_MB_CS_IDE | FLEXCAN_MB_CS_SRR); + FLEXCANb_RXIMR(_bus, i) = 0UL | ((FLEXCANb_CTRL2(_bus) & FLEXCAN_CTRL2_EACEN) ? (1UL << 30) : 0); // (RXIMR) + } + else { + mbxAddr[0] = FLEXCAN_MB_CS_CODE(FLEXCAN_MB_CODE_TX_INACTIVE); + } + } + } + if ( frz_flag_negate ) FLEXCAN_ExitFreezeMode(); +} + +FCTPFD_FUNC void FCTPFD_OPT::setMBFilterProcessing(FLEXCAN_MAILBOX mb_num, uint32_t filter_id, uint32_t calculated_mask) { + bool frz_flag_negate = !(FLEXCANb_MCR(_bus) & FLEXCAN_MCR_FRZ_ACK); + FLEXCAN_EnterFreezeMode(); + uint8_t mbsize = 0; + volatile uint32_t *mbxAddr = &(*(volatile uint32_t*)(mailbox_offset(mb_num, mbsize))); + FLEXCANb_RXIMR(_bus, mb_num) = calculated_mask | ((FLEXCANb_CTRL2(_bus) & FLEXCAN_CTRL2_EACEN) ? (1UL << 30) : 0); + mbxAddr[1] = ((!(mbxAddr[0] & FLEXCAN_MB_CS_IDE)) ? FLEXCAN_MB_ID_IDSTD(filter_id) : FLEXCAN_MB_ID_IDEXT(filter_id)); + if ( frz_flag_negate ) FLEXCAN_ExitFreezeMode(); +} + +FCTPFD_FUNC bool FCTPFD_OPT::setMBFilterRange(FLEXCAN_MAILBOX mb_num, uint32_t id1, uint32_t id2) { + if ( mb_num >= max_mailboxes() ) return 0; /* mailbox not available */ + uint8_t mbsize = 0; + volatile uint32_t *mbxAddr = &(*(volatile uint32_t*)(mailbox_offset(mb_num, mbsize))); + if ( (FLEXCAN_get_code(mbxAddr[0]) >> 3) ) return 0; /* exit on TX mailbox */ + if ( id1 > id2 || ((id2 > id1) && (id2-id1>1000)) || !id1 || !id2 ) return 0; /* don't play around... */ + uint32_t stage1 = id1, stage2 = id1; + for ( uint32_t i = id1 + 1; i <= id2; i++ ) { + stage1 |= i; stage2 &= i; + } + uint32_t mask = ( !(mbxAddr[0] & FLEXCAN_MB_CS_IDE) ) ? FLEXCAN_MB_ID_IDSTD( (stage1 ^ stage2) ^ 0x1FFFFFFF ) : FLEXCAN_MB_ID_IDEXT( (stage1 ^ stage2) ^ 0x1FFFFFFF ); + setMBFilterProcessing(mb_num,id1,mask); + filter_store(FLEXCAN_RANGE, mb_num, 2, id1, id2, 0, 0, 0, mask); + return 1; +} + +FCTPFD_FUNC void FCTPFD_OPT::filter_store(FLEXCAN_FILTER_TABLE type, FLEXCAN_MAILBOX mb_num, uint32_t id_count, uint32_t id1, uint32_t id2, uint32_t id3, uint32_t id4, uint32_t id5, uint32_t mask) { + mb_filter_table[mb_num][0] = mb_num; // first 7 bits reserved for MB + mb_filter_table[mb_num][0] |= (id_count << 7); // we store the quantity of ids after the mailboxes + /* bit 28: filter enabled */ + mb_filter_table[mb_num][0] |= (type << 29); // we reserve 3 upper bits for type + uint8_t mbsize; + volatile uint32_t *mbxAddr = &(*(volatile uint32_t*)(mailbox_offset(mb_num, mbsize))); + mb_filter_table[mb_num][0] |= ( ((mbxAddr[0] & 0x600000) ? 1UL : 0UL) << 27); /* extended flag check */ + mb_filter_table[mb_num][1] = id1; // id1 + mb_filter_table[mb_num][2] = id2; // id2 + mb_filter_table[mb_num][3] = id3; // id3 + mb_filter_table[mb_num][4] = id4; // id4 + mb_filter_table[mb_num][5] = id5; // id5 + mb_filter_table[mb_num][6] = mask; // mask +} + +FCTPFD_FUNC void FCTPFD_OPT::enhanceFilter(FLEXCAN_MAILBOX mb_num) { + if ( !(mb_filter_table[mb_num][0] & 0xE0000000) ) return; + mb_filter_table[mb_num][0] |= (1UL << 28); /* enable enhancement */ +} + +FCTPFD_FUNC bool FCTPFD_OPT::filter_match(FLEXCAN_MAILBOX mb_num, uint32_t id) { + if ( !(mb_filter_table[mb_num][0] & 0x10000000) ) return 1; + if ( (mb_filter_table[mb_num][0] >> 29) == FLEXCAN_MULTI ) { + for ( uint8_t i = 0; i < ((mb_filter_table[mb_num][0] & 0x380) >> 7); i++) if ( id == mb_filter_table[mb_num][i+1] ) return 1; + } + else if ( (mb_filter_table[mb_num][0] >> 29) == FLEXCAN_RANGE ) { + if ( id >= mb_filter_table[mb_num][1] && id <= mb_filter_table[mb_num][2] ) return 1; + } + return 0; +} + +FCTPFD_FUNC void FCTPFD_OPT::frame_distribution(CANFD_message_t &msg) { + if ( !distribution ) return; /* distribution not enabled */ + CANFD_message_t frame = msg; + for ( uint8_t i = 0; i < max_mailboxes(); i++ ) { + if ( frame.mb == i ) continue; // don't distribute to same mailbox + if ( !(mb_filter_table[i][0] & 0xE0000000) ) continue; // skip unset filters + if ( (bool)(mb_filter_table[i][0] & (1UL << 27)) != msg.flags.extended ) continue; /* extended flag check */ + if ( (mb_filter_table[i][0] >> 29) == FLEXCAN_MULTI ) { + for ( uint8_t p = 0; p < ((mb_filter_table[i][0] & 0x380) >> 7); p++) { + if ( frame.id == mb_filter_table[i][p+1] ) { + frame.mb = i; + struct2queueRx(frame); + } + } + } + else if ( (mb_filter_table[i][0] >> 29) == FLEXCAN_RANGE ) { + if ( frame.id >= mb_filter_table[i][1] && frame.id <= mb_filter_table[i][2] ) { + frame.mb = i; + struct2queueRx(frame); + } + } + } +} + +FCTPFD_FUNC void FCTPFD_OPT::setMBFilter(FLEXCAN_FLTEN input) { + bool frz_flag_negate = !(FLEXCANb_MCR(_bus) & FLEXCAN_MCR_FRZ_ACK); + FLEXCAN_EnterFreezeMode(); + for (uint8_t i = 0, mbsize = 0; i < max_mailboxes(); i++) { + volatile uint32_t *mbxAddr = &(*(volatile uint32_t*)(mailbox_offset(i, mbsize))); + if ( (FLEXCAN_get_code(mbxAddr[0]) >> 3) ) continue; /* skip TX mailboxes */ + if ( input == ACCEPT_ALL ) FLEXCANb_RXIMR(_bus, i) = 0UL | ((FLEXCANb_CTRL2(_bus) & FLEXCAN_CTRL2_EACEN) ? (1UL << 30) : 0); // (RXIMR) + if ( input == REJECT_ALL ) FLEXCANb_RXIMR(_bus, i) = ~0UL; // (RXIMR) + mbxAddr[1] = 0UL; + mb_filter_table[i][0] = ( ((mbxAddr[0] & 0x600000) ? 1UL : 0UL) << 27); /* extended flag check */ + } + if ( frz_flag_negate ) FLEXCAN_ExitFreezeMode(); +} + +FCTPFD_FUNC void FCTPFD_OPT::setMBFilter(FLEXCAN_MAILBOX mb_num, FLEXCAN_FLTEN input) { + if ( mb_num >= max_mailboxes() ) return; /* mailbox not available */ + uint8_t mbsize = 0; + volatile uint32_t *mbxAddr = &(*(volatile uint32_t*)(mailbox_offset(mb_num, mbsize))); + if ( (FLEXCAN_get_code(mbxAddr[0]) >> 3) ) return; /* exit on TX mailbox */ + bool frz_flag_negate = !(FLEXCANb_MCR(_bus) & FLEXCAN_MCR_FRZ_ACK); + FLEXCAN_EnterFreezeMode(); + if ( input == ACCEPT_ALL ) FLEXCANb_RXIMR(_bus, mb_num) = 0UL | ((FLEXCANb_CTRL2(_bus) & FLEXCAN_CTRL2_EACEN) ? (1UL << 30) : 0); // (RXIMR) + if ( input == REJECT_ALL ) FLEXCANb_RXIMR(_bus, mb_num) = ~0UL; // (RXIMR) + mbxAddr[1] = 0UL; + mb_filter_table[mb_num][0] = ( ((mbxAddr[0] & 0x600000) ? 1UL : 0UL) << 27); /* extended flag check */ + if ( frz_flag_negate ) FLEXCAN_ExitFreezeMode(); +} + +FCTPFD_FUNC bool FCTPFD_OPT::setMBFilter(FLEXCAN_MAILBOX mb_num, uint32_t id1) { + if ( mb_num >= max_mailboxes() ) return 0; /* mailbox not available */ + uint8_t mbsize = 0; + volatile uint32_t *mbxAddr = &(*(volatile uint32_t*)(mailbox_offset(mb_num, mbsize))); + if ( (FLEXCAN_get_code(mbxAddr[0]) >> 3) ) return 0; /* exit on TX mailbox */ + uint32_t mask = ( !(mbxAddr[0] & FLEXCAN_MB_CS_IDE) ) ? FLEXCAN_MB_ID_IDSTD(((id1) ^ (id1)) ^ 0x7FF) : FLEXCAN_MB_ID_IDEXT(((id1) ^ (id1)) ^ 0x1FFFFFFF); + setMBFilterProcessing(mb_num,id1,mask); + filter_store(FLEXCAN_MULTI, mb_num, 1, id1, 0, 0, 0, 0, mask); + return 1; +} + +FCTPFD_FUNC bool FCTPFD_OPT::setMBFilter(FLEXCAN_MAILBOX mb_num, uint32_t id1, uint32_t id2) { + if ( mb_num >= max_mailboxes() ) return 0; /* mailbox not available */ + uint8_t mbsize = 0; + volatile uint32_t *mbxAddr = &(*(volatile uint32_t*)(mailbox_offset(mb_num, mbsize))); + if ( (FLEXCAN_get_code(mbxAddr[0]) >> 3) ) return 0; /* exit on TX mailbox */ + uint32_t mask = ( !(mbxAddr[0] & FLEXCAN_MB_CS_IDE) ) ? FLEXCAN_MB_ID_IDSTD(((id1 | id2) ^ (id1 & id2)) ^ 0x7FF) : FLEXCAN_MB_ID_IDEXT(((id1 | id2) ^ (id1 & id2)) ^ 0x1FFFFFFF); + setMBFilterProcessing(mb_num,id1,mask); + filter_store(FLEXCAN_MULTI, mb_num, 2, id1, id2, 0, 0, 0, mask); + return 1; +} + +FCTPFD_FUNC bool FCTPFD_OPT::setMBFilter(FLEXCAN_MAILBOX mb_num, uint32_t id1, uint32_t id2, uint32_t id3) { + if ( mb_num >= max_mailboxes() ) return 0; /* mailbox not available */ + uint8_t mbsize = 0; + volatile uint32_t *mbxAddr = &(*(volatile uint32_t*)(mailbox_offset(mb_num, mbsize))); + if ( (FLEXCAN_get_code(mbxAddr[0]) >> 3) ) return 0; /* exit on TX mailbox */ + uint32_t mask = ( !(mbxAddr[0] & FLEXCAN_MB_CS_IDE) ) ? FLEXCAN_MB_ID_IDSTD(((id1 | id2 | id3) ^ (id1 & id2 & id3)) ^ 0x7FF) : FLEXCAN_MB_ID_IDEXT(((id1 | id2 | id3) ^ (id1 & id2 & id3)) ^ 0x1FFFFFFF); + setMBFilterProcessing(mb_num,id1,mask); + filter_store(FLEXCAN_MULTI, mb_num, 3, id1, id2, id3, 0, 0, mask); + return 1; +} + +FCTPFD_FUNC bool FCTPFD_OPT::setMBFilter(FLEXCAN_MAILBOX mb_num, uint32_t id1, uint32_t id2, uint32_t id3, uint32_t id4) { + if ( mb_num >= max_mailboxes() ) return 0; /* mailbox not available */ + uint8_t mbsize = 0; + volatile uint32_t *mbxAddr = &(*(volatile uint32_t*)(mailbox_offset(mb_num, mbsize))); + if ( (FLEXCAN_get_code(mbxAddr[0]) >> 3) ) return 0; /* exit on TX mailbox */ + uint32_t mask = ( !(mbxAddr[0] & FLEXCAN_MB_CS_IDE) ) ? FLEXCAN_MB_ID_IDSTD(((id1 | id2 | id3 | id4) ^ (id1 & id2 & id3 & id4)) ^ 0x7FF) : FLEXCAN_MB_ID_IDEXT(((id1 | id2 | id3 | id4) ^ (id1 & id2 & id3 & id4)) ^ 0x1FFFFFFF); + setMBFilterProcessing(mb_num,id1,mask); + filter_store(FLEXCAN_MULTI, mb_num, 4, id1, id2, id3, id4, 0, mask); + return 1; +} + +FCTPFD_FUNC bool FCTPFD_OPT::setMBFilter(FLEXCAN_MAILBOX mb_num, uint32_t id1, uint32_t id2, uint32_t id3, uint32_t id4, uint32_t id5) { + if ( mb_num >= max_mailboxes() ) return 0; /* mailbox not available */ + uint8_t mbsize = 0; + volatile uint32_t *mbxAddr = &(*(volatile uint32_t*)(mailbox_offset(mb_num, mbsize))); + if ( (FLEXCAN_get_code(mbxAddr[0]) >> 3) ) return 0; /* exit on TX mailbox */ + uint32_t mask = ( !(mbxAddr[0] & FLEXCAN_MB_CS_IDE) ) ? FLEXCAN_MB_ID_IDSTD(((id1 | id2 | id3 | id4 | id5) ^ (id1 & id2 & id3 & id4 & id5)) ^ 0x7FF) : FLEXCAN_MB_ID_IDEXT(((id1 | id2 | id3 | id4 | id5) ^ (id1 & id2 & id3 & id4 & id5)) ^ 0x1FFFFFFF); + setMBFilterProcessing(mb_num,id1,mask); + filter_store(FLEXCAN_MULTI, mb_num, 5, id1, id2, id3, id4, id5, mask); + return 1; +} + +FCTPFD_FUNC void FCTPFD_OPT::mailboxStatus() { + Serial.print("FIFO Disabled\n\tMailboxes:\n"); + for ( uint8_t i = 0, mbsize = 0; i < max_mailboxes(); i++ ) { + volatile uint32_t *mbxAddr = &(*(volatile uint32_t*)(mailbox_offset(i, mbsize))); + switch ( FLEXCAN_get_code(mbxAddr[0]) ) { + case 0b0000: { + Serial.printf("\t\tMB%u%s\n", i, " code: RX_INACTIVE"); + break; + } + case 0b0100: { + Serial.printf("\t\tMB%u%s%s\n", i, " code: RX_EMPTY",((mbxAddr[0] & FLEXCAN_MB_CS_IDE)?"\t(Extended Frame)":"\t(Standard Frame)")); + break; + } + case 0b0010: { + Serial.printf("\t\tMB%u%s\n", i, " code: RX_FULL"); + break; + } + case 0b0110: { + Serial.printf("\t\tMB%u%s\n", i, " code: RX_OVERRUN"); + break; + } + case 0b1010: { + Serial.printf("\t\tMB%u%s\n", i, " code: RX_ANSWER"); + break; + } + case 0b0001: { + Serial.printf("\t\tMB%u%s\n", i, " code: RX_BUSY"); + break; + } + case 0b1000: { + Serial.printf("\t\tMB%u%s\n", i, " code: TX_INACTIVE"); + break; + } + case 0b1001: { + Serial.printf("\t\tMB%u%s\n", i, " code: TX_ABORT"); + break; + } + case 0b1100: { + Serial.printf("\t\tMB%u%s", i, " code: TX_DATA (Transmitting)"); + uint32_t extid = (mbxAddr[0] & FLEXCAN_MB_CS_IDE); + (extid) ? Serial.print("(Extended Frame)") : Serial.print("(Standard Frame)"); + uint32_t dataIn = mbxAddr[2]; + uint32_t id = (FLEXCANb_MBn_ID(_bus, i) & FLEXCAN_MB_ID_EXT_MASK); + if (!extid) id >>= FLEXCAN_MB_ID_STD_BIT_NO; + Serial.print("(ID: 0x"); Serial.print(id, HEX); Serial.print(")"); + Serial.print("(Payload: "); Serial.print((uint8_t)(dataIn >> 24), HEX); + Serial.print(" "); Serial.print((uint8_t)(dataIn >> 16), HEX); + Serial.print(" "); Serial.print((uint8_t)(dataIn >> 8), HEX); + Serial.print(" "); Serial.print((uint8_t)dataIn, HEX); + dataIn = mbxAddr[3]; + Serial.print(" "); Serial.print((uint8_t)(dataIn >> 24), HEX); + Serial.print(" "); Serial.print((uint8_t)(dataIn >> 16), HEX); + Serial.print(" "); Serial.print((uint8_t)(dataIn >> 8), HEX); + Serial.print(" "); Serial.print((uint8_t)dataIn, HEX); + Serial.println(")"); + break; + } + case 0b1110: { + Serial.printf("\t\tMB%u%s\n", i, " code: TX_ANSWER"); + break; + } + } + } // for loop +} + +extern void __attribute__((weak)) ext_outputFD1(const CANFD_message_t &msg); +extern void __attribute__((weak)) ext_outputFD2(const CANFD_message_t &msg); +extern void __attribute__((weak)) ext_outputFD3(const CANFD_message_t &msg); diff --git a/src/FlexCAN_T4FDTimings.tpp b/src/FlexCAN_T4FDTimings.tpp new file mode 100644 index 0000000..206d673 --- /dev/null +++ b/src/FlexCAN_T4FDTimings.tpp @@ -0,0 +1,569 @@ +#include +#include "imxrt_flexcan.hpp" +#include "Arduino.h" + +FCTPFD_FUNC bool FCTPFD_OPT::setBaudRateAdvanced(CANFD_timings_t config, uint8_t nominal_choice, uint8_t flexdata_choice, FLEXCAN_RXTX listen_only) { + return setBaudRate(config, nominal_choice, flexdata_choice, listen_only, 1); +} + +FCTPFD_FUNC bool FCTPFD_OPT::setBaudRate(CANFD_timings_t config, FLEXCAN_RXTX listen_only) { + return setBaudRate(config, 1, 1, listen_only, 0); +} + +FCTPFD_FUNC bool FCTPFD_OPT::setBaudRate(CANFD_timings_t config, uint8_t nominal_choice, uint8_t flexdata_choice, FLEXCAN_RXTX listen_only, bool advanced) { + uint32_t result = 0, result_old = 0; + double results[10][12] = { { 0 } , { 0 } }; + + double baudrate = config.baudrate, propdelay = config.propdelay, bus_length = config.bus_length, req_smp = config.sample; + double cpi_clock = config.clock; + + baudrate /= 1000; + double ratio = cpi_clock * 1000 / baudrate; + double nbt_min = 8, nbt_max = 129, propseg_max = 64, propseg = 0, pseg_1, pseg_2; + + propdelay = (propdelay * 2) + (bus_length * 10); + uint32_t ipt = 2; + double temp = 0, pseg1 = 32, pseg2 = 32, rjw = 16, prescaler_min = 1, prescaler_max = 1024; + + if ( advanced ) Serial.println("\n\n #########################################################################################################################################"); + if ( advanced ) Serial.println(" #\t\t\t\t\t\t\t\t*** CAN NOMINAL CONFIGURATION ***\t\t\t\t\t #"); + if ( advanced ) Serial.println(" #########################################################################################################################################\n"); + for ( uint32_t prescaler = prescaler_min; prescaler < prescaler_max; prescaler++ ) { + temp = ratio / prescaler; + for ( double nbt = nbt_min; nbt < nbt_max; nbt++ ) { + if ( temp == nbt ) { + propseg = ceil(propdelay / 1000 * cpi_clock / prescaler); + if ( propseg <= propseg_max ) { + for ( double pseg = 1; pseg <= pseg1; pseg++ ) { + if ( req_smp != 0 ) { + pseg_1 = round((nbt * req_smp / 100) - 1 - propseg); + pseg_2 = nbt - 1 - propseg - pseg_1; + if ( pseg_2 >= ipt && pseg_2 <= pseg2 && pseg_1 > 0 && pseg_1 <= pseg1 ) { + result = result_old + 1; + rjw = ( pseg_2 <= rjw ) ? pseg_2 : rjw; + double error = (double) rjw / 20 / nbt * 100; + double error1 = (double)( ( pseg_1 <= pseg_2 ) ? pseg_1 : pseg_2 ) / 2 / (13 * nbt - pseg_2) * 100; + double sclk = (cpi_clock / prescaler); + double tq = (1000 / (cpi_clock / prescaler)); + double smp = 100 - pseg_2 / nbt * 100; + results[result - 1][0] = prescaler; + results[result - 1][1] = sclk; + results[result - 1][2] = tq; + results[result - 1][3] = nbt; + results[result - 1][4] = propseg; + results[result - 1][5] = pseg_1; + results[result - 1][6] = pseg_2; + results[result - 1][7] = rjw; + results[result - 1][8] = (error <= error1) ? error : error1; + results[result - 1][9] = smp; + if ( advanced ) Serial.print(" Prescaler Sclk(MHz) Tq(ns) NBT(#Tq) Propseg(#Tq) Pseg1(#Tq) Pseg2(#Tq) RJW(#Tq) ?f(%) Sample Point(%) CBT Register\n"); + if ( advanced ) Serial.printf(" %2u)", result); + if ( advanced ) Serial.printf(" %4u", prescaler); + if ( advanced ) Serial.printf(" %6.2f", sclk); + if ( advanced ) Serial.printf(" %7.2f", tq); + if ( advanced ) Serial.printf(" %2u", (uint32_t)nbt); + if ( advanced ) Serial.printf(" %2u", (uint32_t)propseg); + if ( advanced ) Serial.printf(" %2u", (uint32_t)pseg_1); + if ( advanced ) Serial.printf(" %2u", (uint32_t)pseg_2); + if ( advanced ) Serial.printf(" %2u", (uint32_t)rjw); + if ( advanced ) Serial.printf(" %6.2f", ((error <= error1) ? error : error1)); + if ( advanced ) Serial.printf(" %6.2f", smp); + uint32_t cbt = ((uint32_t)(results[result - 1][4] - 1) << 10); // EPROPSEG + cbt |= ((uint32_t)(results[result - 1][6] - 1) << 0); // EPSEG2 + cbt |= ((uint32_t)(results[result - 1][5] - 1) << 5); // EPSEG1 + cbt |= ((uint32_t)(results[result - 1][7] - 1) << 16); // ERJW + cbt |= ((uint32_t)(results[result - 1][0] - 1) << 21); // EPRESDIV + cbt |= (1UL << 31); // BTF /* use CBT register instead of CTRL1 for FD */ + if ( advanced ) Serial.printf(" 0x%8.08X",cbt); + + if ( advanced ) Serial.printf("\n\t%8s%s%.2f%s%u%s%.2f%s", "", "*** Arbitration phase bit time = ", (results[result - 1][2] * results[result - 1][3] / 1000), "us (", (int)results[result - 1][3], "Tq, Tq=", results[result - 1][2], ")\n"); + if ( advanced ) Serial.printf("\t%8s%s%.2f%s", "", "a) Sync: ", results[result - 1][2], "ns\t"); + if ( advanced ) Serial.printf("%8s%s%.2f%s", "", "b) Propseg: ", results[result - 1][2] * results[result - 1][4], "ns\t"); + if ( advanced ) Serial.printf("%8s%s%.2f%s", "", "c) Pseg1: ", results[result - 1][2] * results[result - 1][5], "ns\t"); + if ( advanced ) Serial.printf("%8s%s%.2f%s", "", "d) Pseg2: ", results[result - 1][2] * results[result - 1][6], "ns\n"); + + uint32_t scale = (uint32_t)ceil(results[result - 1][2]); + char _sync[(scale*124/1000) + 1] = ""; + for ( uint32_t i = 0; i < (scale*149/1200) + 1; i++ ) _sync[i] = 'a'; + _sync[(scale*124/1000)] = '\0'; + + scale = (uint32_t)ceil(results[result - 1][2] * results[result - 1][4]); + char _prop[(scale*124/1000) + 1] = ""; + for ( uint32_t i = 0; i < (scale*149/1200) + 1; i++ ) _prop[i] = 'b'; + _prop[(scale*124/1000)] = '\0'; + + scale = (uint32_t)ceil(results[result - 1][2] * results[result - 1][5]); + char _pseg1[(scale*124/1000) + 1] = ""; + for ( uint32_t i = 0; i < (scale*149/1200) + 1; i++ ) _pseg1[i] = 'c'; + _pseg1[(scale*124/1000)] = '\0'; + + scale = (uint32_t)ceil(results[result - 1][2] * results[result - 1][6]); + char _pseg2[(scale*124/1000) + 1] = ""; + for ( uint32_t i = 0; i < (scale*149/1200) + 1; i++ ) _pseg2[i] = 'd'; + _pseg2[(scale*124/1000)] = '\0'; + + if ( advanced ) Serial.printf("%9s%s%-s%s%-s%s%-s%s%-s", "[", _sync, "|", _prop, "|", _pseg1, "|", _pseg2, "]\n"); + if ( advanced ) Serial.printf("%8s%-s%-s%-s%-s%-s%-s", "", "|________________________", "|________________________", "|________________________", "|________________________", "|________________________", "|________________________|"); + if ( advanced ) Serial.printf("\n%8s%u%*s%u%*s%u%*s%u%*s%u%*s%u%*s%u%s", "", 0, 23, "", 200, 22, "", 400, 22, "", 600, 22, "", 800, 22, "", 1000, 22, "", 1200, "(ns)\n\n\n"); + } + if ( result == result_old ) { + pseg = nbt - 1 - propseg; + if ( pseg == 3 && ipt == 2 ) { + result = result + 1; + double error = (double) rjw / 20 / nbt * 100; + double error1 = ( ( pseg_1 <= pseg_2 ) ? (double)pseg_1 : (double)pseg_2 ) / 2 / (13 * nbt - pseg_2) * 100; + double sclk = (cpi_clock / prescaler); + double tq = (1000 / (cpi_clock / prescaler)); + double smp = 100 - 2 / nbt * 100; + results[result - 1][0] = prescaler; + results[result - 1][1] = sclk; + results[result - 1][2] = tq; + results[result - 1][3] = nbt; + results[result - 1][4] = propseg; + results[result - 1][5] = 1; + results[result - 1][6] = 2; + results[result - 1][7] = 1; + results[result - 1][8] = (error <= error1) ? error : error1; + results[result - 1][9] = smp; + if ( advanced ) Serial.print(" Prescaler Sclk(MHz) Tq(ns) NBT(#Tq) Propseg(#Tq) Pseg1(#Tq) Pseg2(#Tq) RJW(#Tq) ?f(%) Sample Point(%) CBT Register\n"); + if ( advanced ) Serial.printf(" %2u)", result); + if ( advanced ) Serial.printf(" %4u", prescaler); + if ( advanced ) Serial.printf(" %6.2f", sclk); + if ( advanced ) Serial.printf(" %7.2f", tq); + if ( advanced ) Serial.printf(" %2u", (uint32_t)nbt); + if ( advanced ) Serial.printf(" %2u", (uint32_t)propseg); + if ( advanced ) Serial.printf(" %2u", (uint32_t)1); + if ( advanced ) Serial.printf(" %2u", (uint32_t)2); + if ( advanced ) Serial.printf(" %2u", (uint32_t)1); + if ( advanced ) Serial.printf(" %6.2f", ((error <= error1) ? error : error1)); + if ( advanced ) Serial.printf(" %6.2f", smp); + uint32_t cbt = ((uint32_t)(results[result - 1][4] - 1) << 10); // EPROPSEG + cbt |= ((uint32_t)(results[result - 1][6] - 1) << 0); // EPSEG2 + cbt |= ((uint32_t)(results[result - 1][5] - 1) << 5); // EPSEG1 + cbt |= ((uint32_t)(results[result - 1][7] - 1) << 16); // ERJW + cbt |= ((uint32_t)(results[result - 1][0] - 1) << 21); // EPRESDIV + cbt |= (1UL << 31); // BTF /* use CBT register instead of CTRL1 for FD */ + if ( advanced ) Serial.printf(" 0x%8.08X",cbt); + + if ( advanced ) Serial.printf("\n\t%8s%s%.2f%s%u%s%.2f%s", "", "*** Arbitration phase bit time = ", (results[result - 1][2] * results[result - 1][3] / 1000), "us (", (int)results[result - 1][3], "Tq, Tq=", results[result - 1][2], ")\n"); + if ( advanced ) Serial.printf("\t%8s%s%.2f%s", "", "a) Sync: ", results[result - 1][2], "ns\t"); + if ( advanced ) Serial.printf("%8s%s%.2f%s", "", "b) Propseg: ", results[result - 1][2] * results[result - 1][4], "ns\t"); + if ( advanced ) Serial.printf("%8s%s%.2f%s", "", "c) Pseg1: ", results[result - 1][2] * results[result - 1][5], "ns\t"); + if ( advanced ) Serial.printf("%8s%s%.2f%s", "", "d) Pseg2: ", results[result - 1][2] * results[result - 1][6], "ns\n"); + + uint32_t scale = (uint32_t)ceil(results[result - 1][2]); + char _sync[(scale*124/1000) + 1] = ""; + for ( uint32_t i = 0; i < (scale*149/1200) + 1; i++ ) _sync[i] = 'a'; + _sync[(scale*124/1000)] = '\0'; + + scale = (uint32_t)ceil(results[result - 1][2] * results[result - 1][4]); + char _prop[(scale*124/1000) + 1] = ""; + for ( uint32_t i = 0; i < (scale*149/1200) + 1; i++ ) _prop[i] = 'b'; + _prop[(scale*124/1000)] = '\0'; + + scale = (uint32_t)ceil(results[result - 1][2] * results[result - 1][5]); + char _pseg1[(scale*124/1000) + 1] = ""; + for ( uint32_t i = 0; i < (scale*149/1200) + 1; i++ ) _pseg1[i] = 'c'; + _pseg1[(scale*124/1000)] = '\0'; + + scale = (uint32_t)ceil(results[result - 1][2] * results[result - 1][6]); + char _pseg2[(scale*124/1000) + 1] = ""; + for ( uint32_t i = 0; i < (scale*149/1200) + 1; i++ ) _pseg2[i] = 'd'; + _pseg2[(scale*124/1000)] = '\0'; + + if ( advanced ) Serial.printf("%9s%s%-s%s%-s%s%-s%s%-s", "[", _sync, "|", _prop, "|", _pseg1, "|", _pseg2, "]\n"); + if ( advanced ) Serial.printf("%8s%-s%-s%-s%-s%-s%-s", "", "|________________________", "|________________________", "|________________________", "|________________________", "|________________________", "|________________________|"); + if ( advanced ) Serial.printf("\n%8s%u%*s%u%*s%u%*s%u%*s%u%*s%u%*s%u%s", "", 0, 23, "", 200, 22, "", 400, 22, "", 600, 22, "", 800, 22, "", 1000, 22, "", 1200, "(ns)\n\n\n"); + } + if ( pseg > (ipt + 1) && pseg <= (pseg1 * 2) ) { + if ( (uint32_t)pseg % 2 != 0 ) propseg++; + pseg = (nbt - 1 - propseg) / 2; + result = result + 1; + rjw = ( pseg <= rjw ) ? pseg : rjw; + double pseg_val = nbt - 1 - propseg - pseg; + double error = (double)rjw / 20 / nbt * 100; + double error1 = (((pseg <= pseg_val) ? (double)pseg : (double)pseg_val ) / 2 / (13 * nbt - pseg_val) * 100); + double sclk = (cpi_clock / prescaler); + double tq = (1000 / (cpi_clock / prescaler)); + double smp = 100 - pseg / nbt * 100; + results[result - 1][0] = prescaler; + results[result - 1][1] = sclk; + results[result - 1][2] = tq; + results[result - 1][3] = nbt; + results[result - 1][4] = propseg; + results[result - 1][5] = pseg; + results[result - 1][6] = pseg_val; + results[result - 1][7] = rjw; + results[result - 1][8] = (error <= error1) ? error : error1; + results[result - 1][9] = smp; + if ( advanced ) Serial.print(" Prescaler Sclk(MHz) Tq(ns) NBT(#Tq) Propseg(#Tq) Pseg1(#Tq) Pseg2(#Tq) RJW(#Tq) ?f(%) Sample Point(%) CBT Register\n"); + if ( advanced ) Serial.printf(" %2u)", result); + if ( advanced ) Serial.printf(" %4u", prescaler); + if ( advanced ) Serial.printf(" %6.2f", sclk); + if ( advanced ) Serial.printf(" %7.2f", tq); + if ( advanced ) Serial.printf(" %2u", (uint32_t)nbt); + if ( advanced ) Serial.printf(" %2u", (uint32_t)propseg); + if ( advanced ) Serial.printf(" %2u", (uint32_t)pseg); + if ( advanced ) Serial.printf(" %2u", (uint32_t)pseg_val); + if ( advanced ) Serial.printf(" %2u", (uint32_t)rjw); + if ( advanced ) Serial.printf(" %6.2f", ((error <= error1) ? error : error1)); + if ( advanced ) Serial.printf(" %6.2f", smp); + uint32_t cbt = ((uint32_t)(results[result - 1][4] - 1) << 10); // EPROPSEG + cbt |= ((uint32_t)(results[result - 1][6] - 1) << 0); // EPSEG2 + cbt |= ((uint32_t)(results[result - 1][5] - 1) << 5); // EPSEG1 + cbt |= ((uint32_t)(results[result - 1][7] - 1) << 16); // ERJW + cbt |= ((uint32_t)(results[result - 1][0] - 1) << 21); // EPRESDIV + cbt |= (1UL << 31); // BTF /* use CBT register instead of CTRL1 for FD */ + if ( advanced ) Serial.printf(" 0x%8.08X",cbt); + + if ( advanced ) Serial.printf("\n\t%8s%s%.2f%s%u%s%.2f%s", "", "*** Arbitration phase bit time = ", (results[result - 1][2] * results[result - 1][3] / 1000), "us (", (int)results[result - 1][3], "Tq, Tq=", results[result - 1][2], ")\n"); + if ( advanced ) Serial.printf("\t%8s%s%.2f%s", "", "a) Sync: ", results[result - 1][2], "ns\t"); + if ( advanced ) Serial.printf("%8s%s%.2f%s", "", "b) Propseg: ", results[result - 1][2] * results[result - 1][4], "ns\t"); + if ( advanced ) Serial.printf("%8s%s%.2f%s", "", "c) Pseg1: ", results[result - 1][2] * results[result - 1][5], "ns\t"); + if ( advanced ) Serial.printf("%8s%s%.2f%s", "", "d) Pseg2: ", results[result - 1][2] * results[result - 1][6], "ns\n"); + + uint32_t scale = (uint32_t)ceil(results[result - 1][2]); + char _sync[(scale*124/1000) + 1] = ""; + for ( uint32_t i = 0; i < (scale*149/1200) + 1; i++ ) _sync[i] = 'a'; + _sync[(scale*124/1000)] = '\0'; + + scale = (uint32_t)ceil(results[result - 1][2] * results[result - 1][4]); + char _prop[(scale*124/1000) + 1] = ""; + for ( uint32_t i = 0; i < (scale*149/1200) + 1; i++ ) _prop[i] = 'b'; + _prop[(scale*124/1000)] = '\0'; + + scale = (uint32_t)ceil(results[result - 1][2] * results[result - 1][5]); + char _pseg1[(scale*124/1000) + 1] = ""; + for ( uint32_t i = 0; i < (scale*149/1200) + 1; i++ ) _pseg1[i] = 'c'; + _pseg1[(scale*124/1000)] = '\0'; + + scale = (uint32_t)ceil(results[result - 1][2] * results[result - 1][6]); + char _pseg2[(scale*124/1000) + 1] = ""; + for ( uint32_t i = 0; i < (scale*149/1200) + 1; i++ ) _pseg2[i] = 'd'; + _pseg2[(scale*124/1000)] = '\0'; + + if ( advanced ) Serial.printf("%9s%s%-s%s%-s%s%-s%s%-s", "[", _sync, "|", _prop, "|", _pseg1, "|", _pseg2, "]\n"); + if ( advanced ) Serial.printf("%8s%-s%-s%-s%-s%-s%-s", "", "|________________________", "|________________________", "|________________________", "|________________________", "|________________________", "|________________________|"); + if ( advanced ) Serial.printf("\n%8s%u%*s%u%*s%u%*s%u%*s%u%*s%u%*s%u%s", "", 0, 23, "", 200, 22, "", 400, 22, "", 600, 22, "", 800, 22, "", 1000, 22, "", 1200, "(ns)\n\n\n"); + } + } + result_old = result; + break; + } + } + } + } + } + } + if ( advanced ) Serial.println("\n\n\n"); + if ( nominal_choice && flexdata_choice ) { + if ( nominal_choice > result ) { + if ( advanced ) Serial.println("\n\t*** ERROR: Nominal rate selection out of bounds!\n"); + return 0; + } + uint32_t cbt = ((uint32_t)(results[nominal_choice - 1][4] - 1) << 10); // EPROPSEG + cbt |= ((uint32_t)(results[nominal_choice - 1][6] - 1) << 0); // EPSEG2 + cbt |= ((uint32_t)(results[nominal_choice - 1][5] - 1) << 5); // EPSEG1 + cbt |= ((uint32_t)(results[nominal_choice - 1][7] - 1) << 16); // ERJW + cbt |= ((uint32_t)(results[nominal_choice - 1][0] - 1) << 21); // EPRESDIV + cbt |= (1UL << 31); // BTF + uint32_t fdcbt_setting = setBaudRateFD(config, flexdata_choice, advanced); + if ( fdcbt_setting ) { // SET BOTH CBT AND FDCBT HERE + FLEXCAN_EnterFreezeMode(); + setClock(config.clock); + if ( listen_only == LISTEN_ONLY ) { + FLEXCANb_CBT(_bus) &= ~(1UL << 31); /* clear BTE bit to edit CTRL1 register */ + FLEXCANb_CTRL1(_bus) |= FLEXCAN_CTRL_LOM; + } + else { + FLEXCANb_CBT(_bus) &= ~(1UL << 31); /* clear BTE bit to edit CTRL1 register */ + FLEXCANb_CTRL1(_bus) &= ~FLEXCAN_CTRL_LOM; + } + FLEXCANb_CBT(_bus) = cbt; + FLEXCANb_FDCBT(_bus) = fdcbt_setting; + if ( advanced ) Serial.print("CTRL1: 0x"); + if ( advanced ) Serial.println(FLEXCANb_CTRL1(_bus),HEX); + if ( advanced ) Serial.print("CBT: 0x"); + if ( advanced ) Serial.println(FLEXCANb_CBT(_bus),HEX); + if ( advanced ) Serial.print("FDCBT: 0x"); + if ( advanced ) Serial.println(FLEXCANb_FDCBT(_bus) ,HEX); + if ( advanced ) Serial.print("FDCTRL: 0x"); + if ( advanced ) Serial.println(FLEXCANb_FDCTRL(_bus) ,HEX); + FLEXCAN_ExitFreezeMode(); + return 1; + } + else return 0; + } + else setBaudRateFD(config, 0, advanced); /* continue list printout for FD */ + while ( Serial.read() != 'c' ); + return 1; +} + +FCTPFD_FUNC uint32_t FCTPFD_OPT::setBaudRateFD(CANFD_timings_t config, uint32_t flexdata_choice, bool advanced) { + uint32_t result = 0, result_old = 0; + double results[10][12] = { { 0 } , { 0 } }; + + double baudrate = config.baudrateFD, propdelay = config.propdelay, req_smp = config.sample; + double cpi_clock = config.clock; + + baudrate /= 1000; + double ratio = cpi_clock * 1000 / baudrate; + double nbt_min = 5, nbt_max = 48, propseg_max = 32, propseg = 0, pseg_1, pseg_2; + uint32_t tdcen = 0, ipt = 2; + double temp = 0, ppsegmin, ppsegmax, pseg1 = 8, pseg2 = 8, rjw = 8, prescaler_min = 1, prescaler_max = 1024; + if ( advanced ) Serial.println(" ####################################################################################################################################################"); + if ( advanced ) Serial.println(" #\t\t\t\t\t\t\t\t*** CAN FLEXDATA CONFIGURATION ***\t\t\t\t\t\t #"); + if ( advanced ) Serial.println(" ####################################################################################################################################################\n"); + for ( uint32_t prescaler = prescaler_min; prescaler < prescaler_max; prescaler++ ) { + temp = ratio / prescaler; + for ( double nbt = nbt_min; nbt < nbt_max; nbt++ ) { + if ( temp == nbt ) { + propseg = ceil(propdelay / 1000 * cpi_clock / prescaler); + tdcen = 0; + if ( propseg >= (nbt - 2)) { // finished with debug + ppsegmax = nbt - 3; + ppsegmin = nbt - 1 - pseg1 - pseg2; + if ( ppsegmin < 1 ) ppsegmin = 1; + propseg = round((ppsegmax - ppsegmin) / 2); + tdcen = 1; + } + if ( propseg <= propseg_max ) { + for ( double pseg = 1; pseg <= pseg1; pseg++ ) { + pseg_1 = round((nbt * req_smp / 100) - 1 - propseg); + pseg_2 = nbt - 1 - propseg - pseg_1; + if ( pseg_2 >= ipt && pseg_2 <= pseg2 && pseg_1 > 0 && pseg_1 <= pseg1 ) { + result = result_old + 1; + rjw = ( pseg_2 <= rjw ) ? pseg_2 : rjw; + double sclk = (cpi_clock / prescaler); + double tq = (1000 / (cpi_clock / prescaler)); + double smp = 100 - pseg_2 / nbt * 100; + double tdcoff_val = (!tdcen) ? 0 : cpi_clock / (2 * baudrate / 1000); + results[result - 1][0] = prescaler; + results[result - 1][1] = sclk; + results[result - 1][2] = tq; + results[result - 1][3] = nbt; + results[result - 1][4] = propseg; + results[result - 1][5] = pseg_1; + results[result - 1][6] = pseg_2; + results[result - 1][7] = rjw; + results[result - 1][8] = tdcen; + results[result - 1][9] = tdcoff_val; + results[result - 1][10] = smp; + if ( advanced ) Serial.print(" Prescaler Sclk(MHz) Tq(ns) NBT(#Tq) Propseg(#Tq) Pseg1(#Tq) Pseg2(#Tq) RJW(#Tq) TDCEN TDCOFF Sample Point(%) FDCBT Register\n"); + if ( advanced ) Serial.printf(" %2u)", result); + if ( advanced ) Serial.printf(" %4u", prescaler); + if ( advanced ) Serial.printf(" %6.2f", sclk); + if ( advanced ) Serial.printf(" %7.2f", tq); + if ( advanced ) Serial.printf(" %2u", (uint32_t)nbt); + if ( advanced ) Serial.printf(" %2u", (uint32_t)propseg); + if ( advanced ) Serial.printf(" %2u", (uint32_t)pseg_1); + if ( advanced ) Serial.printf(" %2u", (uint32_t)pseg_2); + if ( advanced ) Serial.printf(" %2u", (uint32_t)rjw); + if ( advanced ) Serial.printf(" %2u", (uint32_t)tdcen); + if ( advanced ) Serial.printf(" %4.2f", tdcoff_val); + if ( advanced ) Serial.printf(" %6.2f", smp); + uint32_t fdcbt = 0; + fdcbt |= ((uint32_t)(results[result - 1][6] - 1) << 0); // FPSEG2 + fdcbt |= ((uint32_t)(results[result - 1][5] - 1) << 5); // FPSEG1 + fdcbt |= ((uint32_t)(results[result - 1][4] - 0) << 10); // FPROPSEG + fdcbt |= ((uint32_t)(results[result - 1][7] - 1) << 16); // FRJW + fdcbt |= ((uint32_t)(results[result - 1][0] - 1) << 20); // FPRESDIV + if ( advanced ) Serial.printf(" 0x%8.08X",fdcbt); + + if ( advanced ) Serial.printf("\n\t%8s%s%.2f%s%u%s%.2f%s", "", "*** Data phase bit time = ", (results[result - 1][2] * results[result - 1][3] / 1000), "us (", (int)results[result - 1][3], "Tq, Tq=", results[result - 1][2], ")\n"); + if ( advanced ) Serial.printf("\t%8s%s%.2f%s", "", "a) Sync: ", results[result - 1][2], "ns\t"); + if ( advanced ) Serial.printf("%8s%s%.2f%s", "", "b) Propseg: ", results[result - 1][2] * results[result - 1][4], "ns\t"); + if ( advanced ) Serial.printf("%8s%s%.2f%s", "", "c) Pseg1: ", results[result - 1][2] * results[result - 1][5], "ns\t"); + if ( advanced ) Serial.printf("%8s%s%.2f%s", "", "d) Pseg2: ", results[result - 1][2] * results[result - 1][6], "ns\n"); + + uint32_t scale = (uint32_t)ceil(results[result - 1][2]); + char _sync[(scale*124/1000) + 1] = ""; + for ( uint32_t i = 0; i < (scale*149/1200) + 1; i++ ) _sync[i] = 'a'; + _sync[(scale*124/1000)] = '\0'; + + scale = (uint32_t)ceil(results[result - 1][2] * results[result - 1][4]); + char _prop[(scale*124/1000) + 1] = ""; + for ( uint32_t i = 0; i < (scale*149/1200) + 1; i++ ) _prop[i] = 'b'; + _prop[(scale*124/1000)] = '\0'; + + scale = (uint32_t)ceil(results[result - 1][2] * results[result - 1][5]); + char _pseg1[(scale*124/1000) + 1] = ""; + for ( uint32_t i = 0; i < (scale*149/1200) + 1; i++ ) _pseg1[i] = 'c'; + _pseg1[(scale*124/1000)] = '\0'; + + scale = (uint32_t)ceil(results[result - 1][2] * results[result - 1][6]); + char _pseg2[(scale*124/1000) + 1] = ""; + for ( uint32_t i = 0; i < (scale*149/1200) + 1; i++ ) _pseg2[i] = 'd'; + _pseg2[(scale*124/1000)] = '\0'; + + if ( advanced ) Serial.printf("%9s%s%-s%s%-s%s%-s%s%-s", "[", _sync, "|", _prop, "|", _pseg1, "|", _pseg2, "]\n"); + if ( advanced ) Serial.printf("%8s%-s%-s%-s%-s%-s%-s", "", "|________________________", "|________________________", "|________________________", "|________________________", "|________________________", "|________________________|"); + if ( advanced ) Serial.printf("\n%8s%u%*s%u%*s%u%*s%u%*s%u%*s%u%*s%u%s", "", 0, 23, "", 200, 22, "", 400, 22, "", 600, 22, "", 800, 22, "", 1000, 22, "", 1200, "(ns)\n\n\n"); + } + if ( result == result_old ) { + pseg = nbt - 1 - propseg; + if ( pseg == 3 && ipt == 2 ) { + result = result + 1; + double sclk = (cpi_clock / prescaler); + double tq = (1000 / (cpi_clock / prescaler)); + double smp = 100 - 2 / nbt * 100; + double tdcoff_val = (!tdcen) ? 0 : cpi_clock / (2 * baudrate / 1000); + results[result - 1][0] = prescaler; + results[result - 1][1] = sclk; + results[result - 1][2] = tq; + results[result - 1][3] = nbt; + results[result - 1][4] = propseg; + results[result - 1][5] = 1; + results[result - 1][6] = 2; + results[result - 1][7] = 1; + results[result - 1][8] = tdcen; + results[result - 1][9] = tdcoff_val; + results[result - 1][10] = smp; + if ( advanced ) Serial.print(" Prescaler Sclk(MHz) Tq(ns) NBT(#Tq) Propseg(#Tq) Pseg1(#Tq) Pseg2(#Tq) RJW(#Tq) TDCEN TDCOFF Sample Point(%) FDCBT Register\n"); + if ( advanced ) Serial.printf(" %2u)", result); + if ( advanced ) Serial.printf(" %4u", prescaler); + if ( advanced ) Serial.printf(" %6.2f", sclk); + if ( advanced ) Serial.printf(" %7.2f", tq); + if ( advanced ) Serial.printf(" %2u", (uint32_t)nbt); + if ( advanced ) Serial.printf(" %2u", (uint32_t)propseg); + if ( advanced ) Serial.printf(" %2u", (uint32_t)1); + if ( advanced ) Serial.printf(" %2u", (uint32_t)2); + if ( advanced ) Serial.printf(" %2u", (uint32_t)1); + if ( advanced ) Serial.printf(" %2u", (uint32_t)tdcen); + if ( advanced ) Serial.printf(" %4.2f", tdcoff_val); + if ( advanced ) Serial.printf(" %6.2f", smp); + uint32_t fdcbt = 0; + fdcbt |= ((uint32_t)(results[result - 1][6] - 1) << 0); // FPSEG2 + fdcbt |= ((uint32_t)(results[result - 1][5] - 1) << 5); // FPSEG1 + fdcbt |= ((uint32_t)(results[result - 1][4] - 0) << 10); // FPROPSEG + fdcbt |= ((uint32_t)(results[result - 1][7] - 1) << 16); // FRJW + fdcbt |= ((uint32_t)(results[result - 1][0] - 1) << 20); // FPRESDIV + if ( advanced ) Serial.printf(" 0x%8.08X",fdcbt); + if ( advanced ) Serial.printf("\n\t%8s%s%.2f%s%u%s%.2f%s", "", "*** Data phase bit time = ", (results[result - 1][2] * results[result - 1][3] / 1000), "us (", (int)results[result - 1][3], "Tq, Tq=", results[result - 1][2], ")\n"); + if ( advanced ) Serial.printf("\t%8s%s%.2f%s", "", "a) Sync: ", results[result - 1][2], "ns\t"); + if ( advanced ) Serial.printf("%8s%s%.2f%s", "", "b) Propseg: ", results[result - 1][2] * results[result - 1][4], "ns\t"); + if ( advanced ) Serial.printf("%8s%s%.2f%s", "", "c) Pseg1: ", results[result - 1][2] * results[result - 1][5], "ns\t"); + if ( advanced ) Serial.printf("%8s%s%.2f%s", "", "d) Pseg2: ", results[result - 1][2] * results[result - 1][6], "ns\n"); + + uint32_t scale = (uint32_t)ceil(results[result - 1][2]); + char _sync[(scale*124/1000) + 1] = ""; + for ( uint32_t i = 0; i < (scale*149/1200) + 1; i++ ) _sync[i] = 'a'; + _sync[(scale*124/1000)] = '\0'; + + scale = (uint32_t)ceil(results[result - 1][2] * results[result - 1][4]); + char _prop[(scale*124/1000) + 1] = ""; + for ( uint32_t i = 0; i < (scale*149/1200) + 1; i++ ) _prop[i] = 'b'; + _prop[(scale*124/1000)] = '\0'; + + scale = (uint32_t)ceil(results[result - 1][2] * results[result - 1][5]); + char _pseg1[(scale*124/1000) + 1] = ""; + for ( uint32_t i = 0; i < (scale*149/1200) + 1; i++ ) _pseg1[i] = 'c'; + _pseg1[(scale*124/1000)] = '\0'; + + scale = (uint32_t)ceil(results[result - 1][2] * results[result - 1][6]); + char _pseg2[(scale*124/1000) + 1] = ""; + for ( uint32_t i = 0; i < (scale*149/1200) + 1; i++ ) _pseg2[i] = 'd'; + _pseg2[(scale*124/1000)] = '\0'; + + if ( advanced ) Serial.printf("%9s%s%-s%s%-s%s%-s%s%-s", "[", _sync, "|", _prop, "|", _pseg1, "|", _pseg2, "]\n"); + if ( advanced ) Serial.printf("%8s%-s%-s%-s%-s%-s%-s", "", "|________________________", "|________________________", "|________________________", "|________________________", "|________________________", "|________________________|"); + if ( advanced ) Serial.printf("\n%8s%u%*s%u%*s%u%*s%u%*s%u%*s%u%*s%u%s", "", 0, 23, "", 200, 22, "", 400, 22, "", 600, 22, "", 800, 22, "", 1000, 22, "", 1200, "(ns)\n\n\n"); + } + if ( pseg > (ipt + 1) && pseg <= (pseg1 * 2) ) { + if ( (uint32_t)pseg % 2 != 0 ) propseg++; + pseg = (nbt - 1 - propseg) / 2; + result++; + rjw = ( pseg <= rjw ) ? pseg : rjw; + double sclk = (cpi_clock / prescaler); + double tq = (1000 / (cpi_clock / prescaler)); + double smp = 100 - pseg / nbt * 100; + double tdcoff_val = (!tdcen) ? 0 : cpi_clock / (2 * baudrate / 1000); + results[result - 1][0] = prescaler; + results[result - 1][1] = sclk; + results[result - 1][2] = tq; + results[result - 1][3] = nbt; + results[result - 1][4] = propseg; + results[result - 1][5] = pseg; + results[result - 1][6] = pseg; + results[result - 1][7] = rjw; + results[result - 1][8] = tdcen; + results[result - 1][9] = tdcoff_val; + results[result - 1][10] = smp; + if ( advanced ) Serial.print(" Prescaler Sclk(MHz) Tq(ns) NBT(#Tq) Propseg(#Tq) Pseg1(#Tq) Pseg2(#Tq) RJW(#Tq) TDCEN TDCOFF Sample Point(%) FDCBT Register\n"); + if ( advanced ) Serial.printf(" %2u)", result); + if ( advanced ) Serial.printf(" %4u", prescaler); + if ( advanced ) Serial.printf(" %6.2f", sclk); + if ( advanced ) Serial.printf(" %7.2f", tq); + if ( advanced ) Serial.printf(" %2u", (uint32_t)nbt); + if ( advanced ) Serial.printf(" %2u", (uint32_t)propseg); + if ( advanced ) Serial.printf(" %2u", (uint32_t)pseg); + if ( advanced ) Serial.printf(" %2u", (uint32_t)pseg); + if ( advanced ) Serial.printf(" %2u", (uint32_t)rjw); + if ( advanced ) Serial.printf(" %2u", (uint32_t)tdcen); + if ( advanced ) Serial.printf(" %4.2f", tdcoff_val); + if ( advanced ) Serial.printf(" %6.2f", smp); + uint32_t fdcbt = 0; + fdcbt |= ((uint32_t)(results[result - 1][6] - 1) << 0); // FPSEG2 + fdcbt |= ((uint32_t)(results[result - 1][5] - 1) << 5); // FPSEG1 + fdcbt |= ((uint32_t)(results[result - 1][4] - 0) << 10); // FPROPSEG + fdcbt |= ((uint32_t)(results[result - 1][7] - 1) << 16); // FRJW + fdcbt |= ((uint32_t)(results[result - 1][0] - 1) << 20); // FPRESDIV + if ( advanced ) Serial.printf(" 0x%8.08X",fdcbt); + if ( advanced ) Serial.printf("\n\t%8s%s%.2f%s%u%s%.2f%s", "", "*** Data phase bit time = ", (results[result - 1][2] * results[result - 1][3] / 1000), "us (", (int)results[result - 1][3], "Tq, Tq=", results[result - 1][2], ")\n"); + if ( advanced ) Serial.printf("\t%8s%s%.2f%s", "", "a) Sync: ", results[result - 1][2], "ns\t"); + if ( advanced ) Serial.printf("%8s%s%.2f%s", "", "b) Propseg: ", results[result - 1][2] * results[result - 1][4], "ns\t"); + if ( advanced ) Serial.printf("%8s%s%.2f%s", "", "c) Pseg1: ", results[result - 1][2] * results[result - 1][5], "ns\t"); + if ( advanced ) Serial.printf("%8s%s%.2f%s", "", "d) Pseg2: ", results[result - 1][2] * results[result - 1][6], "ns\n"); + + uint32_t scale = (uint32_t)ceil(results[result - 1][2]); + char _sync[(scale*124/1000) + 1] = ""; + for ( uint32_t i = 0; i < (scale*149/1200) + 1; i++ ) _sync[i] = 'a'; + _sync[(scale*124/1000)] = '\0'; + + scale = (uint32_t)ceil(results[result - 1][2] * results[result - 1][4]); + char _prop[(scale*124/1000) + 1] = ""; + for ( uint32_t i = 0; i < (scale*149/1200) + 1; i++ ) _prop[i] = 'b'; + _prop[(scale*124/1000)] = '\0'; + + scale = (uint32_t)ceil(results[result - 1][2] * results[result - 1][5]); + char _pseg1[(scale*124/1000) + 1] = ""; + for ( uint32_t i = 0; i < (scale*149/1200) + 1; i++ ) _pseg1[i] = 'c'; + _pseg1[(scale*124/1000)] = '\0'; + + scale = (uint32_t)ceil(results[result - 1][2] * results[result - 1][6]); + char _pseg2[(scale*124/1000) + 1] = ""; + for ( uint32_t i = 0; i < (scale*149/1200) + 1; i++ ) _pseg2[i] = 'd'; + _pseg2[(scale*124/1000)] = '\0'; + + if ( advanced ) Serial.printf("%9s%s%-s%s%-s%s%-s%s%-s", "[", _sync, "|", _prop, "|", _pseg1, "|", _pseg2, "]\n"); + if ( advanced ) Serial.printf("%8s%-s%-s%-s%-s%-s%-s", "", "|________________________", "|________________________", "|________________________", "|________________________", "|________________________", "|________________________|"); + if ( advanced ) Serial.printf("\n%8s%u%*s%u%*s%u%*s%u%*s%u%*s%u%*s%u%s\n\n\n\n", "", 0, 23, "", 200, 22, "", 400, 22, "", 600, 22, "", 800, 22, "", 1000, 22, "", 1200, "(ns)\n\n\n"); + } + } + result_old = result; + break; + } + } + } + } + } + if ( flexdata_choice ) { + if ( flexdata_choice > result ) { + if ( advanced ) Serial.println("\n\t*** ERROR: FD rate selection out of bounds!\n"); + return 0; + } + uint32_t fdcbt = 0; + fdcbt |= ((uint32_t)(results[flexdata_choice - 1][6] - 1) << 0); // FPSEG2 + fdcbt |= ((uint32_t)(results[flexdata_choice - 1][5] - 1) << 5); // FPSEG1 + fdcbt |= ((uint32_t)(results[flexdata_choice - 1][4] - 0) << 10); // FPROPSEG + fdcbt |= ((uint32_t)(results[flexdata_choice - 1][7] - 1) << 16); // FRJW + fdcbt |= ((uint32_t)(results[flexdata_choice - 1][0] - 1) << 20); // FPRESDIV + + FLEXCANb_FDCTRL(_bus) = (FLEXCANb_FDCTRL(_bus) & 0xFFFF60FF); /* clear TDC values */ + if ( ((uint32_t)(results[flexdata_choice - 1][8])) ) { /* enable TDC if available */ + FLEXCANb_FDCTRL(_bus) |= (1UL << 15) | (((uint32_t)(results[flexdata_choice - 1][9])) << 8); + } + return fdcbt; + } + return 0; +} diff --git a/src/can_NAME.cpp b/src/can_NAME.cpp new file mode 100644 index 0000000..82fc806 --- /dev/null +++ b/src/can_NAME.cpp @@ -0,0 +1,161 @@ +//================================================================================================ +/// @file can_NAME.cpp +/// +/// @brief A class that represents a control function's NAME +/// @author Adrian Del Grosso +/// +/// @copyright 2022 Adrian Del Grosso +//================================================================================================ +#include "can_NAME.hpp" +#include "can_stack_logger.hpp" + +namespace isobus +{ + NAME::NAME(std::uint64_t rawNAMEData) : + rawName(rawNAMEData) + { + } + + bool NAME::operator==(const NAME &obj) const + { + return this->rawName == obj.rawName; + } + + bool NAME::get_arbitrary_address_capable() const + { + return (0 != (rawName >> 63)); + } + + void NAME::set_arbitrary_address_capable(bool value) + { + rawName &= ~(static_cast(1) << 63); + rawName |= (static_cast(value) << 63); + } + + std::uint8_t NAME::get_industry_group() const + { + return ((rawName >> 60) & 0x07); + } + + void NAME::set_industry_group(std::uint8_t value) + { + if (value > 0x07) + { + CANStackLogger::error("[NAME]: Industry group out of range, must be between 0 and 7"); + } + rawName &= ~static_cast(0x7000000000000000); + rawName |= (static_cast(value & 0x07) << 60); + } + + std::uint8_t NAME::get_device_class_instance() const + { + return ((rawName >> 56) & 0x0F); + } + + void NAME::set_device_class_instance(std::uint8_t value) + { + if (value > 0x0F) + { + CANStackLogger::error("[NAME]: Device class instance out of range, must be between 0 and 15"); + } + rawName &= ~static_cast(0xF00000000000000); + rawName |= (static_cast(value & 0x0F) << 56); + } + + std::uint8_t NAME::get_device_class() const + { + return ((rawName >> 49) & 0x7F); + } + + void NAME::set_device_class(std::uint8_t value) + { + if (value > 0x7F) + { + CANStackLogger::error("[NAME]: Device class out of range, must be between 0 and 127"); + } + rawName &= ~static_cast(0xFE000000000000); + rawName |= (static_cast(value & 0x7F) << 49); + } + + std::uint8_t NAME::get_function_code() const + { + return ((rawName >> 40) & 0xFF); + } + + void NAME::set_function_code(std::uint8_t value) + { + rawName &= ~static_cast(0xFF0000000000); + rawName |= (static_cast(value & 0xFF) << 40); + } + + std::uint8_t NAME::get_function_instance() const + { + return ((rawName >> 35) & 0x1F); + } + + void NAME::set_function_instance(std::uint8_t value) + { + if (value > 0x1F) + { + CANStackLogger::error("[NAME]: Function instance out of range, must be between 0 and 31"); + } + rawName &= ~static_cast(0xF800000000); + rawName |= (static_cast(value & 0x1F) << 35); + } + + std::uint8_t NAME::get_ecu_instance() const + { + return ((rawName >> 32) & 0x07); + } + + void NAME::set_ecu_instance(std::uint8_t value) + { + if (value > 0x07) + { + CANStackLogger::error("[NAME]: ECU instance out of range, must be between 0 and 7"); + } + rawName &= ~static_cast(0x700000000); + rawName |= (static_cast(value & 0x07) << 32); + } + + std::uint16_t NAME::get_manufacturer_code() const + { + return ((rawName >> 21) & 0x07FF); + } + + void NAME::set_manufacturer_code(std::uint16_t value) + { + if (value > 0x07FF) + { + CANStackLogger::error("[NAME]: Manufacturer code out of range, must be between 0 and 2047"); + } + rawName &= ~static_cast(0xFFE00000); + rawName |= (static_cast(value & 0x07FF) << 21); + } + + std::uint32_t NAME::get_identity_number() const + { + return (rawName & 0x001FFFFF); + } + + void NAME::set_identity_number(uint32_t value) + { + if (value > 0x001FFFFF) + { + CANStackLogger::error("[NAME]: Identity number out of range, must be between 0 and 2097151"); + } + rawName &= ~static_cast(0x1FFFFF); + rawName |= static_cast(value & 0x1FFFFF); + } + + std::uint64_t NAME::get_full_name() const + { + return rawName; + } + + void NAME::set_full_name(std::uint64_t value) + { + rawName = value; + } + +} // namespace isobus diff --git a/src/can_NAME.hpp b/src/can_NAME.hpp new file mode 100644 index 0000000..365eb6b --- /dev/null +++ b/src/can_NAME.hpp @@ -0,0 +1,441 @@ +//================================================================================================ +/// @file can_NAME.hpp +/// +/// @brief A class that represents a control function's NAME +/// @author Adrian Del Grosso +/// +/// @copyright 2022 Adrian Del Grosso +//================================================================================================ + +#ifndef CAN_NAME_HPP +#define CAN_NAME_HPP + +#include +#include + +namespace isobus +{ + //================================================================================================ + /// @class NAME + /// + /// @brief A class that represents an ISO11783 control function NAME from an address claim. + //================================================================================================ + class NAME + { + public: + /// @brief The encoded components that comprise a NAME + enum class NAMEParameters + { + IdentityNumber, ///< Usually the serial number of the ECU, unique for all similar control functions + ManufacturerCode, ///< The J1939/ISO11783 manufacturer code of the ECU with this NAME + EcuInstance, ///< The ECU instance of the ECU with this NAME. Usually increments in NAME order with similar CFs + FunctionInstance, ///< The function instance of the ECU. Similar to Virtual Terminal number. + FunctionCode, ///< The function of the ECU, as defined by ISO11783 + DeviceClass, ///< Also known as the vehicle system from J1939, describes general ECU type + DeviceClassInstance, ///< The instance number of this device class + IndustryGroup, ///< The industry group associated with this ECU, such as "agricultural" + ArbitraryAddressCapable ///< Defines if this ECU supports address arbitration + }; + + /// @brief See ISO11783-1 and www.isobus.net For complete descriptions of the ISO NAME function codes + /// @note Functions are defined in relation to industry group and device class. See www.isobus.net for more info. + enum class Function : std::uint8_t + { + // Common Functions + Engine = 0, ///< The typical mechanical power source of the machine + AuxiliaryPowerUnit = 1, ///< Power source for operating systems without the use of the prime drive engine + ElectricPropulsionControl = 2, ///< Control system which operates the drive mechanism when it is electrically powered + Transmission = 3, ///< Mechanical system for altering the speed vs. torque output of the engine + BatteryPackMonitor = 4, ///< Monitors the condition of a battery pack + ShiftControl = 5, ///< Control Unit that determines and transmits onto the network the gear desired by the operator + PowerTakeOffRearOrPrimary = 6, ///< System that controls the mechanical power derived from a prime engine and used to operate auxiliary items + SteeringAxle = 7, ///< Adjusts attack angle of steering axle + DrivingAxle = 8, ///< Adjusts attack angle of driving axle + SystemControlBrakes = 9, ///< Controls service braking system electronically + SteerAxleControlBrakes = 10, ///< Control for actuating the service brakes on a steered axle + DriveAxleControlBrakes = 11, ///< Control for actuating the service brakes on a drive axle + EngineRetarder = 12, ///< Controller for the retarder capabilities of the engine + DrivelineRetarder = 13, ///< Controller for the retarder capabilities of the driveline + CruiseControl = 14, ///< Control system for maintaining the vehicle's speed at a fixed operator selectable value + FuelSystem = 15, ///< Controls fuel flow from the tank to the filter to the water removal/separator to the engine and then back to the tank + SteeringControl = 16, ///< Controls steering in steer-by-wire + SteerAxleSuspensionControl = 17, ///< Control system for the suspension of a steered axle + DriveAxleSuspensionControl = 18, ///< Control system for the suspension of a driven axle + InstrumentCluster = 19, ///< Gauge display for a vehicle, usually in the cab + TripRecorder = 20, ///< System for accumulating data versus travel of the vehicle + CabClimateControl = 21, ///< System for controlling the climate within the cab of the vehicle + AerodynamicControl = 22, ///< Modifies drag by altering body panels + VehicleNavigation = 23, ///< System associated with the vehicles physical location + VehicleSecurity = 24, ///< System for comparing operator-provided data sequences against reference + NetworkInterconnectUnit = 25, ///< ECU for connecting different network segments together + BodyControl = 26, ///< Can handle suspension control for the body sections independent from the axle sections + PowerTakeOffFrontOrSecondary = 27, ///< System that controls the mechanical power derived from a prime engine and used to operate auxiliary items + OffVehicleGateway = 28, ///< ECU for connecting between vehicle network(s) and an off-vehicle system or network + VirtualTerminal = 29, ///< General-purpose intelligent display with a specific message set defined in ISO 11783-6 + ManagementComputerOne = 30, ///< Manages vehicle systems, i.e. powertrain + PropulsionBatteryCharger = 31, ///< Unit used to charge propulsion batteries in an electric vehicle + HeadwayControl = 32, ///< Forward-looking collision avoidance, collision warning, speed controller, or speed control + SystemMonitor = 33, ///< Generic system monitor + HydraulicPumpControl = 34, ///< Pump controller that provides hydraulic power + SystemControlSuspension = 35, ///< Controller responsible for coordinating the over-all suspension of a vehicle + SystemControlPneumatic = 36, ///< Controller responsible for coordinating the pneumatics of a vehicle + CabController = 37, ///< Controller located in/near vehicle cab used to perform functions that are grouped together for convenience + TirePressureControl = 38, ///< Unit that provides control of centralized tire inflation + IgnitionControl = 39, ///< Unit for altering the ignition of an engine + SeatControl = 40, ///< System for controlling the seats (operator and passenger) within the cab + OperatorControlsLighting = 41, ///< Controller for sending the operator lighting controls messages + WaterPumpControl = 42, ///< Controller for a water pump mounted on the vehicle/machine + TransmissionDisplay = 43, ///< Display designed specifically to display transmission information + ExhaustEmissionControl = 44, ///< Emissions controller + VehicleDynamicStabilityControl = 45, ///< Stability controller + OilSystemMonitor = 46, ///< Monitors oil level, life, temperature + InformationSystemControl = 47, ///< Information management for a vehicle's application, such as cargo management + RampControl = 48, ///< Loading unloading chairlift, ramps, lifts or tailgates + ClutchConverterControl = 49, ///< When transmission is distributed this handles torque converter lock-up or engine-transmission connection + AuxiliaryHeater = 50, ///< Primary heat typically being taken from the engine coolant + ForwardLookingCollisionWarningSystem = 51, ///< System which detects and warns of impending collision + ChassisControl = 52, ///< Controls the chassis (not body or cab) components + AlternatorElectricalChargingSystem = 53, ///< Vehicle's primary on-board charging controller + CommunicationsCellular = 54, ///< Radio communications unit designed to communicate via the cellular telephone system + CommunicationsSatellite = 55, ///< Radio communications unit designed specifically to communicate via some satellite system + CommunicationsRadio = 56, ///< Radio unit designed specifically to communicate via a terrestrial p2p system + OperatorControlsSteeringColumn = 57, ///< Unit that gathers the operator inputs from switches/levers/etc and transmits associated messages + FanDriveControl = 58, ///< Primary control system affecting the operation of the main cooling fan + Starter = 59, ///< Mechanical system for initiating rotation in an engine + CabDisplayCab = 60, ///< Used for a fairly elaborate in cab display, non VT and non instrument cluster + FileServerOrPrinter = 61, ///< Printing or file storage unit on the network + OnboardDiagnosticUnit = 62, ///< Tool that can be permanently mounted on the vehicle and which may not support all of the ISO 11783-12 messages + EngineValveController = 63, ///< Control system used to manipulate the actuation of engine intake or exhaust + EnduranceBraking = 64, ///< Sum of all units in a vehicle which enable the driver to brake with virtually no friction + GasFlowMeasurement = 65, ///< Provides measurement of gas flow rates and associated parameters + IOController = 66, ///< Reporting and/or control unit for external input and output channels + ElectricalSystemController = 67, ///< Can include load centres, fuse boxes and power distribution boards + AftertreatmentSystemGasMeasurement = 68, ///< Sensor for measuring gas properties before and after an aftertreatment system + EngineEmissionAftertreatmentSystem = 69, ///< Engine Emission Aftertreatment System + AuxiliaryRegenerationDevice = 70, ///< Auxiliary Regeneration Device used as part of an after treatment system + TransferCaseControl = 71, ///< The device which controls the selection of the number of drive wheels (for example 2 or 4 wheel drive) + CoolantValveController = 72, ///< Device used to control the flow of coolant (water, oil, air, etc�) for any thermal management system + RolloverDetectionControl = 73, ///< Device designed for detection of vehicle rollover + LubricationSystem = 74, ///< The Lubrication System pumps quantities of lubricant to each machine/vehicle joint that need to be lubricated + SupplementalFan = 75, ///< This is an auxillary fan used for additional cooling. It is in addition to the primary cooling fan. + TemperatureSensor = 76, ///< Device which measures temperature. + FuelPropertiesSensor = 77, ///< Device which measures fuel properties + FireSuppressionSystem = 78, ///< Fire Suppression System + PowerSystemsManager = 79, ///< Controller application that manages the power output of one or more power systems. See also IG 5 Function 129 - Generator Set Controller + ElectricPowertrain = 80, ///< Controller application in charge of controlling and coordinating the operation of an electric drive system + HydraulicPowertrain = 81, ///< Controller application in charge of controlling and coordinating the operation of a hydraulic drive system + FileServer = 82, ///< A file storage unit on the network - A permanent connection may exist and the unit is expected to store data (as in magnetic or eeprom devices). See Function 61 for a combination File Server/Printer unit + Printer = 83, ///< A printing unit on the network - A permanent connection may exist and the unit is expected to be able to print (paper type output). See Function 61 for a combination File Server/Printer unit + StartAidDevice = 84, ///< Device that controls hardware and/or conveys information related to assisting an engine in starting, such as a glow plug, grid heater, etc. + EngineInjectionControlModule = 85, ///< A device for direct or port injection of fuel for engine combustion and with which an engine controller may communicate + EVCommunicationController = 86, ///< A controller or application that manages the connection to an external power source, i.e. the Electric Vehicle Supply Equipment + DriverImpairmentDevice = 87, ///< Device which prevents the starting of a vehicle motor due to driver impairment. Example is an alcohol interlock device + ElectricPowerConverter = 88, ///< An inverter or converter used to transform AC or DC power to or from an AC or DC source + SupplyEquipmentCommunicationController = 89, ///< Typically part of an Electrical Vehicle Supply Equipment (EVSE) in an electric vehicle charging station + VehicleAdapterCommunicationController = 90, ///< A controller inside of the adapter placed between an Electric Vehicle Supply Equipment (EVSE) vehicle connector and the vehicle inlet + RateControl = 128, ///< Control of the rate of product placed on or in the soil + SectionOnOffControl = 129, ///< On/Off control of individual sections + PositionControl = 131, ///< Multiple axis position control of a device's application boom + MachineControl = 132, ///< Control of outputs including adjustment of any ancillary functions like position and/or rotation speed. + ProductFlow = 133, ///< Measuring function to monitor the current product flow. + ProductLevel = 134, ///< Measuring function to monitor the current product level in the bin/tank + DepthOrHeightControl = 135, ///< Control of the depth of the tool in the soil, or control of the height of the boom above the surface of the soil or above the height of the standing crop + FrameControl = 136, ///< Control of the folding and unfolding of the frame of the device. Control used to change between transport and field operation position. Not to be used for field operations + + // Non-specific system (Device class 0) industry group and vehicle system + OffBoardDiagnosticServiceTool = 129, ///< Off-board diagnostic-service tool + OnBoardDiagnosticDataLogger = 130, ///< On-board data logger + PCKeyboard = 131, ///< A user interface similar to a PC keyboard + SafetyRestraintSystem = 132, ///< The safety restraint system can be for controlling activation of airbags, belt tensioners, roll over protection systems, etc + Turbocharger = 133, ///< Turbocharger used on the engine + GroundBasedSpeedSensor = 134, ///< Measures actual ground speed of a vehicle with a device such as radar or other such devices + Keypad = 135, ///< An operator input device used to control machine functions or provide data + HumiditySensor = 136, ///< Device which measures air humidity + ThermalManagementSystemController = 137, ///< This device controls all devices that may be used in a thermal management system including Jacket Water Cooling, Charged Air Cooling, Transmission Cooling, Electronics Cooling, Aux Oil Cooling, etc + BrakeStrokeAlert = 138, ///< The device that evaluates air brake stroke for normal stroke, over stroke, dragging brake, or a non-functioning brake actuator and is permanently mounted on the vehicle + OnBoardAxleGroupScale = 139, ///< The device that determines axle group weights and is permanently mounted on the vehicle. + OnBoardAxleGroupDisplay = 140, ///< The device that displays axle group weights and may be permanently mounted on the vehicle + BatteryCharger = 141, ///< A device used to charge batteries in a vehicle from an off-board source of electrical energy. + TurbochargerCompressorBypass = 142, ///< Device used to control the flow across the Compressor Bypass + TurbochargerWastegate = 143, ///< Device used to control the position of the Wastegate to adjust the exhaust flow + Throttle = 144, ///< Device used to control the air/fuel mixture into the cylinders for combustion + InertialSensor = 145, ///< Detects a change in geographic position, a change in velocity, and/or a change in orientation. This may include but is not limited to an accelerometer, gyroscopes, etc + FuelActuator = 146, ///< Device used to control the flow of fuel (or fuel rack) on a engine + EngineExhaustGasRecirculation = 147, ///< Device that controls the engine exhaust gas recirculation system + EngineExhaustBackpressure = 148, ///< Device that controls the engine exhaust backpressure + OnBoardBinWeightingScale = 149, ///< Device that determines bin weights and is permanently mounted on the vehicle + OnBoardBinWeighingScaleDisplay = 150, ///< Device that displays bin weights and may be permanently mounted on the vehicle + EngineCylinderPressureMonitoringSystem = 151, ///< System designed to monitor engine cylinder pressures and provide combustion information + ObjectDetection = 152, ///< System for detection of undesirable objects in the product flow + ObjectDetectionDisplay = 153, ///< Display designed specifically for displaying and managing object detection information + ObjectDetectionSensor = 154, ///< Detects the presence of objects within a region. + PersonnelDetectionDevice = 155, /// < Device for the detection of personnel in proximity to a vehicle. + + // ******** On-Highway (Industry Group 1) ******** + // Non-specific system (Device class 0) industry group 1 + Tachograph = 128, ///< Records engine speed over a period of time + DoorController = 129, ///< Door controller + ArticulationTurntableControl = 130, ///< Control of the articulation turntable for joined body buses + BodyToVehicleInterfaceControl = 131, ///< Manages interaction of vehicle functions and body functions. May be combination of body signals and gateway functionalities + SlopeSensor = 132, ///< Sensor for measuring a slope along an axis + RetarderDisplay = 134, ///< Display module that shows information pertaining to the retarder (driveline or exhaust or engine) + DifferentialLockController = 135, ///< Differential Lock Controller + LowVoltageDisconnect = 136, ///< Monitors the voltage of the starting battery bank and disconnects predetermined auxiliary loads to assure enough power is left in the batteries for starting the vehicle + RoadwayInformation = 137, ///< Devices that use this function will provide information relevant to the roadway in which the vehicle is traveling. This includes attributes such as intersections, grade, speed limit, number of lanes, etc + AutomatedDriving = 138, ///< Automated Driving System. See SAE J3016 + + // Non-specific system (Device class 0) Tractor industry group 1 + ForwardRoadImageProcessing = 128, ///< Determine vehicle position from lane markings. Performance, Advisory & Warning only + FifthWheelSmartSystem = 129, ///< Any systems relative to the operation & status/safety monitoring of the fifth wheel coupler system + CatalystFluidSensor = 130, ///< The Catalyst Fluid Sensor can measure the catalyst fluid temperature, the catalyst fluid level and the catalyst fluid quality + AdaptiveFrontLightingSystem = 131, ///< System used to adjust the vehicle front lighting for the current operating conditions (city, highway, country,etc.) + IdleControlSystem = 132, ///< The device automatically starts and stops the engine when the vehicle is stationary for the purpose of reducing excess idle time. + UserInterfaceSystem = 133, ///< The User Interface System is a two way interface system. Uses of this may include, but are not limited to, setting climate conditions, setting system parameters, and/or logging operating conditions + + // ******** Agriculture (Industry Group 2) ******** + // Non-specific system (Device class 0) industry group 2 + NonVirtualTerminalDisplay = 128, ///< An operator display connected to the 11783 network that cannot perform as a Virtual Terminal and is not allowed to send a VT status message + OperatorControlsMachineSpecific = 129, ///< Operator interface controls, either auxiliary control inputs or a proprietary means, provided by a control function + TaskController = 130, ///< A control function on the 11783 network that is responsible for the sending, receiving and logging of process data as defined in ISO11783-10. + ForeignObjectDetection = 133, ///< Detection of undesirable objects in the product flow + TractorECU = 134, ///< (TECU) An interface unit between the tractor and the implement bus representing the tractor and its messages on the 11783 network + SequenceControlMaster = 135, ///< The master controller in the Sequence Control System as defined in ISO11783-14 + ProductDosing = 136, ///< Control function that adds an active ingredient to a liquid carrier for application to fields (direct injection systems) + ProductTreatment = 137, ///< Control function that mixes a treatment to a dry product applied to or harvested from fields + DataLogger = 139, ///< Data logger as defined in ISO11783-10 for non-task related data logging + DecisionSupport = 140, ///< A control function which is used by the operator or by another control function to configure an operation to perform optimally under the current circumstances + LightingController = 141, ///< Control function that controls electrical power to the lights and reports the status of the lights. This control function can be used on trailers or implements + TIMServer = 142, ///< Control function that represents a Tractor Implement Management (TIM) Server + + // Tractor (Device class 1) Industry Group 2 + AuxiliaryValveControl = 129, ///< Control of addressed tractor mounted auxiliary valves + RearHitchControl = 130, ///< Control of the rear hitch of an agricultural tractor + FrontHitchControl = 131, ///< Control of the front hitch of an agricultural tractor + CenterHitchControl = 134, ///< Control of center hitch of an agricultural tractor + + // Planters/Seeders (Device Class 4) Industry Group 2 + DownPressure = 137, ///< Control of the ground contact pressure on the product delivery unit for optimal operation e.g. pressure on openers for penetrating the ground + + // Fertilizers (Device Class 5) Industry Group 2 + ProductPressure = 130, ///< Monitoring of the pressure of the product in the delivery booms + + // Harvesters (Device Class 7) Industry Group 2 + TailingMonitor = 128, ///< Measuring system to monitor the quantity of unthreshed material returned to threshing machine + HeaderControl = 129, ///< Control of the headers reel height and rotation and material delivery rate + ProductLossMonitor = 130, ///< Measuring system to monitor the amount of grain being delivered back onto the soil + HarvesterProductMoisture = 131, ///< Measuring system to monitor the moisture content of the grain + + // Forage (Device Class 9) Industry Group 2 + TwineWrapperControl = 128, ///< Control of the wrapping of twine around a bale before discharge from the baler + ProductPackagingControl = 129, ///< Control of packaging process for the forage material. + ForageProductMoisture = 131, ///< Measuring system to monitor the moisture of the forage content. + + // Transport/Trailer (Device Class 11) Industry Group 2 + UnloadControl = 136, ///< Control of trailer unloading process + + // Sensor Systems (Device Class 17) Industry Group 2 + GuidanceFeeler = 128, ///< Mechanical function for determining row position in the field + CameraSystem = 129, ///< Provides images or processed data for control operations. + CropScouting = 130, ///< Measures vegetation parameters in a standing crop + MaterialPropertiesSensing = 131, ///< Sensing system to detect material properties like density, particle size, color or constituents + InertialMeasurementUnit = 132, /// < A sensor unit providing inertial measurements + ProductMass = 135, ///< Measuring function to monitor the mass of the product + VibrationKnock = 136, ///< Measuring function to determine the vibration or knock behaviour of a system + WeatherInstruments = 137, ///< The "Weather Instruments" function code shall be used by ISO11783 compliant Weather Instruments + SoilScouting = 138, ///< Soil Sensor to measure different soil physical parameters. One example of a soil sensor is a system that measures the apparent conductivity + + // ******** Construction (Industry Group 3) ******** + // Non-specific system (Device class 0) Industry Group 3 + ConstructionSupplementalEngineControlSensing = 128, ///< Supplemental Engine Control Sensing + LaserReceiver = 129, ///< Laser Receiver + LandLevelingSystemOperatorInterface = 130, ///< A component that allows the user to control the Land Leveling System and display information about the operation of the system + LandLevelingElectricMast = 131, ///< Land Leveling Electric Mast + SingleLandLevelingSystemSupervisor = 132, ///< Single Land Leveling System Supervisor + LandLevelingSystemDisplay = 133, ///< Land Leveling System Display + LaserTracer = 134, ///< Laser Tracer + LoaderControl = 135, ///< Loader control unit + ConstructionEquipmentSlopeSensor = 136, ///< Measures the slope along a axis. + LiftArmControl = 137, ///< Controller whose primary purpose is to control the lift arms and tilt functions on a construction loader, skid steer loader, or similar machine. + SupplementalSensorProcessingUnits = 138, ///< An ECU functioning as an I/O module connected to the bus with the designed purpose of data collection (input or output) + HydraulicSystemPlanner = 139, ///< Coordinates the functions of a number of valve controllers + HydraulicValveController = 140, ///< The valve controller will typically control the flow of oil to a specific cylinder. + JoystickControl = 141, ///< Joystick Control + RotationSensor = 142, ///< A device that measures the rotational angle around an axis + SonicSensor = 143, ///< A device that measures distance via ultrasonic pulse/echo range techniques. + SurveyTotalStationTarget = 144, ///< A survey total station target shall be located on a construction machine and shall be connected to the CAN network. It is targeted by the total station device. + HeadingSensor = 145, ///< A device that measures vehicle azimuth. + AlarmDevice = 146, ///< Device that provides an audible and/or visual alarm + + // Skid Steer Loader (Device Class 1) Industry Group 3 + SkidSteerMainController = 128, ///< Main controller for a skid steer machine + + // Crawler (Device Class 4) Industry Group 3 + BladeController = 128, ///< Controller for blade height. + + // Grader (Device Class 8) Industry Group 3 + HFWDController = 128, ///< Hydraulic front wheel drive controller + + // ******** Marine (Industry Group 4) ******** + Alarm1SystemControlForMarineEngines = 128, ///< The ECU that controls the Alarm functions on an engine of a Marine System. + ProtectionSystemForMarineEngines = 129, ///< The first ECU that controls the Protection functions on the first engine of a Marine System. + DisplayForProtectionSystemForMarineEngines = 130, ///< The ECU that provides the display of information and/or indicators associated specifically with the protection system on an engine of a Marine System. + + // Power Management And Lighting System (Device Class 30) + Switch = 130, ///< A CAN switch + Load = 140, ///< Load + + // Steering systems (Device class 40) + FollowUpController = 130, ///< Follow-up controller + ModeController = 140, ///< Mode Controller + AutomaticSteeringController = 150, ///< Automatic Steering Controller + HeadingSensors = 160, ///< Heading Sensors + + // Propulsion Systems + EngineRoomMonitoring = 130, ///< Marine engine room monitoring system + EngineInterface = 140, ///< Marine Engine interface + EngineController = 150, ///< Marine Engine Controller + EngineGateway = 160, ///< Marine engine gateway + ControlHead = 170, ///< Marine electronic control head + Actuator = 180, ///< Marine actuator + GaugeInterface = 190, ///< Marine Gauge Interface + GaugeLarge = 200, ///< Large marine gauge + GaugeSmall = 210, ///< Small marine gauge + PropulsionSensorsAndGateway = 220, ///< Propulsion sensors and gateway + + // Navigation Systems + SounderDepth = 130, ///< Sounder. + GlobalNavigationSatelliteSystem = 145, ///< Marine GNSS + LoranC = 150, ///< Marine Loran C + SpeedSensors = 155, ///< Marine speed sensors + TurnRateIndicator = 160, ///< Marine turn rate indicator + IntegratedNavigation = 170, ///< Marine integrated navigation + RadarOrRadarPlotting = 200, ///< Radar and/or Radar Plotting + ElectronicChartDisplayAndInformationSystem = 205, ///< ECDIS + ElectronicChartSystem = 210, ///< ECS + DirectionFinder = 220, ///< Direction Finder + + // Communications Systems + EmergencyPositionIndicatingBeacon = 130, ///< EPIRB + AutomaticIdentificationSystem = 140, ///< Marine automatic identification system + DigitalSelectiveCalling = 150, ///< DSC + DataReceiver = 160, ///< Marine data receiver + Satellite = 170, ///< A satellite ? + RadioTelephoneMF_HF = 180, ///< Radio - Telephone(MF / HF) + RadioTelephoneVHF = 190, ///< Radio - Telephone(VHF) + TimeDateSystems = 130, ///< Marine time date system + VoyageDataRecorder = 140, ///< Marine Voyage Data Recorder + IntegratedInstrumentation = 150, ///< Marine Integrated Instrumentation + GeneralPurposeDisplays = 160, ///< Marine General Purpose Displays + GeneralSensorBox = 170, ///< Marine General Sensor Box + MarineWeatherInstruments = 180, ///< Marine Weather Instruments + TransducerGeneral = 190, ///< Marine Transducer/general + NMEA0183Converter = 200, ///< NMEA 0183 Converter + + // ******** Industrial / Process Control (Industry Group 5) ******** + GeneratorSupplementalEngineControlSensing = 128, ///< Supplemental Engine Control Sensing + GeneratorSetController = 129, ///< Generator set controller used to collect data and control. + GeneratorVoltageRegulator = 130, ///< Generator Voltage Regulator + ChokeActuator = 131, ///< Device used to Control the flow of air on a Gas Engine. + WellStimulationPump = 132, ///< Device which communicates operating parameters of a well stimulation pump used in oil and gas drilling applications. + + MaxFunctionCode = 255 ///< Max allocated function code + }; + + /// @brief A useful way to compare session objects to each other for equality + /// @param[in] obj The rhs of the operator + /// @returns `true` if the objects are "equal" + bool operator==(const NAME &obj) const; + + /// @brief A structure that tracks the pair of a NAME parameter and associated value + using NameParameterFilter = std::pair; + + /// @brief Constructor for a NAME + /// @param[in] rawNAMEData The raw 64 bit NAME of an ECU + explicit NAME(std::uint64_t rawNAMEData = 0); + + /// @brief Returns if the ECU is capable of address arbitration + /// @returns true if the ECU can arbitrate addresses + bool get_arbitrary_address_capable() const; + + /// @brief Sets the data in the NAME that corresponds to the arbitration capability of the ECU + /// @param[in] value True if the ECU supports arbitration, false if not + void set_arbitrary_address_capable(bool value); + + /// @brief Returns the industry group encoded in the NAME + /// @returns The industry group encoded in the NAME + std::uint8_t get_industry_group() const; + + /// @brief Sets the industry group encoded in the NAME + /// @param[in] value The industry group to encode in the NAME + void set_industry_group(std::uint8_t value); + + /// @brief Returns the device class (vehicle system) encoded in the NAME + /// @returns The device class (vehicle system) encoded in the NAME + std::uint8_t get_device_class_instance() const; + + /// @brief Sets the device class instance (vehicle system) to be encoded in the NAME + /// @param[in] value The device class instance (vehicle system) to be encoded in the NAME + void set_device_class_instance(std::uint8_t value); + + /// @brief Returns the device class (vehicle system) encoded in the NAME + /// @returns The device class (vehicle system) encoded in the NAME + std::uint8_t get_device_class() const; + + /// @brief Sets the device class (vehicle system) to be encoded in the NAME + /// @param[in] value The device class (vehicle system) to be encoded in the NAME + void set_device_class(std::uint8_t value); + + /// @brief Gets the function code encoded in the NAME + /// @returns The function code encoded in the NAME + std::uint8_t get_function_code() const; + + /// @brief Sets the function code encoded in the NAME + /// @param[in] value The function code to be encoded in the NAME + void set_function_code(std::uint8_t value); + + /// @brief Gets the function instance encoded in the NAME + /// @returns The function instance encoded in the NAME + std::uint8_t get_function_instance() const; + + /// @brief Sets the function instance encoded in the NAME + /// @param[in] value The function instance to be encoded in the NAME + void set_function_instance(std::uint8_t value); + + /// @brief Gets the ecu instance encoded in the NAME + /// @returns The ecu instance encoded in the NAME + std::uint8_t get_ecu_instance() const; + + /// @brief Sets the ecu instance encoded in the NAME + /// @param[in] value The ecu instance to be encoded in the NAME + void set_ecu_instance(std::uint8_t value); + + /// @brief Gets the manufacturer code encoded in the NAME + /// @returns The manufacturer code encoded in the NAME + std::uint16_t get_manufacturer_code() const; + + /// @brief Sets the manufacturer code encoded in the NAME + /// @param[in] value The manufacturer code to be encoded in the NAME + void set_manufacturer_code(std::uint16_t value); + + /// @brief Gets the identity number encoded in the NAME + /// @returns The identity number encoded in the NAME + std::uint32_t get_identity_number() const; + + /// @brief Sets the identity number encoded in the NAME + /// @param[in] value The identity number to be encoded in the NAME + void set_identity_number(std::uint32_t value); + + /// @brief Gets the raw 64 bit NAME + /// @returns The raw 64 bit NAME + std::uint64_t get_full_name() const; + + /// @brief Sets the raw, encoded 64 bit NAME + /// @param[in] value The raw, encoded 64 bit NAME + void set_full_name(std::uint64_t value); + + private: + std::uint64_t rawName; ///< A raw, 64 bit NAME encoded with all NAMEParameters + }; +} // namespace isobus + +#endif // CAN_NAME_HPP diff --git a/src/can_NAME_filter.cpp b/src/can_NAME_filter.cpp new file mode 100644 index 0000000..f0adf8d --- /dev/null +++ b/src/can_NAME_filter.cpp @@ -0,0 +1,98 @@ +//================================================================================================ +/// @file can_NAME_filter.cpp +/// +/// @brief Defines a filter value for an ISONAME component. Used to tell the stack what kind of +/// ECU you want to talk to when creating a partnered control function. +/// @author Adrian Del Grosso +/// +/// @copyright 2022 Adrian Del Grosso +//================================================================================================ +#include "can_NAME_filter.hpp" + +namespace isobus +{ + NAMEFilter::NAMEFilter(NAME::NAMEParameters nameParameter, std::uint32_t parameterMatchValue) : + parameter(nameParameter), + value(parameterMatchValue) + { + } + + NAME::NAMEParameters NAMEFilter::get_parameter() const + { + return parameter; + } + + std::uint32_t NAMEFilter::get_value() const + { + return value; + } + + bool NAMEFilter::check_name_matches_filter(const NAME &nameToCompare) const + { + bool retVal = false; + + switch (parameter) + { + case NAME::NAMEParameters::IdentityNumber: + { + retVal = (nameToCompare.get_identity_number() == value); + } + break; + + case NAME::NAMEParameters::ManufacturerCode: + { + retVal = (nameToCompare.get_manufacturer_code() == value); + } + break; + + case NAME::NAMEParameters::EcuInstance: + { + retVal = (nameToCompare.get_ecu_instance() == value); + } + break; + + case NAME::NAMEParameters::FunctionInstance: + { + retVal = (nameToCompare.get_function_instance() == value); + } + break; + + case NAME::NAMEParameters::FunctionCode: + { + retVal = (nameToCompare.get_function_code() == value); + } + break; + + case NAME::NAMEParameters::DeviceClass: + { + retVal = (nameToCompare.get_device_class() == value); + } + break; + + case NAME::NAMEParameters::DeviceClassInstance: + { + retVal = (nameToCompare.get_device_class_instance() == value); + } + break; + + case NAME::NAMEParameters::IndustryGroup: + { + retVal = (nameToCompare.get_industry_group() == value); + } + break; + + case NAME::NAMEParameters::ArbitraryAddressCapable: + { + retVal = (nameToCompare.get_arbitrary_address_capable() == (0 != value)); + } + break; + + default: + { + } + break; + } + return retVal; + } + +} // namespace isobus \ No newline at end of file diff --git a/src/can_NAME_filter.hpp b/src/can_NAME_filter.hpp new file mode 100644 index 0000000..832a612 --- /dev/null +++ b/src/can_NAME_filter.hpp @@ -0,0 +1,52 @@ +//================================================================================================ +/// @file can_NAME_filter.hpp +/// +/// @brief Defines a filter value for an ISONAME component. Used to tell the stack what kind of +/// ECU you want to talk to when creating a partnered control function. +/// @author Adrian Del Grosso +/// +/// @copyright 2022 Adrian Del Grosso +//================================================================================================ + +#ifndef CAN_NAME_FILTER_HPP +#define CAN_NAME_FILTER_HPP + +#include "can_NAME.hpp" + +namespace isobus +{ + //================================================================================================ + /// @class NAMEFilter + /// + /// @brief A class that associates a NAME parameter with a value of that parameter. + /// @details This class is used to match a partner control function with specific criteria that + /// defines it. Use these to define what device you want to talk to. + //================================================================================================ + class NAMEFilter + { + public: + /// @brief Constructor for the NAMEFilter + /// @param[in] nameParameter The component of the NAME to filter on + /// @param[in] parameterMatchValue The value to match with the nameParameter + NAMEFilter(NAME::NAMEParameters nameParameter, std::uint32_t parameterMatchValue); + + /// @brief Returns the parameter data associated with this filter + /// @returns The parameter/NAME component associated with this filter + NAME::NAMEParameters get_parameter() const; + + /// @brief Returns the value associated with this filter + /// @returns The data associated with this filter component + std::uint32_t get_value() const; + + /// @brief Returns true if a NAME matches this filter class's components + /// @returns true if a NAME matches this filter class's components + bool check_name_matches_filter(const NAME &nameToCompare) const; + + private: + NAME::NAMEParameters parameter; ///< The NAME component to filter against + std::uint32_t value; ///< The value of the data associated with the filter component + }; + +} // namespace isobus + +#endif // CAN_NAME_FILTER_HPP diff --git a/src/can_address_claim_state_machine.cpp b/src/can_address_claim_state_machine.cpp new file mode 100644 index 0000000..bf1605e --- /dev/null +++ b/src/can_address_claim_state_machine.cpp @@ -0,0 +1,376 @@ +//================================================================================================ +/// @file can_address_claim_state_machine.cpp +/// +/// @brief Defines a class for managing the address claiming process +/// @author Adrian Del Grosso +/// +/// @copyright 2022 Adrian Del Grosso +//================================================================================================ +#include "can_address_claim_state_machine.hpp" +#include "can_general_parameter_group_numbers.hpp" +#include "can_network_manager.hpp" +#include "can_stack_logger.hpp" +#include "system_timing.hpp" + +#include +#include +#include + +namespace isobus +{ + AddressClaimStateMachine::AddressClaimStateMachine(std::uint8_t preferredAddressValue, NAME ControlFunctionNAME, std::uint8_t portIndex) : + m_isoname(ControlFunctionNAME), + m_portIndex(portIndex), + m_preferredAddress(preferredAddressValue) + { + assert(m_preferredAddress != BROADCAST_CAN_ADDRESS); + assert(m_preferredAddress != NULL_CAN_ADDRESS); + assert(portIndex < CAN_PORT_MAXIMUM); + std::default_random_engine generator; + std::uniform_int_distribution distribution(0, 255); + m_randomClaimDelay_ms = distribution(generator) * 0.6f; // Defined by ISO part 5 + CANNetworkManager::CANNetwork.add_global_parameter_group_number_callback(static_cast(CANLibParameterGroupNumber::ParameterGroupNumberRequest), process_rx_message, this); + CANNetworkManager::CANNetwork.add_global_parameter_group_number_callback(static_cast(CANLibParameterGroupNumber::AddressClaim), process_rx_message, this); + } + + AddressClaimStateMachine ::~AddressClaimStateMachine() + { + CANNetworkManager::CANNetwork.remove_global_parameter_group_number_callback(static_cast(CANLibParameterGroupNumber::ParameterGroupNumberRequest), process_rx_message, this); + CANNetworkManager::CANNetwork.remove_global_parameter_group_number_callback(static_cast(CANLibParameterGroupNumber::AddressClaim), process_rx_message, this); + } + + AddressClaimStateMachine::State AddressClaimStateMachine::get_current_state() const + { + return m_currentState; + } + + void AddressClaimStateMachine::process_commanded_address(std::uint8_t commandedAddress) + { + if (State::AddressClaimingComplete == get_current_state()) + { + if (!m_isoname.get_arbitrary_address_capable()) + { + CANStackLogger::error("[AC]: Our address was commanded to a new value, but our ISO NAME doesn't support changing our address."); + } + else + { + std::shared_ptr deviceAtOurPreferredAddress = CANNetworkManager::CANNetwork.get_control_function(m_portIndex, commandedAddress, {}); + m_preferredAddress = commandedAddress; + + if (nullptr == deviceAtOurPreferredAddress) + { + // Commanded address is free. We'll claim it. + set_current_state(State::SendPreferredAddressClaim); + CANStackLogger::info("[AC]: Our address was commanded to a new value of %u", commandedAddress); + } + else if (deviceAtOurPreferredAddress->get_NAME().get_full_name() < m_isoname.get_full_name()) + { + // We can steal the address of the device at our commanded address and force it to move + set_current_state(State::SendArbitraryAddressClaim); + CANStackLogger::info("[AC]: Our address was commanded to a new value of %u, and an ECU at the target address is being evicted.", commandedAddress); + } + else + { + CANStackLogger::error("[AC]: Our address was commanded to a new value of %u, but we cannot move to the target address.", commandedAddress); + } + } + } + } + + void AddressClaimStateMachine::set_is_enabled(bool value) + { + m_enabled = value; + } + + bool AddressClaimStateMachine::get_enabled() const + { + return m_enabled; + } + + std::uint8_t AddressClaimStateMachine::get_claimed_address() const + { + return m_claimedAddress; + } + + void AddressClaimStateMachine::update() + { + if (get_enabled()) + { + switch (get_current_state()) + { + case State::None: + { + set_current_state(State::WaitForClaim); + } + break; + + case State::WaitForClaim: + { + if (0 == m_timestamp_ms) + { + m_timestamp_ms = SystemTiming::get_timestamp_ms(); + } + if (SystemTiming::time_expired_ms(m_timestamp_ms, m_randomClaimDelay_ms)) + { + set_current_state(State::SendRequestForClaim); + } + } + break; + + case State::SendRequestForClaim: + { + if (send_request_to_claim()) + { + set_current_state(State::WaitForRequestContentionPeriod); + } + } + break; + + case State::WaitForRequestContentionPeriod: + { + const std::uint32_t addressContentionTime_ms = 250; + + if (SystemTiming::time_expired_ms(m_timestamp_ms, addressContentionTime_ms + m_randomClaimDelay_ms)) + { + std::shared_ptr deviceAtOurPreferredAddress = CANNetworkManager::CANNetwork.get_control_function(m_portIndex, m_preferredAddress, {}); + // Time to find a free address + if (nullptr == deviceAtOurPreferredAddress) + { + // Our address is free. This is the best outcome. Claim it. + set_current_state(State::SendPreferredAddressClaim); + } + else if ((!m_isoname.get_arbitrary_address_capable()) && + (deviceAtOurPreferredAddress->get_NAME().get_full_name() > m_isoname.get_full_name())) + { + // Our address is not free, we cannot be at an arbitrary address, and address is contendable + set_current_state(State::ContendForPreferredAddress); + } + else if (!m_isoname.get_arbitrary_address_capable()) + { + // Can't claim because we cannot tolerate an arbitrary address, and the CF at that spot wins contention + set_current_state(State::UnableToClaim); + } + else + { + // We will move to another address if whoever is in our spot has a lower NAME + if (deviceAtOurPreferredAddress->get_NAME().get_full_name() < m_isoname.get_full_name()) + { + set_current_state(State::SendArbitraryAddressClaim); + } + else + { + set_current_state(State::SendPreferredAddressClaim); + } + } + } + } + break; + + case State::SendPreferredAddressClaim: + { + if (send_address_claim(m_preferredAddress)) + { + CANStackLogger::debug("[AC]: Internal control function %016llx has claimed address %u on channel %u", + m_isoname.get_full_name(), + m_preferredAddress, + m_portIndex); + set_current_state(State::AddressClaimingComplete); + } + else + { + set_current_state(State::None); + } + } + break; + + case State::SendArbitraryAddressClaim: + { + // Search the range of generally available addresses + bool addressFound = false; + + for (std::uint8_t i = 128; i <= 247; i++) + { + if ((nullptr == CANNetworkManager::CANNetwork.get_control_function(m_portIndex, i, {})) && (send_address_claim(i))) + { + addressFound = true; + CANStackLogger::debug("[AC]: Internal control function %016llx could not use the preferred address, but has claimed address %u on channel %u", + m_isoname.get_full_name(), + i, + m_portIndex); + set_current_state(State::AddressClaimingComplete); + break; + } + } + + if (!addressFound) + { + CANStackLogger::critical("[AC]: Internal control function %016llx failed to claim an address on channel %u", + m_isoname.get_full_name(), + m_portIndex); + set_current_state(State::UnableToClaim); + } + } + break; + + case State::SendReclaimAddressOnRequest: + { + if (send_address_claim(m_claimedAddress)) + { + set_current_state(State::AddressClaimingComplete); + } + } + break; + + case State::ContendForPreferredAddress: + { + /// @todo Non-arbitratable address contention (there is not a good reason to use this, but we should add support anyways) + } + break; + + default: + { + } + break; + } + } + else + { + set_current_state(State::None); + } + } + + void AddressClaimStateMachine::process_rx_message(const CANMessage &message, void *parentPointer) + { + if (nullptr != parentPointer) + { + AddressClaimStateMachine *parent = reinterpret_cast(parentPointer); + + if ((message.get_can_port_index() == parent->m_portIndex) && + (parent->get_enabled())) + { + switch (message.get_identifier().get_parameter_group_number()) + { + case static_cast(CANLibParameterGroupNumber::ParameterGroupNumberRequest): + { + const auto &messageData = message.get_data(); + std::uint32_t requestedPGN = messageData.at(0); + requestedPGN |= (static_cast(messageData.at(1)) << 8); + requestedPGN |= (static_cast(messageData.at(2)) << 16); + + if ((static_cast(CANLibParameterGroupNumber::AddressClaim) == requestedPGN) && + (State::AddressClaimingComplete == parent->get_current_state())) + { + parent->set_current_state(State::SendReclaimAddressOnRequest); + } + } + break; + + case static_cast(CANLibParameterGroupNumber::AddressClaim): + { + if (parent->m_claimedAddress == message.get_identifier().get_source_address()) + { + const auto &messageData = message.get_data(); + std::uint64_t NAMEClaimed = messageData.at(0); + NAMEClaimed |= (static_cast(messageData.at(1)) << 8); + NAMEClaimed |= (static_cast(messageData.at(2)) << 16); + NAMEClaimed |= (static_cast(messageData.at(3)) << 24); + NAMEClaimed |= (static_cast(messageData.at(4)) << 32); + NAMEClaimed |= (static_cast(messageData.at(5)) << 40); + NAMEClaimed |= (static_cast(messageData.at(6)) << 48); + NAMEClaimed |= (static_cast(messageData.at(7)) << 56); + + // Check to see if another ECU is hijacking our address + // This is not really a needed check, as we can be pretty sure that our address + // has been stolen if we're running this logic. But, you never know, someone could be + // spoofing us I guess, or we could be getting an echo? CAN Bridge from another channel? + // Seemed safest to just confirm. + if (NAMEClaimed != parent->m_isoname.get_full_name()) + { + // Wait for things to shake out a bit, then claim a new address. + parent->set_current_state(State::WaitForRequestContentionPeriod); + parent->m_claimedAddress = NULL_CAN_ADDRESS; + CANStackLogger::warn("[AC]: Internal control function %016llx on channel %u must re-arbitrate its address because it was stolen by another ECU with NAME %016llx.", + parent->m_isoname.get_full_name(), + parent->m_portIndex, + NAMEClaimed); + } + } + } + break; + + default: + { + } + break; + } + } + } + } + + void AddressClaimStateMachine::set_current_state(State value) + { + m_currentState = value; + } + + bool AddressClaimStateMachine::send_request_to_claim() const + { + bool retVal = false; + + if (get_enabled()) + { + const std::uint8_t addressClaimRequestLength = 3; + const auto PGN = static_cast(CANLibParameterGroupNumber::AddressClaim); + std::uint8_t dataBuffer[addressClaimRequestLength]; + + dataBuffer[0] = (PGN & std::numeric_limits::max()); + dataBuffer[1] = ((PGN >> 8) & std::numeric_limits::max()); + dataBuffer[2] = ((PGN >> 16) & std::numeric_limits::max()); + + retVal = CANNetworkManager::CANNetwork.send_can_message_raw(m_portIndex, + NULL_CAN_ADDRESS, + BROADCAST_CAN_ADDRESS, + static_cast(CANLibParameterGroupNumber::ParameterGroupNumberRequest), + static_cast(CANIdentifier::CANPriority::PriorityDefault6), + dataBuffer, + 3, + {}); + } + return retVal; + } + + bool AddressClaimStateMachine::send_address_claim(std::uint8_t address) + { + bool retVal = false; + + assert(address < BROADCAST_CAN_ADDRESS); + + if (get_enabled()) + { + std::uint64_t isoNAME = m_isoname.get_full_name(); + std::uint8_t dataBuffer[CAN_DATA_LENGTH]; + + dataBuffer[0] = static_cast(isoNAME); + dataBuffer[1] = static_cast(isoNAME >> 8); + dataBuffer[2] = static_cast(isoNAME >> 16); + dataBuffer[3] = static_cast(isoNAME >> 24); + dataBuffer[4] = static_cast(isoNAME >> 32); + dataBuffer[5] = static_cast(isoNAME >> 40); + dataBuffer[6] = static_cast(isoNAME >> 48); + dataBuffer[7] = static_cast(isoNAME >> 56); + retVal = CANNetworkManager::CANNetwork.send_can_message_raw(m_portIndex, + address, + BROADCAST_CAN_ADDRESS, + static_cast(CANLibParameterGroupNumber::AddressClaim), + static_cast(CANIdentifier::CANPriority::PriorityDefault6), + dataBuffer, + CAN_DATA_LENGTH, + {}); + if (retVal) + { + m_claimedAddress = address; + } + } + return retVal; + } + +} // namespace isobus diff --git a/src/can_address_claim_state_machine.hpp b/src/can_address_claim_state_machine.hpp new file mode 100644 index 0000000..5329bde --- /dev/null +++ b/src/can_address_claim_state_machine.hpp @@ -0,0 +1,108 @@ +//================================================================================================ +/// @file can_address_claim_state_machine.hpp +/// +/// @brief Defines a class for managing the address claiming process +/// @author Adrian Del Grosso +/// +/// @copyright 2022 Adrian Del Grosso +//================================================================================================ + +#ifndef CAN_ADDRESS_CLAIM_STATE_MACHINE_HPP +#define CAN_ADDRESS_CLAIM_STATE_MACHINE_HPP + +#include "can_NAME.hpp" +#include "can_constants.hpp" + +namespace isobus +{ + class CANMessage; ///< Forward declare CANMessage + + //================================================================================================ + /// @class AddressClaimStateMachine + /// + /// @brief State machine for managing the J1939/ISO11783 address claim process + /// + /// @details This class manages address claiming for internal control functions + /// and keeps track of things like requests for address claim. + //================================================================================================ + class AddressClaimStateMachine + { + public: + /// @brief Defines the state machine states for address claiming + enum class State + { + None, ///< Address claiming is uninitialized + WaitForClaim, ///< State machine is waiting for the random delay time + SendRequestForClaim, ///< State machine is sending the request for address claim + WaitForRequestContentionPeriod, ///< State machine is waiting for the address claim contention period + SendPreferredAddressClaim, ///< State machine is claiming the preferred address + ContendForPreferredAddress, ///< State machine is contending the preferred address + SendArbitraryAddressClaim, ///< State machine is claiming an address + SendReclaimAddressOnRequest, ///< An ECU requested address claim, inform the bus of our current address + UnableToClaim, ///< State machine could not claim an address + AddressClaimingComplete ///< Address claiming is complete and we have an address + }; + + /// @brief The constructor of the state machine class + /// @param[in] preferredAddressValue The address you prefer to claim + /// @param[in] ControlFunctionNAME The NAME you want to claim + /// @param[in] portIndex The CAN channel index to claim on + AddressClaimStateMachine(std::uint8_t preferredAddressValue, NAME ControlFunctionNAME, std::uint8_t portIndex); + + /// @brief The destructor for the address claim state machine + ~AddressClaimStateMachine(); + + /// @brief Returns the current state of the state machine + /// @returns The current state of the state machine + State get_current_state() const; + + /// @brief Attempts to process a commanded address. + /// @details If the state machine has claimed successfully before, + /// this will attempt to move a NAME from the claimed address to the new, specified address. + /// @param[in] commandedAddress The address to attempt to claim + void process_commanded_address(std::uint8_t commandedAddress); + + /// @brief Enables or disables the address claimer + /// @param[in] value true if you want the class to claim, false if you want to be a sniffer only + void set_is_enabled(bool value); + + /// @brief Returns if the address claimer is enabled + /// @returns true if the class will address claim, false if in sniffing mode + bool get_enabled() const; + + /// @brief Returns the address claimed by the state machine or 0xFE if none claimed + /// @returns The address claimed by the state machine or 0xFE if no address has been claimed + std::uint8_t get_claimed_address() const; + + /// @brief Updates the state machine, should be called periodically + void update(); + + private: + /// @brief Processes a CAN message + /// @param[in] message The CAN message being received + /// @param[in] parentPointer A context variable to find the relevant address claimer + static void process_rx_message(const CANMessage &message, void *parentPointer); + + /// @brief Sets the current state machine state + void set_current_state(State value); + + /// @brief Sends the PGN request for the address claim PGN + bool send_request_to_claim() const; + + /// @brief Sends the address claim message + /// @param[in] address The address to claim + bool send_address_claim(std::uint8_t address); + + NAME m_isoname; ///< The ISO NAME to claim as + State m_currentState = State::None; ///< The address claim state machine state + std::uint32_t m_timestamp_ms = 0; ///< A generic timestamp in milliseconds used to find timeouts + std::uint8_t m_portIndex; ///< The CAN channel index to claim on + std::uint8_t m_preferredAddress; ///< The address we'd prefer to claim as (we may not get it) + std::uint8_t m_randomClaimDelay_ms; ///< The random delay as required by the ISO11783 standard + std::uint8_t m_claimedAddress = NULL_CAN_ADDRESS; ///< The actual address we ended up claiming + bool m_enabled = true; ///< Enable/disable state for this state machine + }; + +} // namespace isobus + +#endif // CAN_ADDRESS_CLAIM_STATE_MACHINE_HPP diff --git a/src/can_badge.hpp b/src/can_badge.hpp new file mode 100644 index 0000000..0c67939 --- /dev/null +++ b/src/can_badge.hpp @@ -0,0 +1,40 @@ +//================================================================================================ +/// @file can_badge.hpp +/// +/// @brief A way to only allow certain object types to access certain functions that is enforced +/// at compile time. A neat trick from Serenity OS :^) +/// @author Adrian Del Grosso +/// +/// @copyright 2022 Adrian Del Grosso +//================================================================================================ + +#ifndef CAN_BADGE_HPP +#define CAN_BADGE_HPP + +namespace isobus +{ + //================================================================================================ + /// @class CANLibBadge + /// + /// @brief This is a way to protect functions on public interfaces from being + /// accessed by objects that shouldn't access them. + /// + /// @details This class was inspired from SerenityOS. It's a way to avoid + /// friends. It protects functions on a class's public interface from being + /// called by types that were not explicitly allowed in the function signature. + //================================================================================================ + + //! @cond DoNotRaiseWarning + template + class CANLibBadge + { + private: + friend T; ///< Our best friend, T + //! @brief An empty function for our best friend + CANLibBadge() = default; + }; + //! @endcond + +} // namespace isobus + +#endif // CAN_BADGE_HPP diff --git a/src/can_callbacks.cpp b/src/can_callbacks.cpp new file mode 100644 index 0000000..5dc2dc6 --- /dev/null +++ b/src/can_callbacks.cpp @@ -0,0 +1,65 @@ +//================================================================================================ +/// @file can_callbacks.cpp +/// +/// @brief An object to represent common callbacks used within this CAN stack. +/// @author Adrian Del Grosso +/// +/// @copyright 2022 Adrian Del Grosso +//================================================================================================ +#include "can_callbacks.hpp" + +namespace isobus +{ + ParameterGroupNumberCallbackData::ParameterGroupNumberCallbackData(std::uint32_t parameterGroupNumber, CANLibCallback callback, void *parentPointer, std::shared_ptr internalControlFunction) : + mCallback(callback), + mParameterGroupNumber(parameterGroupNumber), + mParent(parentPointer), + mInternalControlFunctionFilter(internalControlFunction) + { + } + + ParameterGroupNumberCallbackData::ParameterGroupNumberCallbackData(const ParameterGroupNumberCallbackData &oldObj) : + mCallback(oldObj.mCallback), + mParameterGroupNumber(oldObj.mParameterGroupNumber), + mParent(oldObj.mParent), + mInternalControlFunctionFilter(oldObj.mInternalControlFunctionFilter) + { + } + + bool ParameterGroupNumberCallbackData::operator==(const ParameterGroupNumberCallbackData &obj) const + { + return ((obj.mCallback == this->mCallback) && + (obj.mParameterGroupNumber == this->mParameterGroupNumber) && + (obj.mParent == this->mParent) && + (obj.mInternalControlFunctionFilter == this->mInternalControlFunctionFilter)); + } + + ParameterGroupNumberCallbackData &ParameterGroupNumberCallbackData::operator=(const ParameterGroupNumberCallbackData &obj) + { + mCallback = obj.mCallback; + mParameterGroupNumber = obj.mParameterGroupNumber; + mParent = obj.mParent; + mInternalControlFunctionFilter = obj.mInternalControlFunctionFilter; + return *this; + } + + std::uint32_t ParameterGroupNumberCallbackData::get_parameter_group_number() const + { + return mParameterGroupNumber; + } + + CANLibCallback ParameterGroupNumberCallbackData::get_callback() const + { + return mCallback; + } + + void *ParameterGroupNumberCallbackData::get_parent() const + { + return mParent; + } + + std::shared_ptr ParameterGroupNumberCallbackData::get_internal_control_function() const + { + return mInternalControlFunctionFilter; + } +} // namespace isobus diff --git a/src/can_callbacks.hpp b/src/can_callbacks.hpp new file mode 100644 index 0000000..4a851e5 --- /dev/null +++ b/src/can_callbacks.hpp @@ -0,0 +1,118 @@ +//================================================================================================ +/// @file can_callbacks.hpp +/// +/// @brief An object to represent common callbacks used within this CAN stack. +/// @author Adrian Del Grosso +/// +/// @copyright 2022 Adrian Del Grosso +//================================================================================================ + +#ifndef CAN_CALLBACKS_HPP +#define CAN_CALLBACKS_HPP + +#include "can_message.hpp" + +namespace isobus +{ + // Forward declare some classes + class InternalControlFunction; + class ControlFunction; + + /// @brief The types of acknowledgement that can be sent in the Ack PGN + enum class AcknowledgementType : std::uint8_t + { + Positive = 0, ///< "ACK" Indicates that the request was completed + Negative = 1, ///< "NACK" Indicates the request was not completed or we do not support the PGN + AccessDenied = 2, ///< Signals to the requestor that their CF is not allowed to request this PGN + CannotRespond = 3 ///< Signals to the requestor that we are unable to accept the request for some reason + }; + + /// @brief Enumerates the "online" states of a control function. + enum class ControlFunctionState + { + Offline, ///< The CF's address claim state is not valid + Online ///< The CF's address claim state is valid + }; + + /// @brief A callback for control functions to get CAN messages + using CANLibCallback = void (*)(const CANMessage &message, void *parentPointer); + /// @brief A callback that can inform you when a control function changes state between online and offline + using ControlFunctionStateCallback = void (*)(std::shared_ptr controlFunction, ControlFunctionState state); + /// @brief A callback to get chunks of data for transfer by a protocol + using DataChunkCallback = bool (*)(std::uint32_t callbackIndex, + std::uint32_t bytesOffset, + std::uint32_t numberOfBytesNeeded, + std::uint8_t *chunkBuffer, + void *parentPointer); + /// @brief A callback for when a transmit is completed by the stack + using TransmitCompleteCallback = void (*)(std::uint32_t parameterGroupNumber, + std::uint32_t dataLength, + std::shared_ptr sourceControlFunction, + std::shared_ptr destinationControlFunction, + bool successful, + void *parentPointer); + /// @brief A callback for handling a PGN request + using PGNRequestCallback = bool (*)(std::uint32_t parameterGroupNumber, + std::shared_ptr requestingControlFunction, + bool &acknowledge, + AcknowledgementType &acknowledgeType, + void *parentPointer); + /// @brief A callback for handling a request for repetition rate for a specific PGN + using PGNRequestForRepetitionRateCallback = bool (*)(std::uint32_t parameterGroupNumber, + std::shared_ptr requestingControlFunction, + std::uint32_t repetitionRate, + void *parentPointer); + + //================================================================================================ + /// @class ParameterGroupNumberCallbackData + /// + /// @brief A storage class to hold data about PGN callbacks. + //================================================================================================ + class ParameterGroupNumberCallbackData + { + public: + /// @brief A constructor for holding callback data + /// @param[in] parameterGroupNumber The PGN you want to register a callback for + /// @param[in] callback The function you want the stack to call when it gets receives a message with a matching PGN + /// @param[in] parentPointer A generic variable that can provide context to which object the callback was meant for + /// @param[in] internalControlFunction An internal control function to use as an additional filter for the callback + ParameterGroupNumberCallbackData(std::uint32_t parameterGroupNumber, CANLibCallback callback, void *parentPointer, std::shared_ptr internalControlFunction); + + /// @brief A copy constructor for holding callback data + /// @param[in] oldObj The object to copy from + ParameterGroupNumberCallbackData(const ParameterGroupNumberCallbackData &oldObj); + + /// @brief Equality operator for this class + /// @param[in] obj The object to check equality against + /// @returns true if the objects have equivalent data + bool operator==(const ParameterGroupNumberCallbackData &obj) const; + + /// @brief Assignment operator for this class + /// @param[in] obj The object to assign data from + /// @returns The lhs of the operator + ParameterGroupNumberCallbackData &operator=(const ParameterGroupNumberCallbackData &obj); + + /// @brief Returns the PGN associated with this callback data + /// @returns The PGN associated with this callback data + std::uint32_t get_parameter_group_number() const; + + /// @brief Returns the callback pointer for this data object + /// @returns The callback pointer for this data object + CANLibCallback get_callback() const; + + /// @brief Returns the parent pointer for this data object + void *get_parent() const; + + /// @brief Returns the ICF being used as a filter for this callback + /// @returns A pointer to the ICF being used as a filter, or nullptr + std::shared_ptr get_internal_control_function() const; + + private: + CANLibCallback mCallback; ///< The callback that will get called when a matching PGN is received + std::uint32_t mParameterGroupNumber; ///< The PGN assocuiated with this callback + void *mParent; ///< A generic variable that can provide context to which object the callback was meant for + std::shared_ptr mInternalControlFunctionFilter; ///< An optional way to filter callbacks based on the destination of messages from the partner + }; +} // namespace isobus + +#endif // CAN_CALLBACKS_HPP diff --git a/src/can_constants.hpp b/src/can_constants.hpp new file mode 100644 index 0000000..561a54e --- /dev/null +++ b/src/can_constants.hpp @@ -0,0 +1,23 @@ +//================================================================================================ +/// @file can_constants.hpp +/// +/// @brief General constants used throughout this library +/// @author Adrian Del Grosso +/// +/// @copyright 2022 Adrian Del Grosso +//================================================================================================ +#ifndef CAN_CONSTANTS_HPP +#define CAN_CONSTANTS_HPP + +namespace isobus +{ + constexpr std::uint64_t DEFAULT_NAME = 0xFFFFFFFFFFFFFFFF; ///< An invalid NAME used as a default + constexpr std::uint32_t DEFAULT_IDENTIFIER = 0xFFFFFFFF; ///< An invalid identifier used as a default + constexpr std::uint8_t NULL_CAN_ADDRESS = 0xFE; ///< The NULL CAN address defined by J1939 and ISO11783 + constexpr std::uint8_t BROADCAST_CAN_ADDRESS = 0xFF; ///< The global/broadcast CAN address + constexpr std::uint8_t CAN_DATA_LENGTH = 8; ///< The length of a classical CAN frame + constexpr std::uint32_t CAN_PORT_MAXIMUM = 4; ///< An arbitrary limit for memory consumption + +} + +#endif // CAN_CONSTANTS_HPP diff --git a/src/can_control_function.cpp b/src/can_control_function.cpp new file mode 100644 index 0000000..8ebc076 --- /dev/null +++ b/src/can_control_function.cpp @@ -0,0 +1,91 @@ +//================================================================================================ +/// @file can_control_function.cpp +/// +/// @brief Defines a base class to represent a generic ISOBUS control function. +/// @author Adrian Del Grosso +/// @author Daan Steenbergen +/// +/// @copyright 2022 Adrian Del Grosso +//================================================================================================ + +#include "can_control_function.hpp" + +#include "can_constants.hpp" +#include "can_network_manager.hpp" + +#include + +namespace isobus +{ +#if !defined CAN_STACK_DISABLE_THREADS && !defined ARDUINO + std::mutex ControlFunction::controlFunctionProcessingMutex; +#endif + + isobus::ControlFunction::ControlFunction(NAME NAMEValue, std::uint8_t addressValue, std::uint8_t CANPort, Type type) : + controlFunctionType(type), + controlFunctionNAME(NAMEValue), + address(addressValue), + canPortIndex(CANPort) + { + } + + std::shared_ptr ControlFunction::create(NAME NAMEValue, std::uint8_t addressValue, std::uint8_t CANPort) + { + // Unfortunately, we can't use `std::make_shared` here because the constructor is private + auto controlFunction = std::shared_ptr(new ControlFunction(NAMEValue, addressValue, CANPort)); + CANNetworkManager::CANNetwork.on_control_function_created(controlFunction, CANLibBadge()); + return controlFunction; + } + + bool ControlFunction::destroy(std::uint32_t expectedRefCount) + { +#if !defined CAN_STACK_DISABLE_THREADS && !defined ARDUINO + std::lock_guard lock(controlFunctionProcessingMutex); +#endif + + CANNetworkManager::CANNetwork.on_control_function_destroyed(shared_from_this(), {}); + + return static_cast(shared_from_this().use_count()) == expectedRefCount + 1; + } + + std::uint8_t ControlFunction::get_address() const + { + return address; + } + + bool ControlFunction::get_address_valid() const + { + return ((BROADCAST_CAN_ADDRESS != address) && (NULL_CAN_ADDRESS != address)); + } + + std::uint8_t ControlFunction::get_can_port() const + { + return canPortIndex; + } + + NAME ControlFunction::get_NAME() const + { + return controlFunctionNAME; + } + + ControlFunction::Type ControlFunction::get_type() const + { + return controlFunctionType; + } + + std::string ControlFunction::get_type_string() const + { + switch (controlFunctionType) + { + case Type::Internal: + return "Internal"; + case Type::External: + return "External"; + case Type::Partnered: + return "Partnered"; + default: + return "Unknown"; + } + } + +} // namespace isobus diff --git a/src/can_control_function.hpp b/src/can_control_function.hpp new file mode 100644 index 0000000..b741043 --- /dev/null +++ b/src/can_control_function.hpp @@ -0,0 +1,98 @@ +//================================================================================================ +/// @file can_control_function.hpp +/// +/// @brief Defines a base class to represent a generic ISOBUS control function. +/// @author Adrian Del Grosso +/// @author Daan Steenbergen +/// +/// @copyright 2022 Adrian Del Grosso +//================================================================================================ + +#ifndef CAN_CONTROL_FUNCTION_HPP +#define CAN_CONTROL_FUNCTION_HPP + +#include "can_NAME.hpp" + +#include + +#if !defined CAN_STACK_DISABLE_THREADS && !defined ARDUINO +#include +#endif + +namespace isobus +{ + //================================================================================================ + /// @class ControlFunction + /// + /// @brief A class that describes an ISO11783 control function, which includes a NAME and address. + //================================================================================================ + class ControlFunction : public std::enable_shared_from_this + { + public: + /// @brief The type of the control function + enum class Type + { + Internal, ///< The control function is part of our stack and can address claim + External, ///< The control function is some other device on the bus + Partnered ///< An external control function that you explicitly want to talk to + }; + + virtual ~ControlFunction() = default; + + /// @brief The factory function to construct a control function + /// @param[in] NAMEValue The NAME of the control function + /// @param[in] addressValue The current address of the control function + /// @param[in] CANPort The CAN channel index that the control function communicates on + static std::shared_ptr create(NAME NAMEValue, std::uint8_t addressValue, std::uint8_t CANPort); + + /// @brief Destroys this control function, by removing it from the network manager + /// @param[in] expectedRefCount The expected number of shared pointers to this control function after removal + /// @returns true if the control function was successfully removed from everywhere in the stack, otherwise false + virtual bool destroy(std::uint32_t expectedRefCount = 1); + + /// @brief Returns the current address of the control function + /// @returns The current address of the control function + std::uint8_t get_address() const; + + /// @brief Describes if the control function has a valid address (not NULL or global) + /// @returns true if the address is < 0xFE + bool get_address_valid() const; + + /// @brief Returns the CAN channel index the control function communicates on + /// @returns The control function's CAN channel index + std::uint8_t get_can_port() const; + + /// @brief Returns the NAME of the control function as described by its address claim message + /// @returns The control function's NAME + NAME get_NAME() const; + + /// @brief Returns the `Type` of the control function + /// @returns The control function type + Type get_type() const; + + ///@brief Returns the 'Type' of the control function as a string + ///@returns The control function type as a string + std::string get_type_string() const; + + protected: + /// @brief The protected constructor for the control function, which is called by the (inherited) factory function + /// @param[in] NAMEValue The NAME of the control function + /// @param[in] addressValue The current address of the control function + /// @param[in] CANPort The CAN channel index that the control function communicates on + /// @param[in] type The 'Type' of control function to create + ControlFunction(NAME NAMEValue, std::uint8_t addressValue, std::uint8_t CANPort, Type type = Type::External); + + friend class CANNetworkManager; +#if !defined CAN_STACK_DISABLE_THREADS && !defined ARDUINO + static std::mutex controlFunctionProcessingMutex; ///< Protects the control function tables +#endif + const Type controlFunctionType; ///< The Type of the control function + NAME controlFunctionNAME; ///< The NAME of the control function + bool claimedAddressSinceLastAddressClaimRequest = false; ///< Used to mark CFs as stale if they don't claim within a certain time + std::uint8_t address; ///< The address of the control function + const std::uint8_t canPortIndex; ///< The CAN channel index of the control function + }; + +} // namespace isobus + +#endif // CAN_CONTROL_FUNCTION_HPP diff --git a/src/can_extended_transport_protocol.cpp b/src/can_extended_transport_protocol.cpp new file mode 100644 index 0000000..ec9d610 --- /dev/null +++ b/src/can_extended_transport_protocol.cpp @@ -0,0 +1,835 @@ +//================================================================================================ +/// @file can_extended_transport_protocol.cpp +/// +/// @brief A protocol class that handles the ISO11783 extended transport protocol. +/// @author Adrian Del Grosso +/// +/// @copyright 2022 Adrian Del Grosso +//================================================================================================ + +#include "can_extended_transport_protocol.hpp" + +#include "can_general_parameter_group_numbers.hpp" +#include "can_network_configuration.hpp" +#include "can_network_manager.hpp" +#include "can_stack_logger.hpp" +#include "system_timing.hpp" +#include "to_string.hpp" + +#include + +namespace isobus +{ + ExtendedTransportProtocolManager::ExtendedTransportProtocolSession::ExtendedTransportProtocolSession(Direction sessionDirection, std::uint8_t canPortIndex) : + sessionMessage(canPortIndex), + sessionDirection(sessionDirection) + { + } + + bool ExtendedTransportProtocolManager::ExtendedTransportProtocolSession::operator==(const ExtendedTransportProtocolSession &obj) + { + return ((sessionMessage.get_source_control_function() == obj.sessionMessage.get_source_control_function()) && + (sessionMessage.get_destination_control_function() == obj.sessionMessage.get_destination_control_function()) && + (sessionMessage.get_identifier().get_parameter_group_number() == obj.sessionMessage.get_identifier().get_parameter_group_number())); + } + + std::uint32_t ExtendedTransportProtocolManager::ExtendedTransportProtocolSession::get_message_data_length() const + { + if (nullptr != frameChunkCallback) + { + return frameChunkCallbackMessageLength; + } + return sessionMessage.get_data_length(); + } + + ExtendedTransportProtocolManager::ExtendedTransportProtocolSession::~ExtendedTransportProtocolSession() + { + } + + ExtendedTransportProtocolManager::ExtendedTransportProtocolManager() + { + } + + ExtendedTransportProtocolManager ::~ExtendedTransportProtocolManager() + { + // No need to clean up, as this object is a member of the network manager + // so its callbacks will be cleared at destruction time + } + + void ExtendedTransportProtocolManager::initialize(CANLibBadge) + { + if (!initialized) + { + initialized = true; + CANNetworkManager::CANNetwork.add_protocol_parameter_group_number_callback(static_cast(CANLibParameterGroupNumber::ExtendedTransportProtocolDataTransfer), process_message, this); + CANNetworkManager::CANNetwork.add_protocol_parameter_group_number_callback(static_cast(CANLibParameterGroupNumber::ExtendedTransportProtocolConnectionManagement), process_message, this); + } + } + + void ExtendedTransportProtocolManager::process_message(const CANMessage &message) + { + if ((nullptr != CANNetworkManager::CANNetwork.get_internal_control_function(message.get_destination_control_function()))) + { + switch (message.get_identifier().get_parameter_group_number()) + { + case static_cast(CANLibParameterGroupNumber::ExtendedTransportProtocolConnectionManagement): + { + if (CAN_DATA_LENGTH == message.get_data_length()) + { + ExtendedTransportProtocolSession *session; + const auto &data = message.get_data(); + const std::uint32_t pgn = (static_cast(data[5]) | (static_cast(data[6]) << 8) | (static_cast(data[7]) << 16)); + + switch (message.get_data()[0]) + { + case EXTENDED_REQUEST_TO_SEND_MULTIPLEXOR: + { + if ((nullptr != message.get_destination_control_function()) && + (activeSessions.size() < CANNetworkManager::CANNetwork.get_configuration().get_max_number_transport_protocol_sessions()) && + (!get_session(session, message.get_source_control_function(), message.get_destination_control_function(), pgn))) + { + ExtendedTransportProtocolSession *newSession = new ExtendedTransportProtocolSession(ExtendedTransportProtocolSession::Direction::Receive, message.get_can_port_index()); + CANIdentifier tempIdentifierData(CANIdentifier::Type::Extended, pgn, CANIdentifier::CANPriority::PriorityLowest7, message.get_destination_control_function()->get_address(), message.get_source_control_function()->get_address()); + newSession->sessionMessage.set_data_size(static_cast(data[1]) | static_cast(data[2] << 8) | static_cast(data[3] << 16) | static_cast(data[4] << 24)); + newSession->sessionMessage.set_source_control_function(message.get_source_control_function()); + newSession->sessionMessage.set_destination_control_function(message.get_destination_control_function()); + newSession->packetCount = 0xFF; + newSession->sessionMessage.set_identifier(tempIdentifierData); + newSession->state = StateMachineState::ClearToSend; + newSession->timestamp_ms = SystemTiming::get_timestamp_ms(); + activeSessions.push_back(newSession); + } + else if ((get_session(session, message.get_source_control_function(), message.get_destination_control_function(), pgn)) && + (nullptr != message.get_destination_control_function()) && + (ControlFunction::Type::Internal == message.get_destination_control_function()->get_type())) + { + abort_session(pgn, ConnectionAbortReason::AlreadyInConnectionManagedSessionAndCannotSupportAnother, std::static_pointer_cast(message.get_destination_control_function()), message.get_source_control_function()); + CANStackLogger::CAN_stack_log(CANStackLogger::LoggingLevel::Error, "[ETP]: Sent abort to address " + isobus::to_string(static_cast(message.get_source_control_function()->get_address())) + " RTS when already in session"); + close_session(session, false); + } + else if ((activeSessions.size() >= CANNetworkManager::CANNetwork.get_configuration().get_max_number_transport_protocol_sessions()) && + (nullptr != message.get_destination_control_function()) && + (ControlFunction::Type::Internal == message.get_destination_control_function()->get_type())) + { + abort_session(pgn, ConnectionAbortReason::SystemResourcesNeededForAnotherTask, std::static_pointer_cast(message.get_destination_control_function()), message.get_source_control_function()); + CANStackLogger::CAN_stack_log(CANStackLogger::LoggingLevel::Error, "[ETP]: Sent abort to address " + isobus::to_string(static_cast(message.get_source_control_function()->get_address())) + " No Sessions Available"); + close_session(session, false); + } + } + break; + + case EXTENDED_CLEAR_TO_SEND_MULTIPLEXOR: + { + const std::uint8_t packetsToBeSent = data[1]; + + if (get_session(session, message.get_destination_control_function(), message.get_source_control_function(), pgn)) + { + if (StateMachineState::WaitForClearToSend == session->state) + { + session->packetCount = packetsToBeSent; + + if (session->packetCount > CANNetworkManager::CANNetwork.get_configuration().get_max_number_of_etp_frames_per_edpo()) + { + session->packetCount = CANNetworkManager::CANNetwork.get_configuration().get_max_number_of_etp_frames_per_edpo(); + } + session->timestamp_ms = SystemTiming::get_timestamp_ms(); + // If 0 was sent as the packet number, they want us to wait. + // Just sit here in this state until we get a non-zero packet count + if (0 != packetsToBeSent) + { + session->lastPacketNumber = 0; + session->state = StateMachineState::TxDataSession; + } + } + else + { + // The session exists, but we're probably already in the TxDataSession state. Need to abort + // In the case of Rx'ing a CTS, we're the source in the session + abort_session(pgn, ConnectionAbortReason::ClearToSendReceivedWhenDataTransferInProgress, std::static_pointer_cast(message.get_destination_control_function()), message.get_source_control_function()); + CANStackLogger::CAN_stack_log(CANStackLogger::LoggingLevel::Error, "[ETP]: Sent abort to address " + isobus::to_string(static_cast(message.get_source_control_function()->get_address())) + " CTS while in data session"); + close_session(session, false); + } + } + else + { + // We got a CTS but no session exists. Aborting clears up the situation faster than waiting for them to timeout + // In the case of Rx'ing a CTS, we're the source in the session + abort_session(pgn, ConnectionAbortReason::AnyOtherReason, std::static_pointer_cast(message.get_destination_control_function()), message.get_source_control_function()); + CANStackLogger::CAN_stack_log(CANStackLogger::LoggingLevel::Error, "[ETP]: Sent abort to address " + isobus::to_string(static_cast(message.get_source_control_function()->get_address())) + " CTS With no matching session"); + } + } + break; + + case EXTENDED_DATA_PACKET_OFFSET_MULTIPLEXOR: + { + const std::uint32_t dataPacketOffset = (static_cast(data[2]) | (static_cast(data[3]) << 8) | (static_cast(data[4]) << 16)); + + if (get_session(session, message.get_source_control_function(), message.get_destination_control_function(), pgn)) + { + const std::uint8_t packetsToBeSent = data[1]; + + if (packetsToBeSent != session->packetCount) + { + if (packetsToBeSent > session->packetCount) + { + CANStackLogger::CAN_stack_log(CANStackLogger::LoggingLevel::Error, "[ETP]: Sent abort to address " + isobus::to_string(static_cast(message.get_source_control_function()->get_address())) + " DPO packet count is greater than CTS"); + abort_session(session, ConnectionAbortReason::EDPONumberOfPacketsGreaterThanClearToSend); + close_session(session, false); + } + else + { + /// @note If byte 2 is less than byte 2 of the ETP.CM_CTS message, then the receiver shall make + /// necessary adjustments to its session to accept the data block defined by the + /// ETP.CM_DPO message and the subsequent ETP.DT packets. + CANStackLogger::CAN_stack_log(CANStackLogger::LoggingLevel::Warning, "[ETP]: DPO packet count disagrees with CTS. Using DPO value."); + session->packetCount = packetsToBeSent; + } + } + + if (dataPacketOffset == session->processedPacketsThisSession) + { + // All is good. Proceed with message. + session->lastPacketNumber = 0; + set_state(session, StateMachineState::RxDataSession); + } + else + { + CANStackLogger::CAN_stack_log(CANStackLogger::LoggingLevel::Error, "[ETP]: Sent abort to address " + isobus::to_string(static_cast(message.get_source_control_function()->get_address())) + " DPO packet offset is not valid"); + abort_session(session, ConnectionAbortReason::BadEDPOOffset); + close_session(session, false); + } + } + else + { + bool anySessionMatched = false; + // Do we have any session that matches except for PGN? + for (auto currentSession : activeSessions) + { + if ((currentSession->sessionMessage.get_source_control_function() == message.get_source_control_function()) && + (currentSession->sessionMessage.get_destination_control_function() == message.get_destination_control_function())) + { + // Sending EDPO for this session with mismatched PGN is not allowed + CANStackLogger::CAN_stack_log(CANStackLogger::LoggingLevel::Error, "[ETP]: Sent abort to address " + isobus::to_string(static_cast(message.get_source_control_function()->get_address())) + " EDPO for this session with mismatched PGN is not allowed"); + abort_session(currentSession, ConnectionAbortReason::UnexpectedEDPOPgn); + close_session(session, false); + anySessionMatched = true; + break; + } + } + + if (!anySessionMatched) + { + abort_session(pgn, ConnectionAbortReason::UnexpectedEDPOPacket, std::static_pointer_cast(message.get_destination_control_function()), message.get_source_control_function()); + } + } + } + break; + + case EXTENDED_END_OF_MESSAGE_ACKNOWLEDGEMENT: + { + if ((nullptr != message.get_destination_control_function()) && + (nullptr != message.get_source_control_function())) + { + if (get_session(session, message.get_destination_control_function(), message.get_source_control_function(), pgn)) + { + if (StateMachineState::WaitForEndOfMessageAcknowledge == session->state) + { + // We completed our Tx session! + session->state = StateMachineState::None; + close_session(session, true); + } + else + { + abort_session(pgn, ConnectionAbortReason::AnyOtherReason, std::static_pointer_cast(message.get_destination_control_function()), message.get_source_control_function()); + close_session(session, false); + CANStackLogger::CAN_stack_log(CANStackLogger::LoggingLevel::Error, "[ETP]: Sent abort to address " + isobus::to_string(static_cast(message.get_source_control_function()->get_address())) + " received EOM in wrong session state"); + } + } + else + { + abort_session(pgn, ConnectionAbortReason::AnyOtherReason, std::static_pointer_cast(message.get_destination_control_function()), message.get_source_control_function()); + CANStackLogger::CAN_stack_log(CANStackLogger::LoggingLevel::Error, "[ETP]: Sent abort to address " + isobus::to_string(static_cast(message.get_source_control_function()->get_address())) + " EOM without matching session"); + } + } + else + { + CANStackLogger::CAN_stack_log(CANStackLogger::LoggingLevel::Warning, "[ETP]: Bad EOM received, sent to or from an invalid control function"); + } + } + break; + + case EXTENDED_CONNECTION_ABORT_MULTIPLEXOR: + { + if (get_session(session, message.get_destination_control_function(), message.get_source_control_function(), pgn)) + { + CANStackLogger::CAN_stack_log(CANStackLogger::LoggingLevel::Error, "[ETP]: Received an abort for an session with PGN: " + isobus::to_string(pgn)); + close_session(session, false); + } + else + { + CANStackLogger::CAN_stack_log(CANStackLogger::LoggingLevel::Error, "[ETP]: Received an abort with no matching session with PGN: " + isobus::to_string(pgn)); + } + } + break; + + default: + { + } + break; + } + } + else + { + CANStackLogger::CAN_stack_log(CANStackLogger::LoggingLevel::Warning, "[ETP]: Received an invalid ETP CM frame"); + } + } + break; + + case static_cast(CANLibParameterGroupNumber::ExtendedTransportProtocolDataTransfer): + { + ExtendedTransportProtocolSession *tempSession = nullptr; + auto &messageData = message.get_data(); + + if ((CAN_DATA_LENGTH == message.get_data_length()) && + (get_session(tempSession, message.get_source_control_function(), message.get_destination_control_function())) && + (StateMachineState::RxDataSession == tempSession->state) && + (messageData[SEQUENCE_NUMBER_DATA_INDEX] == (tempSession->lastPacketNumber + 1))) + { + for (std::uint8_t i = SEQUENCE_NUMBER_DATA_INDEX; (i < PROTOCOL_BYTES_PER_FRAME) && ((PROTOCOL_BYTES_PER_FRAME * tempSession->lastPacketNumber) + i < tempSession->get_message_data_length()); i++) + { + std::uint32_t currentDataIndex = (PROTOCOL_BYTES_PER_FRAME * tempSession->processedPacketsThisSession) + i; + tempSession->sessionMessage.set_data(messageData[1 + SEQUENCE_NUMBER_DATA_INDEX + i], currentDataIndex); + } + tempSession->lastPacketNumber++; + tempSession->processedPacketsThisSession++; + if ((tempSession->processedPacketsThisSession * PROTOCOL_BYTES_PER_FRAME) >= tempSession->get_message_data_length()) + { + if (nullptr != tempSession->sessionMessage.get_destination_control_function()) + { + send_end_of_session_acknowledgement(tempSession); + } + CANNetworkManager::CANNetwork.process_any_control_function_pgn_callbacks(tempSession->sessionMessage); + CANNetworkManager::CANNetwork.protocol_message_callback(tempSession->sessionMessage); + close_session(tempSession, true); + } + tempSession->timestamp_ms = SystemTiming::get_timestamp_ms(); + } + else + { + CANStackLogger::CAN_stack_log(CANStackLogger::LoggingLevel::Warning, "[ETP]: Received an unexpected or invalid data transfer frame"); + } + } + break; + } + } + } + + void ExtendedTransportProtocolManager::process_message(const CANMessage &message, void *parent) + { + if (nullptr != parent) + { + reinterpret_cast(parent)->process_message(message); + } + } + + bool ExtendedTransportProtocolManager::protocol_transmit_message(std::uint32_t parameterGroupNumber, + const std::uint8_t *dataBuffer, + std::uint32_t messageLength, + std::shared_ptr source, + std::shared_ptr destination, + TransmitCompleteCallback sessionCompleteCallback, + void *parentPointer, + DataChunkCallback frameChunkCallback) + { + ExtendedTransportProtocolSession *session; + bool retVal = false; + + if ((messageLength < MAX_PROTOCOL_DATA_LENGTH) && + (messageLength >= MIN_PROTOCOL_DATA_LENGTH) && + (nullptr != destination) && + ((nullptr != dataBuffer) || + (nullptr != frameChunkCallback)) && + (nullptr != source) && + (true == source->get_address_valid()) && + (destination->get_address_valid()) && + (!get_session(session, source, destination, parameterGroupNumber))) + { + ExtendedTransportProtocolSession *newSession = new ExtendedTransportProtocolSession(ExtendedTransportProtocolSession::Direction::Transmit, + source->get_can_port()); + + if (dataBuffer != nullptr) + { + newSession->sessionMessage.set_data(dataBuffer, messageLength); + } + else + { + newSession->frameChunkCallback = frameChunkCallback; + newSession->frameChunkCallbackMessageLength = messageLength; + } + newSession->sessionMessage.set_source_control_function(source); + newSession->sessionMessage.set_destination_control_function(destination); + newSession->packetCount = (messageLength / PROTOCOL_BYTES_PER_FRAME); + newSession->lastPacketNumber = 0; + newSession->processedPacketsThisSession = 0; + newSession->sessionCompleteCallback = sessionCompleteCallback; + newSession->parent = parentPointer; + if (0 != (messageLength % PROTOCOL_BYTES_PER_FRAME)) + { + newSession->packetCount++; + } + CANIdentifier messageVirtualID(CANIdentifier::Type::Extended, + parameterGroupNumber, + CANIdentifier::CANPriority::PriorityDefault6, + destination->get_address(), + source->get_address()); + + newSession->sessionMessage.set_identifier(messageVirtualID); + set_state(newSession, StateMachineState::RequestToSend); + activeSessions.push_back(newSession); + CANStackLogger::CAN_stack_log(CANStackLogger::LoggingLevel::Debug, "[ETP]: New ETP Session. Dest: " + isobus::to_string(static_cast(destination->get_address()))); + retVal = true; + } + return retVal; + } + + void ExtendedTransportProtocolManager::update(CANLibBadge) + { + for (auto i : activeSessions) + { + update_state_machine(i); + } + } + + bool ExtendedTransportProtocolManager::abort_session(ExtendedTransportProtocolSession *session, ConnectionAbortReason reason) + { + bool retVal = false; + + if (nullptr != session) + { + std::shared_ptr myControlFunction; + std::shared_ptr partnerControlFunction; + std::array data; + std::uint32_t pgn = session->sessionMessage.get_identifier().get_parameter_group_number(); + + if (ExtendedTransportProtocolSession::Direction::Transmit == session->sessionDirection) + { + myControlFunction = CANNetworkManager::CANNetwork.get_internal_control_function(session->sessionMessage.get_source_control_function()); + partnerControlFunction = session->sessionMessage.get_destination_control_function(); + } + else + { + myControlFunction = CANNetworkManager::CANNetwork.get_internal_control_function(session->sessionMessage.get_destination_control_function()); + partnerControlFunction = session->sessionMessage.get_source_control_function(); + } + + data[0] = EXTENDED_CONNECTION_ABORT_MULTIPLEXOR; + data[1] = static_cast(reason); + data[2] = 0xFF; + data[3] = 0xFF; + data[4] = 0xFF; + data[5] = static_cast(pgn & 0xFF); + data[6] = static_cast((pgn >> 8) & 0xFF); + data[7] = static_cast((pgn >> 16) & 0xFF); + retVal = CANNetworkManager::CANNetwork.send_can_message(static_cast(CANLibParameterGroupNumber::ExtendedTransportProtocolConnectionManagement), + data.data(), + CAN_DATA_LENGTH, + myControlFunction, + partnerControlFunction, + CANIdentifier::CANPriority::PriorityLowest7); + } + return retVal; + } + + bool ExtendedTransportProtocolManager::abort_session(std::uint32_t parameterGroupNumber, ConnectionAbortReason reason, std::shared_ptr source, std::shared_ptr destination) + { + std::array data; + + data[0] = EXTENDED_CONNECTION_ABORT_MULTIPLEXOR; + data[1] = static_cast(reason); + data[2] = 0xFF; + data[3] = 0xFF; + data[4] = 0xFF; + data[5] = static_cast(parameterGroupNumber & 0xFF); + data[6] = static_cast((parameterGroupNumber >> 8) & 0xFF); + data[7] = static_cast((parameterGroupNumber >> 16) & 0xFF); + return CANNetworkManager::CANNetwork.send_can_message(static_cast(CANLibParameterGroupNumber::ExtendedTransportProtocolConnectionManagement), + data.data(), + CAN_DATA_LENGTH, + source, + destination, + CANIdentifier::CANPriority::PriorityLowest7); + } + + void ExtendedTransportProtocolManager::close_session(ExtendedTransportProtocolSession *session, bool successfull) + { + if (nullptr != session) + { + process_session_complete_callback(session, successfull); + auto sessionLocation = std::find(activeSessions.begin(), activeSessions.end(), session); + if (activeSessions.end() != sessionLocation) + { + activeSessions.erase(sessionLocation); + delete session; + CANStackLogger::CAN_stack_log(CANStackLogger::LoggingLevel::Debug, "[ETP]: Session Closed"); + } + } + } + + bool ExtendedTransportProtocolManager::get_session(ExtendedTransportProtocolSession *&session, std::shared_ptr source, std::shared_ptr destination) const + { + session = nullptr; + + for (auto i : activeSessions) + { + if ((i->sessionMessage.get_source_control_function() == source) && + (i->sessionMessage.get_destination_control_function() == destination)) + { + session = i; + break; + } + } + return (nullptr != session); + } + + bool ExtendedTransportProtocolManager::get_session(ExtendedTransportProtocolSession *&session, std::shared_ptr source, std::shared_ptr destination, std::uint32_t parameterGroupNumber) const + { + bool retVal = false; + session = nullptr; + + if ((get_session(session, source, destination)) && + (session->sessionMessage.get_identifier().get_parameter_group_number() == parameterGroupNumber)) + { + retVal = true; + } + return retVal; + } + + void ExtendedTransportProtocolManager::process_session_complete_callback(ExtendedTransportProtocolSession *session, bool success) + { + if ((nullptr != session) && + (nullptr != session->sessionCompleteCallback) && + (nullptr != session->sessionMessage.get_source_control_function()) && + (ControlFunction::Type::Internal == session->sessionMessage.get_source_control_function()->get_type())) + { + session->sessionCompleteCallback(session->sessionMessage.get_identifier().get_parameter_group_number(), + session->get_message_data_length(), + std::static_pointer_cast(session->sessionMessage.get_source_control_function()), + session->sessionMessage.get_destination_control_function(), + success, + session->parent); + } + } + + bool ExtendedTransportProtocolManager::send_end_of_session_acknowledgement(ExtendedTransportProtocolSession *session) const + { + bool retVal = false; + + if (nullptr != session) + { + std::uint32_t totalBytesTransferred = (session->get_message_data_length()); + const std::uint8_t dataBuffer[CAN_DATA_LENGTH] = { EXTENDED_END_OF_MESSAGE_ACKNOWLEDGEMENT, + static_cast(totalBytesTransferred & 0xFF), + static_cast((totalBytesTransferred >> 8) & 0xFF), + static_cast((totalBytesTransferred >> 16) & 0xFF), + static_cast((totalBytesTransferred >> 24) & 0xFF), + static_cast(session->sessionMessage.get_identifier().get_parameter_group_number() & 0xFF), + static_cast((session->sessionMessage.get_identifier().get_parameter_group_number() >> 8) & 0xFF), + static_cast((session->sessionMessage.get_identifier().get_parameter_group_number() >> 16) & 0xFF) }; + retVal = CANNetworkManager::CANNetwork.send_can_message(static_cast(CANLibParameterGroupNumber::ExtendedTransportProtocolConnectionManagement), + dataBuffer, + CAN_DATA_LENGTH, + std::static_pointer_cast(session->sessionMessage.get_destination_control_function()), + session->sessionMessage.get_source_control_function(), + CANIdentifier::CANPriority::PriorityDefault6); + } + return retVal; + } + + bool ExtendedTransportProtocolManager::send_extended_connection_mode_clear_to_send(ExtendedTransportProtocolSession *session) const + { + bool retVal = false; + + if (nullptr != session) + { + std::uint32_t packetMax = ((((session->get_message_data_length() - 1) / 7) + 1) - session->processedPacketsThisSession); + + if (packetMax > 0xFF) + { + packetMax = 0xFF; + } + + if (packetMax < session->packetCount) + { + session->packetCount = packetMax; // If we're sending a CTS with less than 0xFF, set the expected packet count to the CTS packet count + } + + const std::uint8_t dataBuffer[CAN_DATA_LENGTH] = { EXTENDED_CLEAR_TO_SEND_MULTIPLEXOR, + static_cast(packetMax), /// @todo Make CTS Max user configurable + static_cast((session->processedPacketsThisSession + 1) & 0xFF), + static_cast(((session->processedPacketsThisSession + 1) >> 8) & 0xFF), + static_cast(((session->processedPacketsThisSession + 1) >> 16) & 0xFF), + static_cast(session->sessionMessage.get_identifier().get_parameter_group_number() & 0xFF), + static_cast((session->sessionMessage.get_identifier().get_parameter_group_number() >> 8) & 0xFF), + static_cast((session->sessionMessage.get_identifier().get_parameter_group_number() >> 16) & 0xFF) }; + retVal = CANNetworkManager::CANNetwork.send_can_message(static_cast(CANLibParameterGroupNumber::ExtendedTransportProtocolConnectionManagement), + dataBuffer, + CAN_DATA_LENGTH, + std::static_pointer_cast(session->sessionMessage.get_destination_control_function()), + session->sessionMessage.get_source_control_function(), + CANIdentifier::CANPriority::PriorityDefault6); + } + return retVal; + } + + bool ExtendedTransportProtocolManager::send_extended_connection_mode_request_to_send(const ExtendedTransportProtocolSession *session) const + { + bool retVal = false; + + if (nullptr != session) + { + const std::uint8_t dataBuffer[CAN_DATA_LENGTH] = { EXTENDED_REQUEST_TO_SEND_MULTIPLEXOR, + static_cast(session->get_message_data_length() & 0xFF), + static_cast((session->get_message_data_length() >> 8) & 0xFF), + static_cast((session->get_message_data_length() >> 16) & 0xFF), + static_cast((session->get_message_data_length() >> 24) & 0xFF), + static_cast(session->sessionMessage.get_identifier().get_parameter_group_number() & 0xFF), + static_cast((session->sessionMessage.get_identifier().get_parameter_group_number() >> 8) & 0xFF), + static_cast((session->sessionMessage.get_identifier().get_parameter_group_number() >> 16) & 0xFF) }; + retVal = CANNetworkManager::CANNetwork.send_can_message(static_cast(CANLibParameterGroupNumber::ExtendedTransportProtocolConnectionManagement), + dataBuffer, + CAN_DATA_LENGTH, + std::static_pointer_cast(session->sessionMessage.get_source_control_function()), + session->sessionMessage.get_destination_control_function(), + CANIdentifier::CANPriority::PriorityDefault6); + } + return retVal; + } + + bool ExtendedTransportProtocolManager::send_extended_connection_mode_data_packet_offset(const ExtendedTransportProtocolSession *session) const + { + bool retVal = false; + + if (nullptr != session) + { + const std::uint8_t dataBuffer[CAN_DATA_LENGTH] = { EXTENDED_DATA_PACKET_OFFSET_MULTIPLEXOR, + static_cast(session->packetCount & 0xFF), + static_cast((session->processedPacketsThisSession) & 0xFF), + static_cast((session->processedPacketsThisSession >> 8) & 0xFF), + static_cast((session->processedPacketsThisSession >> 16) & 0xFF), + static_cast(session->sessionMessage.get_identifier().get_parameter_group_number() & 0xFF), + static_cast((session->sessionMessage.get_identifier().get_parameter_group_number() >> 8) & 0xFF), + static_cast((session->sessionMessage.get_identifier().get_parameter_group_number() >> 16) & 0xFF) }; + retVal = CANNetworkManager::CANNetwork.send_can_message(static_cast(CANLibParameterGroupNumber::ExtendedTransportProtocolConnectionManagement), + dataBuffer, + CAN_DATA_LENGTH, + std::static_pointer_cast(session->sessionMessage.get_source_control_function()), + session->sessionMessage.get_destination_control_function(), + CANIdentifier::CANPriority::PriorityDefault6); + } + return retVal; + } + + void ExtendedTransportProtocolManager::set_state(ExtendedTransportProtocolSession *session, StateMachineState value) + { + if (nullptr != session) + { + session->timestamp_ms = SystemTiming::get_timestamp_ms(); + session->state = value; + } + } + + void ExtendedTransportProtocolManager::update_state_machine(ExtendedTransportProtocolSession *session) + { + if (nullptr != session) + { + switch (session->state) + { + case StateMachineState::RequestToSend: + { + if (send_extended_connection_mode_request_to_send(session)) + { + set_state(session, StateMachineState::WaitForClearToSend); + } + else + { + if (SystemTiming::time_expired_ms(session->timestamp_ms, T2_3_TIMEOUT_MS)) + { + CANStackLogger::CAN_stack_log(CANStackLogger::LoggingLevel::Error, "[ETP]: Aborting session, T2-3 timeout reached while in RTS state"); + abort_session(session, ConnectionAbortReason::Timeout); + close_session(session, false); + } + } + } + break; + + case StateMachineState::WaitForEndOfMessageAcknowledge: + case StateMachineState::WaitForExtendedDataPacketOffset: + case StateMachineState::WaitForClearToSend: + { + if (SystemTiming::time_expired_ms(session->timestamp_ms, T2_3_TIMEOUT_MS)) + { + CANStackLogger::CAN_stack_log(CANStackLogger::LoggingLevel::Error, "[ETP]: Aborting session, T2-3 timeout reached while waiting for CTS"); + abort_session(session, ConnectionAbortReason::Timeout); + close_session(session, false); + } + } + break; + + case StateMachineState::TxDataSession: + { + if (nullptr != session->sessionMessage.get_destination_control_function()) + { + std::uint8_t dataBuffer[CAN_DATA_LENGTH]; + bool proceedToSendDataPackets = true; + bool sessionStillValid = true; + + if (0 == session->lastPacketNumber) + { + proceedToSendDataPackets = send_extended_connection_mode_data_packet_offset(session); + } + + if (proceedToSendDataPackets) + { + std::uint32_t framesSentThisUpdate = 0; + // Try and send packets + for (std::uint32_t i = session->lastPacketNumber; i < session->packetCount; i++) + { + dataBuffer[0] = (session->lastPacketNumber + 1); + + if (nullptr != session->frameChunkCallback) + { + // Use the callback to get this frame's data + std::uint8_t callbackBuffer[7] = { + 0xFF, + 0xFF, + 0xFF, + 0xFF, + 0xFF, + 0xFF, + 0xFF + }; + std::uint32_t numberBytesLeft = (session->get_message_data_length() - (PROTOCOL_BYTES_PER_FRAME * session->processedPacketsThisSession)); + + if (numberBytesLeft > PROTOCOL_BYTES_PER_FRAME) + { + numberBytesLeft = PROTOCOL_BYTES_PER_FRAME; + } + + bool callbackSuccessful = session->frameChunkCallback(dataBuffer[0], (PROTOCOL_BYTES_PER_FRAME * session->processedPacketsThisSession), numberBytesLeft, callbackBuffer, session->parent); + + if (callbackSuccessful) + { + for (std::uint8_t j = 0; j < PROTOCOL_BYTES_PER_FRAME; j++) + { + dataBuffer[1 + j] = callbackBuffer[j]; + } + } + else + { + CANStackLogger::CAN_stack_log(CANStackLogger::LoggingLevel::Error, "[ETP]: Aborting session, unable to transfer chunk of data (numberBytesLeft=" + to_string(numberBytesLeft) + ")"); + abort_session(session, ConnectionAbortReason::AnyOtherReason); + close_session(session, false); + sessionStillValid = false; + break; + } + } + else + { + // Use the data buffer to get the data for this frame + for (std::uint8_t j = 0; j < PROTOCOL_BYTES_PER_FRAME; j++) + { + std::uint32_t index = (j + (PROTOCOL_BYTES_PER_FRAME * session->processedPacketsThisSession)); + if (index < session->get_message_data_length()) + { + dataBuffer[1 + j] = session->sessionMessage.get_data()[j + (PROTOCOL_BYTES_PER_FRAME * session->processedPacketsThisSession)]; + } + else + { + dataBuffer[1 + j] = 0xFF; + } + } + } + + if (CANNetworkManager::CANNetwork.send_can_message(static_cast(CANLibParameterGroupNumber::ExtendedTransportProtocolDataTransfer), + dataBuffer, + CAN_DATA_LENGTH, + std::static_pointer_cast(session->sessionMessage.get_source_control_function()), + session->sessionMessage.get_destination_control_function(), + CANIdentifier::CANPriority::PriorityLowest7)) + { + framesSentThisUpdate++; + session->lastPacketNumber++; + session->processedPacketsThisSession++; + session->timestamp_ms = SystemTiming::get_timestamp_ms(); + + if (framesSentThisUpdate >= CANNetworkManager::CANNetwork.get_configuration().get_max_number_of_network_manager_protocol_frames_per_update()) + { + break; // Throttle the session + } + } + else + { + // Process more next time protocol is updated + break; + } + } + } + + if (sessionStillValid) + { + if ((session->lastPacketNumber == (session->packetCount)) && + (session->get_message_data_length() <= (PROTOCOL_BYTES_PER_FRAME * session->processedPacketsThisSession))) + { + set_state(session, StateMachineState::WaitForEndOfMessageAcknowledge); + session->timestamp_ms = SystemTiming::get_timestamp_ms(); + } + else if (session->lastPacketNumber == session->packetCount) + { + set_state(session, StateMachineState::WaitForClearToSend); + session->timestamp_ms = SystemTiming::get_timestamp_ms(); + } + } + } + } + break; + + case StateMachineState::RxDataSession: + { + if (session->packetCount == session->lastPacketNumber) + { + set_state(session, StateMachineState::ClearToSend); + } + else if (SystemTiming::time_expired_ms(session->timestamp_ms, T1_TIMEOUT_MS)) + { + CANStackLogger::CAN_stack_log(CANStackLogger::LoggingLevel::Error, "[ETP]: Aborting session, RX T1 timeout reached"); + abort_session(session, ConnectionAbortReason::Timeout); + close_session(session, false); + } + } + break; + + case StateMachineState::ClearToSend: + { + if (send_extended_connection_mode_clear_to_send(session)) + { + set_state(session, StateMachineState::WaitForExtendedDataPacketOffset); + } + else if (SystemTiming::time_expired_ms(session->timestamp_ms, T2_3_TIMEOUT_MS)) + { + CANStackLogger::CAN_stack_log(CANStackLogger::LoggingLevel::Error, "[ETP]: Aborting session, T2-3 timeout reached while in CTS state"); + abort_session(session, ConnectionAbortReason::Timeout); + close_session(session, false); + } + } + break; + + default: + { + // Nothing to do. + } + break; + } + } + } + +} // namespace isobus diff --git a/src/can_extended_transport_protocol.hpp b/src/can_extended_transport_protocol.hpp new file mode 100644 index 0000000..10dc18c --- /dev/null +++ b/src/can_extended_transport_protocol.hpp @@ -0,0 +1,237 @@ +//================================================================================================ +/// @file can_extended_transport_protocol.hpp +/// +/// @brief A protocol class that handles the ISO11783 extended transport protocol. +/// Designed for packets larger than 1785 bytes. +/// @author Adrian Del Grosso +/// +/// @copyright 2022 Adrian Del Grosso +//================================================================================================ + +#ifndef CAN_EXTENDED_TRANSPORT_PROTOCOL_HPP +#define CAN_EXTENDED_TRANSPORT_PROTOCOL_HPP + +#include "can_badge.hpp" +#include "can_control_function.hpp" +#include "can_protocol.hpp" + +namespace isobus +{ + //================================================================================================ + /// @class ExtendedTransportProtocolManager + /// + /// @brief A class that handles the ISO11783 extended transport protocol. + /// @details This class handles transmission and reception of CAN messages more than 1785 bytes. + /// Simply call send_can_message on the network manager with an appropriate data length, + /// and the protocol will be automatically selected to be used. + //================================================================================================ + class ExtendedTransportProtocolManager : public CANLibProtocol + { + public: + /// @brief A list of all defined abort reasons in ISO11783 + enum class ConnectionAbortReason + { + Reserved = 0, ///< Reserved, not to be used, but should be tolerated + AlreadyInConnectionManagedSessionAndCannotSupportAnother = 1, ///< We are already in a session and can't support another + SystemResourcesNeededForAnotherTask = 2, ///< Session must be aborted because the system needs resources + Timeout = 3, ///< General timeout + ClearToSendReceivedWhenDataTransferInProgress = 4, ///< A CTS was received while already processing the last CTS + MaximumRetransmitRequestLimitReached = 5, ///< Maxmimum retries for the data has been reached + UnexpectedDataTransferPacket = 6, ///< A data packet was received outside the proper state + BadSequenceNumber = 7, ///< Incorrect sequence number was received and cannot be recovered + DuplicateSequenceNumber = 8, ///< Re-received a sequence number we've already processed + UnexpectedEDPOPacket = 9, ///< EDPO Received in an invalid state + UnexpectedEDPOPgn = 10, ///< Unexpected PGN in received EDPO + EDPONumberOfPacketsGreaterThanClearToSend = 11, ///< Number of EDPO packets is > CTS + BadEDPOOffset = 12, ///< EDPO offset was invalid + DeprecatedReason = 13, ///< Don't use this. Use AnyOtherReason instead + UnexpectedECTSPgn = 14, ///< PGN in received ECTS doesn't match session + ECTSRequestedPacketsExceedsMessageSize = 15, ///< ETP Can't support a message this large (CANMessage::ABSOLUTE_MAX_MESSAGE_LENGTH) + AnyOtherReason = 254 ///< Any other error not enumerated above, 0xFE + }; + + /// @brief The states that a ETP session could be in. Used for the internal state machine. + enum class StateMachineState + { + None, ///< Protocol session is not in progress + RequestToSend, ///< We are sending the request to send message + WaitForClearToSend, ///< We are waiting for a clear to send message + RxDataSession, ///< We are receiving data packets + ClearToSend, ///< We are sending clear to send message + WaitForExtendedDataPacketOffset, ///< We are waiting for an EDPO message + TxDataSession, ///< We are transmitting EDPOs and data packets + WaitForEndOfMessageAcknowledge ///< We are waiting for an end of message acknowledgement + }; + + //================================================================================================ + /// @class ExtendedTransportProtocolSession + /// + /// @brief A storage object to keep track of session information internally + //================================================================================================ + class ExtendedTransportProtocolSession + { + public: + /// @brief Enumerates the possible session directions, Rx or Tx + enum class Direction + { + Transmit, ///< We are transmitting a message + Receive ///< We are receving a message + }; + + /// @brief A useful way to compare sesson objects to each other for equality + bool operator==(const ExtendedTransportProtocolSession &obj); + + /// @brief Get the total number of bytes that will be sent or received in this session + /// @return The length of the message in number of bytes + std::uint32_t get_message_data_length() const; + + private: + friend class ExtendedTransportProtocolManager; ///< Allows the ETP manager full access + + /// @brief The constructor for an ETP session + /// @param[in] sessionDirection Tx or Rx + /// @param[in] canPortIndex The CAN channel index for the session + ExtendedTransportProtocolSession(Direction sessionDirection, std::uint8_t canPortIndex); + + /// @brief The destructor for a ETP session + ~ExtendedTransportProtocolSession(); + + StateMachineState state = StateMachineState::None; ///< The state machine state for this session + CANMessage sessionMessage; ///< A CAN message is used in the session to represent and store data like PGN + TransmitCompleteCallback sessionCompleteCallback = nullptr; ///< A callback that is to be called when the session is completed + DataChunkCallback frameChunkCallback = nullptr; ///< A callback that might be used to get chunks of data to send + std::uint32_t frameChunkCallbackMessageLength = 0; ///< The length of the message that is being sent in chunks + void *parent = nullptr; ///< A generic context variable that helps identify what object callbacks are destined for. Can be nullptr + std::uint32_t timestamp_ms = 0; ///< A timestamp used to track session timeouts + std::uint32_t lastPacketNumber = 0; ///< The last processed sequence number for this set of packets + std::uint32_t packetCount = 0; ///< The total number of packets to receive or send in this session + std::uint32_t processedPacketsThisSession = 0; ///< The total processed packet count for the whole session so far + const Direction sessionDirection; ///< Represents Tx or Rx session + }; + + /// @brief The constructor for the TransportProtocolManager + ExtendedTransportProtocolManager(); + + /// @brief The destructor for the TransportProtocolManager + ~ExtendedTransportProtocolManager() final; + + /// @brief The protocol's initializer function + void initialize(CANLibBadge) override; + + /// @brief A generic way for a protocol to process a received message + /// @param[in] message A received CAN message + void process_message(const CANMessage &message) override; + + /// @brief A generic way for a protocol to process a received message + /// @param[in] message A received CAN message + /// @param[in] parent Provides the context to the actual TP manager object + static void process_message(const CANMessage &message, void *parent); + + /// @brief The network manager calls this to see if the protocol can accept a long CAN message for processing + /// @param[in] parameterGroupNumber The PGN of the message + /// @param[in] data The data to be sent + /// @param[in] messageLength The length of the data to be sent + /// @param[in] source The source control function + /// @param[in] destination The destination control function + /// @param[in] transmitCompleteCallback A callback for when the protocol completes its work + /// @param[in] parentPointer A generic context object for the tx complete and chunk callbacks + /// @param[in] frameChunkCallback A callback to get some data to send + /// @returns true if the message was accepted by the protocol for processing + bool protocol_transmit_message(std::uint32_t parameterGroupNumber, + const std::uint8_t *data, + std::uint32_t messageLength, + std::shared_ptr source, + std::shared_ptr destination, + TransmitCompleteCallback transmitCompleteCallback, + void *parentPointer, + DataChunkCallback frameChunkCallback) override; + + /// @brief Updates the protocol cyclically + void update(CANLibBadge) override; + + private: + static constexpr std::uint32_t MAX_PROTOCOL_DATA_LENGTH = CANMessage::ABSOLUTE_MAX_MESSAGE_LENGTH; ///< The max payload this protocol can support + static constexpr std::uint32_t MIN_PROTOCOL_DATA_LENGTH = 1786; ///< The min payload this protocol can support + static constexpr std::uint32_t TR_TIMEOUT_MS = 200; ///< The Tr timeout as defined by the standard + static constexpr std::uint32_t T1_TIMEOUT_MS = 750; ///< The t1 timeout as defined by the standard + static constexpr std::uint32_t T2_3_TIMEOUT_MS = 1250; ///< The t2/t3 timeouts as defined by the standard + static constexpr std::uint32_t TH_TIMEOUT_MS = 500; ///< The Th timeout as defined by the standard + static constexpr std::uint8_t EXTENDED_REQUEST_TO_SEND_MULTIPLEXOR = 0x14; ///< The multiplexor for the extended request to send message + static constexpr std::uint8_t EXTENDED_CLEAR_TO_SEND_MULTIPLEXOR = 0x15; ///< The multiplexor for the extended clear to send message + static constexpr std::uint8_t EXTENDED_DATA_PACKET_OFFSET_MULTIPLEXOR = 0x16; ///< The multiplexor for the extended data packet offset message + static constexpr std::uint8_t EXTENDED_END_OF_MESSAGE_ACKNOWLEDGEMENT = 0x17; ///< Multiplexor for the extended end of message acknowledgement message + static constexpr std::uint8_t EXTENDED_CONNECTION_ABORT_MULTIPLEXOR = 0xFF; ///< Multiplexor for the extended connection abort message + static constexpr std::uint8_t PROTOCOL_BYTES_PER_FRAME = 7; ///< The number of payload bytes per frame minus overhead of sequence number + static constexpr std::uint8_t SEQUENCE_NUMBER_DATA_INDEX = 0; ///< The index of the sequence number in a frame + + /// @brief Aborts the session with the specified abort reason. Sends a CAN message. + /// @param[in] session The session to abort + /// @param[in] reason The reason we're aborting the session + /// @returns true if the abort was send OK, false if not sent + bool abort_session(ExtendedTransportProtocolSession *session, ConnectionAbortReason reason); + + /// @brief Aborts Tp with no corresponding session with the specified abort reason. Sends a CAN message. + /// @param[in] parameterGroupNumber The PGN of the ETP "session" we're aborting + /// @param[in] reason The reason we're aborting the session + /// @param[in] source The source control function from which we'll send the abort + /// @param[in] destination The destination control function to which we'll send the abort + bool abort_session(std::uint32_t parameterGroupNumber, ConnectionAbortReason reason, std::shared_ptr source, std::shared_ptr destination); + + /// @brief Gracefully closes a session to prepare for a new session + /// @param[in] session The session to close + /// @param[in] successfull True if the session was closed successfully, false if not + void close_session(ExtendedTransportProtocolSession *session, bool successfull); + + /// @brief Gets an ETP session from the passed in source and destination combination + /// @param[in] source The source control function for the session + /// @param[in] destination The destination control function for the session + /// @param[out] session The found session, or nullptr if no session matched the supplied parameters + bool get_session(ExtendedTransportProtocolSession *&session, std::shared_ptr source, std::shared_ptr destination) const; + + /// @brief Gets an ETP session from the passed in source and destination and PGN combination + /// @param[in] source The source control function for the session + /// @param[in] destination The destination control function for the session + /// @param[in] parameterGroupNumber The PGN of the session + /// @param[out] session The found session, or nullptr if no session matched the supplied parameters + bool get_session(ExtendedTransportProtocolSession *&session, std::shared_ptr source, std::shared_ptr destination, std::uint32_t parameterGroupNumber) const; + + /// @brief Processes end of session callbacks + /// @param[in] session The session we've just completed + /// @param[in] success Denotes if the session was successful + void process_session_complete_callback(ExtendedTransportProtocolSession *session, bool success); + + /// @brief Sends the "end of message acknowledgement" message for the provided session + /// @param[in] session The session for which we're sending the EOM ACK + /// @returns true if the EOM was sent, false if sending was not successful + bool send_end_of_session_acknowledgement(ExtendedTransportProtocolSession *session) const; // ETP.CM_EOMA + + /// @brief Sends the "clear to send" message + /// @param[in] session The session for which we're sending the CTS + /// @returns true if the CTS was sent, false if sending was not successful + bool send_extended_connection_mode_clear_to_send(ExtendedTransportProtocolSession *session) const; // ETP.CM_CTS + + /// @brief Sends the "request to send" message as part of initiating a transmit + /// @param[in] session The session for which we're sending the RTS + /// @returns true if the RTS was sent, false if sending was not successful + bool send_extended_connection_mode_request_to_send(const ExtendedTransportProtocolSession *session) const; // ETP.CM_RTS + + /// @brief Sends the data packet offset message for the supplied session + /// @param[in] session The session for which we're sending the EDPO + /// @returns true if the EDPO was sent, false if sending was not successful + bool send_extended_connection_mode_data_packet_offset(const ExtendedTransportProtocolSession *session) const; // ETP.CM_DPO + + /// @brief Sets the state machine state of the ETP session + /// @param[in] session The session to update + /// @param[in] value The state to update the session to + void set_state(ExtendedTransportProtocolSession *session, StateMachineState value); + + /// @brief Updates the state machine of a ETP session + /// @param[in] session The session to update + void update_state_machine(ExtendedTransportProtocolSession *session); + + std::vector activeSessions; ///< A list of all active TP sessions + }; + +} // namespace isobus + +#endif // CAN_EXTENDED_TRANSPORT_PROTOCOL_HPP diff --git a/src/can_general_parameter_group_numbers.hpp b/src/can_general_parameter_group_numbers.hpp new file mode 100644 index 0000000..5cd421a --- /dev/null +++ b/src/can_general_parameter_group_numbers.hpp @@ -0,0 +1,57 @@ +//================================================================================================ +/// @file can_general_parameter_group_numbers.hpp +/// +/// @brief Defines some PGNs that are used in the library or are very common +/// @author Adrian Del Grosso +/// +/// @copyright 2022 Adrian Del Grosso +//================================================================================================ + +#ifndef CAN_GENERAL_PARAMETER_GROUP_NUMBERS_HPP +#define CAN_GENERAL_PARAMETER_GROUP_NUMBERS_HPP + +namespace isobus +{ + /// @brief PGNs commonly used by the CAN stack + enum class CANLibParameterGroupNumber + { + Any = 0x0000, + AgriculturalGuidanceMachineInfo = 0xAC00, + AgriculturalGuidanceSystemCommand = 0xAD00, + DiagnosticMessage22 = 0xC300, + ExtendedTransportProtocolDataTransfer = 0xC700, + ExtendedTransportProtocolConnectionManagement = 0xC800, + ProcessData = 0xCB00, + RequestForRepetitionRate = 0xCC00, + DiagnosticMessage13 = 0xDF00, + VirtualTerminalToECU = 0xE600, + ECUtoVirtualTerminal = 0xE700, + Acknowledge = 0xE800, + ParameterGroupNumberRequest = 0xEA00, + TransportProtocolData = 0xEB00, + TransportProtocolCommand = 0xEC00, + AddressClaim = 0xEE00, + ProprietaryA = 0xEF00, + MachineSelectedSpeed = 0xF022, + ProductIdentification = 0xFC8D, + ControlFunctionFunctionalities = 0xFC8E, + DiagnosticProtocolIdentification = 0xFD32, + MachineSelectedSpeedCommand = 0xFD43, + WorkingSetMaster = 0xFE0D, + LanguageCommand = 0xFE0F, + MaintainPower = 0xFE47, + WheelBasedSpeedAndDistance = 0xFE48, + GroundBasedSpeedAndDistance = 0xFE49, + ECUIdentificationInformation = 0xFDC5, + DiagnosticMessage1 = 0xFECA, + DiagnosticMessage2 = 0xFECB, + DiagnosticMessage3 = 0xFECC, + DiagnosticMessage11 = 0xFED3, + CommandedAddress = 0xFED8, + SoftwareIdentification = 0xFEDA, + AllImplementsStopOperationsSwitchState = 0xFD02 + }; + +} // namespace isobus + +#endif // CAN_GENERAL_PARAMETER_GROUP_NUMBERS_HPP diff --git a/src/can_hardware_abstraction.hpp b/src/can_hardware_abstraction.hpp new file mode 100644 index 0000000..94d52d3 --- /dev/null +++ b/src/can_hardware_abstraction.hpp @@ -0,0 +1,36 @@ +//================================================================================================ +/// @file can_hardware_abstraction.hpp +/// +/// @brief An abstraction between this CAN stack and any hardware layer +/// @author Adrian Del Grosso +/// +/// @copyright 2022 Adrian Del Grosso +//================================================================================================ + +#ifndef CAN_HARDWARE_ABSTRACTION_HPP +#define CAN_HARDWARE_ABSTRACTION_HPP + +#include "can_message_frame.hpp" + +#include + +namespace isobus +{ + /// @brief The sending abstraction layer between the hardware and the stack + /// @param[in] frame The frame to transmit from the hardware + bool send_can_message_frame_to_hardware(const CANMessageFrame &frame); + + /// @brief The receiving abstraction layer between the hardware and the stack + /// @param[in] frame The frame to receive from the hardware + void receive_can_message_frame_from_hardware(const CANMessageFrame &frame); + + /// @brief Informs the network manager whenever messages are emitted on the bus + /// @param[in] txFrame The CAN frame that was just emitted + void on_transmit_can_message_frame_from_hardware(const CANMessageFrame &txFrame); + + /// @brief The periodic update abstraction layer between the hardware and the stack + void periodic_update_from_hardware(); + +} // namespace isobus + +#endif // CAN_HARDWARE_ABSTRACTION_HPP diff --git a/src/can_hardware_interface_single_thread.cpp b/src/can_hardware_interface_single_thread.cpp new file mode 100644 index 0000000..6e4cfd5 --- /dev/null +++ b/src/can_hardware_interface_single_thread.cpp @@ -0,0 +1,265 @@ +//================================================================================================ +/// @file can_hardware_interface_single_thread.cpp +/// +/// @brief The hardware abstraction layer that separates the stack from the underlying CAN driver +/// @author Adrian Del Grosso +/// +/// @copyright 2023 Adrian Del Grosso +//================================================================================================ +#include "can_hardware_interface_single_thread.hpp" +#include "can_stack_logger.hpp" +#include "system_timing.hpp" +#include "to_string.hpp" + +#include + +namespace isobus +{ + isobus::EventDispatcher CANHardwareInterface::frameReceivedEventDispatcher; + isobus::EventDispatcher CANHardwareInterface::frameTransmittedEventDispatcher; + + std::vector> CANHardwareInterface::hardwareChannels; + bool CANHardwareInterface::started = false; + + CANHardwareInterface CANHardwareInterface::SINGLETON; + + bool send_can_message_frame_to_hardware(const CANMessageFrame &frame) + { + return CANHardwareInterface::transmit_can_frame(frame); + } + + bool CANHardwareInterface::set_number_of_can_channels(uint8_t value) + { + if (started) + { + isobus::CANStackLogger::error("[HardwareInterface] Cannot set number of channels after interface is started."); + return false; + } + + while (value > hardwareChannels.size()) + { + hardwareChannels.push_back(std::make_unique()); + hardwareChannels.back()->frameHandler = nullptr; + } + while (value < hardwareChannels.size()) + { + hardwareChannels.pop_back(); + } + return true; + } + + bool CANHardwareInterface::assign_can_channel_frame_handler(std::uint8_t channelIndex, std::shared_ptr driver) + { + if (started) + { + isobus::CANStackLogger::error("[HardwareInterface] Cannot assign frame handlers after interface is started."); + return false; + } + + if (channelIndex >= hardwareChannels.size()) + { + isobus::CANStackLogger::error("[HardwareInterface] Unable to set frame handler at channel " + isobus::to_string(channelIndex) + + ", because there are only " + isobus::to_string(hardwareChannels.size()) + " channels set. " + + "Use set_number_of_can_channels() to increase the number of channels before assigning frame handlers."); + return false; + } + + if (nullptr != hardwareChannels[channelIndex]->frameHandler) + { + isobus::CANStackLogger::error("[HardwareInterface] Unable to set frame handler at channel " + isobus::to_string(channelIndex) + ", because it is already assigned."); + return false; + } + + hardwareChannels[channelIndex]->frameHandler = driver; + return true; + } + + std::uint8_t CANHardwareInterface::get_number_of_can_channels() + { + return static_cast(hardwareChannels.size() & std::numeric_limits::max()); + } + + bool CANHardwareInterface::unassign_can_channel_frame_handler(std::uint8_t channelIndex) + { + if (started) + { + isobus::CANStackLogger::error("[HardwareInterface] Cannot remove frame handlers after interface is started."); + return false; + } + + if (channelIndex >= hardwareChannels.size()) + { + isobus::CANStackLogger::error("[HardwareInterface] Unable to remove frame handler at channel " + isobus::to_string(channelIndex) + + ", because there are only " + isobus::to_string(hardwareChannels.size()) + " channels set."); + return false; + } + + if (nullptr == hardwareChannels[channelIndex]->frameHandler) + { + isobus::CANStackLogger::error("[HardwareInterface] Unable to remove frame handler at channel " + isobus::to_string(channelIndex) + ", because it is not assigned."); + return false; + } + + hardwareChannels[channelIndex]->frameHandler = nullptr; + return true; + } + + bool CANHardwareInterface::start() + { + if (started) + { + isobus::CANStackLogger::error("[HardwareInterface] Cannot start interface more than once."); + return false; + } + + started = true; + + for (std::size_t i = 0; i < hardwareChannels.size(); i++) + { + if (nullptr != hardwareChannels[i]->frameHandler) + { + hardwareChannels[i]->frameHandler->open(); + } + } + + return true; + } + + bool CANHardwareInterface::stop() + { + if (!started) + { + isobus::CANStackLogger::error("[HardwareInterface] Cannot stop interface before it is started."); + return false; + } + + std::for_each(hardwareChannels.begin(), hardwareChannels.end(), [](const std::unique_ptr &channel) { + if (nullptr != channel->frameHandler) + { + channel->frameHandler = nullptr; + } + channel->messagesToBeTransmitted.clear(); + channel->receivedMessages.clear(); + }); + return true; + } + bool CANHardwareInterface::is_running() + { + return started; + } + + bool CANHardwareInterface::transmit_can_frame(const isobus::CANMessageFrame &frame) + { + if (!started) + { + isobus::CANStackLogger::error("[HardwareInterface] Cannot transmit message before interface is started."); + return false; + } + + if (frame.channel >= hardwareChannels.size()) + { + isobus::CANStackLogger::error("[HardwareInterface] Cannot transmit message on channel %u, because there are only %u channels set.", frame.channel, hardwareChannels.size()); + return false; + } + + const std::unique_ptr &channel = hardwareChannels[frame.channel]; + if (nullptr == channel->frameHandler) + { + isobus::CANStackLogger::error("[HardwareInterface] Cannot transmit message on channel %u, because it is not assigned.", frame.channel); + return false; + } + + if (channel->frameHandler->get_is_valid()) + { + channel->messagesToBeTransmitted.push_back(frame); + return true; + } + return false; + } + + isobus::EventDispatcher &CANHardwareInterface::get_can_frame_received_event_dispatcher() + { + return frameReceivedEventDispatcher; + } + + isobus::EventDispatcher &CANHardwareInterface::get_can_frame_transmitted_event_dispatcher() + { + return frameTransmittedEventDispatcher; + } + + void CANHardwareInterface::update() + { + if (started) + { + // Stage 1 - Receiving messages from hardware + for (std::size_t i = 0; i < hardwareChannels.size(); i++) + { + receive_can_frame(i); + while (!hardwareChannels[i]->receivedMessages.empty()) + { + const auto &frame = hardwareChannels[i]->receivedMessages.front(); + + frameReceivedEventDispatcher.invoke(frame); + isobus::receive_can_message_frame_from_hardware(frame); + + hardwareChannels[i]->receivedMessages.pop_front(); + } + } + + // Stage 2 - Sending messages + isobus::periodic_update_from_hardware(); + + // Stage 3 - Transmitting messages to hardware + std::for_each(hardwareChannels.begin(), hardwareChannels.end(), [](const std::unique_ptr &channel) { + while (!channel->messagesToBeTransmitted.empty()) + { + const auto &frame = channel->messagesToBeTransmitted.front(); + + if (transmit_can_frame_from_buffer(frame)) + { + frameTransmittedEventDispatcher.invoke(frame); + isobus::on_transmit_can_message_frame_from_hardware(frame); + channel->messagesToBeTransmitted.pop_front(); + } + else + { + break; + } + } + }); + } + } + + void CANHardwareInterface::receive_can_frame(std::uint8_t channelIndex) + { + isobus::CANMessageFrame frame; + if (started && + (nullptr != hardwareChannels[channelIndex]->frameHandler)) + { + if (hardwareChannels[channelIndex]->frameHandler->get_is_valid()) + { + // Socket or other hardware still open + if (hardwareChannels[channelIndex]->frameHandler->read_frame(frame)) + { + frame.channel = channelIndex; + hardwareChannels[channelIndex]->receivedMessages.push_back(frame); + } + } + else + { + isobus::CANStackLogger::CAN_stack_log(isobus::CANStackLogger::LoggingLevel::Critical, "[CAN Rx Thread]: CAN Channel " + isobus::to_string(channelIndex) + " appears to be invalid."); + } + } + } + + bool CANHardwareInterface::transmit_can_frame_from_buffer(const isobus::CANMessageFrame &frame) + { + bool retVal = false; + if (frame.channel < hardwareChannels.size()) + { + retVal = ((nullptr != hardwareChannels[frame.channel]->frameHandler) && + (hardwareChannels[frame.channel]->frameHandler->write_frame(frame))); + } + return retVal; + } +} // namespace isobus \ No newline at end of file diff --git a/src/can_hardware_interface_single_thread.hpp b/src/can_hardware_interface_single_thread.hpp new file mode 100644 index 0000000..c73a83a --- /dev/null +++ b/src/can_hardware_interface_single_thread.hpp @@ -0,0 +1,128 @@ +//================================================================================================ +/// @file can_hardware_interface_single_thread.hpp +/// +/// @brief The hardware abstraction layer that separates the stack from the underlying CAN driver +/// @note This version of the CAN Hardware Interface is meant for systems without multi-threading. +/// @author Adrian Del Grosso +/// +/// @copyright 2023 Adrian Del Grosso +//================================================================================================ +#ifndef CAN_HARDWARE_INTERFACE_SINGLE_THREAD_HPP +#define CAN_HARDWARE_INTERFACE_SINGLE_THREAD_HPP + +#include +#include +#include +#include + +#include "can_hardware_plugin.hpp" +#include "can_hardware_abstraction.hpp" +#include "can_message_frame.hpp" +#include "event_dispatcher.hpp" + +namespace isobus +{ + //================================================================================================ + /// @class CANHardwareInterface + /// + /// @brief Provides a common queuing layer for running the CAN stack and all CAN drivers and is + /// meant for systems that are single threaded or do not support std::thread and its friends. + /// + /// @details The `CANHardwareInterface` class was created to provide a common queuing + /// layer for running the CAN stack and all CAN drivers to simplify integration and crucially to + /// provide a consistent, safe order of operations for all the function calls needed to properly + /// drive the stack. + //================================================================================================ + class CANHardwareInterface + { + public: + /// @brief Returns the number of configured CAN channels that the class is managing + /// @returns The number of configured CAN channels that the class is managing + static std::uint8_t get_number_of_can_channels(); + + /// @brief Sets the number of CAN channels to manage + /// @details Allocates the proper number of `CanHardware` objects to track + /// each CAN channel's Tx and Rx message queues. If you pass in a smaller number than what was + /// already configured, it will delete the unneeded `CanHardware` objects. + /// @note The function will fail if the channel is already assigned to a driver or the interface is already started + /// @param value The number of CAN channels to manage + /// @returns `true` if the channel count was set, otherwise `false`. + static bool set_number_of_can_channels(std::uint8_t value); + + /// @brief Assigns a CAN driver to a channel + /// @param[in] channelIndex The channel to assign to + /// @param[in] canDriver The driver to assign to the channel + /// @note The function will fail if the channel is already assigned to a driver or the interface is already started + /// @returns `true` if the driver was assigned to the channel, otherwise `false` + static bool assign_can_channel_frame_handler(std::uint8_t channelIndex, std::shared_ptr canDriver); + + /// @brief Removes a CAN driver from a channel + /// @param[in] channelIndex The channel to remove the driver from + /// @note The function will fail if the channel is already assigned to a driver or the interface is already started + /// @returns `true` if the driver was removed from the channel, otherwise `false` + static bool unassign_can_channel_frame_handler(std::uint8_t channelIndex); + + /// @brief Initializes the hardware interface + /// @returns `true` if the interface was initialized, otherwise false (perhaps you already called start) + static bool start(); + + /// @brief Cleans up and discards all remaining messages in the Tx and Rx queues. + /// @returns `true` if the interface was stopped, otherwise `false` + static bool stop(); + + /// @brief Checks if the CAN stack and CAN drivers are running + /// @returns `true` if the threads are running, otherwise `false` + static bool is_running(); + + /// @brief Called externally, adds a message to a CAN channel's Tx queue + /// @param[in] frame The frame to add to the Tx queue + /// @returns `true` if the frame was accepted, otherwise `false` (maybe wrong channel assigned) + static bool transmit_can_frame(const isobus::CANMessageFrame &frame); + + /// @brief Get the event dispatcher for when a CAN message frame is received from hardware event + /// @returns The event dispatcher which can be used to register callbacks/listeners to + static isobus::EventDispatcher &get_can_frame_received_event_dispatcher(); + + /// @brief Get the event dispatcher for when a CAN message frame will be send to hardware event + /// @returns The event dispatcher which can be used to register callbacks/listeners to + static isobus::EventDispatcher &get_can_frame_transmitted_event_dispatcher(); + + /// @brief Call this periodically. Does most of the work of this class. + /// @note You must call this very often, such as least every millisecond to ensure CAN messages get retrieved from the hardware + static void update(); + + private: + /// @brief Stores the Tx/Rx queues, mutexes, and driver needed to run a single CAN channel + struct CANHardware + { + std::deque messagesToBeTransmitted; ///< Tx message queue for a CAN channel + std::deque receivedMessages; ///< Rx message queue for a CAN channel + std::shared_ptr frameHandler; ///< The CAN driver to use for a CAN channel + }; + + /// @brief Singleton instance of the CANHardwareInterface class + /// @details This is a little hack that allows to have the destructor called + static CANHardwareInterface SINGLETON; + + /// @brief Constructor for the CANHardwareInterface class + CANHardwareInterface() = default; + + /// @brief Destructor for the CANHardwareInterface class + ~CANHardwareInterface() = default; + + /// @brief The receive thread(s) execute this function + /// @param[in] channelIndex The associated CAN channel for the thread + static void receive_can_frame(std::uint8_t channelIndex); + + /// @brief Attempts to write a frame using the driver assigned to a frame's channel + /// @param[in] frame The frame to try and write to the bus + static bool transmit_can_frame_from_buffer(const isobus::CANMessageFrame &frame); + + static isobus::EventDispatcher frameReceivedEventDispatcher; ///< The event dispatcher for when a CAN message frame is received from hardware event + static isobus::EventDispatcher frameTransmittedEventDispatcher; ///< The event dispatcher for when a CAN message has been transmitted via hardware + + static std::vector> hardwareChannels; ///< A list of all CAN channel's metadata + static bool started; ///< Stores if the threads have been started + }; +} +#endif // CAN_HARDWARE_INTERFACE_SINGLE_THREAD_HPP diff --git a/src/can_hardware_plugin.hpp b/src/can_hardware_plugin.hpp new file mode 100644 index 0000000..a4da8bb --- /dev/null +++ b/src/can_hardware_plugin.hpp @@ -0,0 +1,47 @@ +//================================================================================================ +/// @file can_hardware_plugin.hpp +/// +/// @brief A base class for a CAN driver. Can be derived into your platform's required interface. +/// @author Adrian Del Grosso +/// +/// @copyright 2022 Adrian Del Grosso +//================================================================================================ +#ifndef CAN_HARDEWARE_PLUGIN_HPP +#define CAN_HARDEWARE_PLUGIN_HPP + +#include "can_message_frame.hpp" + +namespace isobus +{ + //================================================================================================ + /// @class CANHardwarePlugin + /// + /// @brief An abstract base class for a CAN driver + //================================================================================================ + class CANHardwarePlugin + { + public: + /// @brief Returns if the driver is ready and in a good state + /// @details This should return `false` until `open` is called, and after `close` is called, or + /// if anything happens that causes the driver to be invalid, like the hardware is disconnected. + /// @returns `true` if the driver is good/connected, `false` if the driver is not usable + virtual bool get_is_valid() const = 0; + + /// @brief Disconnects the driver from the hardware + virtual void close() = 0; + + /// @brief Connects the driver to the hardware + virtual void open() = 0; + + /// @brief Reads one frame from the bus synchronously + /// @param[in, out] canFrame The CAN frame that was read + /// @returns `true` if a CAN frame was read, otherwise `false` + virtual bool read_frame(isobus::CANMessageFrame &canFrame) = 0; + + /// @brief Writes a frame to the bus (synchronous) + /// @param[in] canFrame The frame to write to the bus + /// @returns `true` if the frame was written, otherwise `false` + virtual bool write_frame(const isobus::CANMessageFrame &canFrame) = 0; + }; +} +#endif // CAN_HARDEWARE_PLUGIN_HPP diff --git a/src/can_identifier.cpp b/src/can_identifier.cpp new file mode 100644 index 0000000..c3411b4 --- /dev/null +++ b/src/can_identifier.cpp @@ -0,0 +1,160 @@ +//================================================================================================ +/// @file can_identifier.cpp +/// +/// @brief A representation of a classical CAN identifier with utility functions for ectracting +/// values that are encoded inside, along with some helpful constants. +/// @author Adrian Del Grosso +/// +/// @copyright 2022 Adrian Del Grosso +//================================================================================================ +#include "can_identifier.hpp" + +namespace isobus +{ + CANIdentifier::CANIdentifier(std::uint32_t rawIdentifierData) : + m_RawIdentifier(rawIdentifierData) + { + } + + CANIdentifier::CANIdentifier(Type identifierType, + std::uint32_t pgn, + CANPriority priority, + std::uint8_t destinationAddress, + std::uint8_t sourceAddress) : + m_RawIdentifier(0) + { + if (Type::Extended == identifierType) + { + m_RawIdentifier = (static_cast(priority) << PRIORITY_DATA_BIT_OFFSET); + + if (((pgn << PARAMTER_GROUP_NUMBER_OFFSET) & PDU2_FORMAT_MASK) < PDU2_FORMAT_MASK) + { + pgn = (pgn & DESTINATION_SPECIFIC_PGN_MASK); + pgn = (pgn << PARAMTER_GROUP_NUMBER_OFFSET); + m_RawIdentifier |= pgn; + m_RawIdentifier |= (static_cast(destinationAddress) << PARAMTER_GROUP_NUMBER_OFFSET); + } + else + { + m_RawIdentifier |= ((pgn & BROADCAST_PGN_MASK) << PARAMTER_GROUP_NUMBER_OFFSET); + } + } + m_RawIdentifier |= static_cast(sourceAddress); + } + + CANIdentifier::CANIdentifier(const CANIdentifier &copiedObject) + { + m_RawIdentifier = copiedObject.m_RawIdentifier; + } + + CANIdentifier::~CANIdentifier() + { + } + + CANIdentifier &CANIdentifier::operator=(const CANIdentifier &obj) + { + m_RawIdentifier = obj.m_RawIdentifier; + return *this; + } + + CANIdentifier::CANPriority CANIdentifier::get_priority() const + { + const std::uint8_t EXTENDED_IDENTIFIER_MASK = 0x07; + + CANPriority retVal; + + if (Type::Extended == get_identifier_type()) + { + retVal = static_cast((m_RawIdentifier >> PRIORITY_DATA_BIT_OFFSET) & EXTENDED_IDENTIFIER_MASK); + } + else + { + retVal = CANPriority::PriorityHighest0; + } + return retVal; + } + + std::uint32_t CANIdentifier::get_identifier() const + { + return (m_RawIdentifier & (~IDENTIFIER_TYPE_BIT_MASK)); + } + + CANIdentifier::Type CANIdentifier::get_identifier_type() const + { + const std::uint32_t STANDARD_ID_11_BIT_SIZE = 0x7FF; + Type retVal; + + if (m_RawIdentifier <= STANDARD_ID_11_BIT_SIZE) + { + retVal = Type::Standard; + } + else + { + retVal = Type::Extended; + } + return retVal; + } + + std::uint32_t CANIdentifier::get_parameter_group_number() const + { + std::uint32_t retVal = UNDEFINED_PARAMETER_GROUP_NUMBER; + + if (Type::Extended == get_identifier_type()) + { + if ((PDU2_FORMAT_MASK & m_RawIdentifier) < PDU2_FORMAT_MASK) + { + retVal = ((m_RawIdentifier >> PARAMTER_GROUP_NUMBER_OFFSET) & DESTINATION_SPECIFIC_PGN_MASK); + } + else + { + retVal = ((m_RawIdentifier >> PARAMTER_GROUP_NUMBER_OFFSET) & BROADCAST_PGN_MASK); + } + } + return retVal; + } + + std::uint8_t CANIdentifier::get_destination_address() const + { + const std::uint8_t ADDRESS_BITS_SIZE = 0xFF; + const std::uint8_t ADDRESS_DATA_OFFSET = 8; + + std::uint8_t retVal = GLOBAL_ADDRESS; + + if ((CANIdentifier::Type::Extended == get_identifier_type()) && + ((PDU2_FORMAT_MASK & m_RawIdentifier) < PDU2_FORMAT_MASK)) + { + retVal = ((m_RawIdentifier >> ADDRESS_DATA_OFFSET) & ADDRESS_BITS_SIZE); + } + return retVal; + } + + std::uint8_t CANIdentifier::get_source_address() const + { + const std::uint8_t ADDRESS_BITS_SIZE = 0xFF; + std::uint8_t retVal = CANIdentifier::GLOBAL_ADDRESS; + + if (CANIdentifier::Type::Extended == get_identifier_type()) + { + retVal = (m_RawIdentifier & ADDRESS_BITS_SIZE); + } + return retVal; + } + + bool CANIdentifier::get_is_valid() const + { + const std::uint32_t EXTENDED_ID_29_BIT_SIZE = 0x1FFFFFFF; + const std::uint32_t STANDARD_ID_11_BIT_SIZE = 0x7FF; + bool retVal; + + if (Type::Extended == get_identifier_type()) + { + retVal = (m_RawIdentifier <= EXTENDED_ID_29_BIT_SIZE); + } + else + { + retVal = (m_RawIdentifier <= STANDARD_ID_11_BIT_SIZE); + } + return retVal; + } + +} // namespace isobus diff --git a/src/can_identifier.hpp b/src/can_identifier.hpp new file mode 100644 index 0000000..5623c55 --- /dev/null +++ b/src/can_identifier.hpp @@ -0,0 +1,118 @@ +//================================================================================================ +/// @file can_identifier.hpp +/// +/// @brief A representation of a classical CAN identifier with utility functions for ectracting +/// values that are encoded inside, along with some helpful constants. +/// @author Adrian Del Grosso +/// +/// @copyright 2022 Adrian Del Grosso +//================================================================================================ + +#ifndef CAN_IDENTIFIER_HPP +#define CAN_IDENTIFIER_HPP + +#include + +namespace isobus +{ + //================================================================================================ + /// @class CANIdentifier + /// + /// @brief A utility class that allows easy interpretation of a 32 bit CAN identifier + //================================================================================================ + class CANIdentifier + { + public: + /// @brief Defines all the CAN frame priorities that can be encoded in a frame ID + enum CANPriority + { + PriorityHighest0 = 0, ///< Highest CAN priority + Priority1 = 1, ///< Priority highest - 1 + Priority2 = 2, ///< Priority highest - 2 + Priority3 = 3, ///< Priority highest - 3 (Control messages priority) + Priority4 = 4, ///< Priority highest - 4 + Priority5 = 5, ///< Priority highest - 5 + PriorityDefault6 = 6, ///< The default priority + PriorityLowest7 = 7 ///< The lowest priority + }; + + /// @brief Defines if a frame is a standard (11 bit) or extended (29 bit) ID frame + enum Type + { + Standard = 0, ///< Frame is an 11bit ID standard (legacy) message with no PGN and highest priority + Extended = 1 ///< Frame is a modern 29 bit ID CAN frame + }; + + /// @brief Constructor for a CAN Identifier class based on a raw 32 bit ID + /// @param[in] rawIdentifierData The raw 32 bit ID to interpret + explicit CANIdentifier(std::uint32_t rawIdentifierData); + + /// @brief Constructor for a CAN Identifier class based on all discrete components + /// @param[in] identifierType Type of frame, either standard 11 bit ID, or extended 29 bit ID + /// @param[in] pgn The parameter group number encoded in the frame (extended only) + /// @param[in] priority The priority of the frame (extended only) + /// @param[in] destinationAddress The destination address of the frame + /// @param[in] sourceAddress The source address of the frame + CANIdentifier(Type identifierType, + std::uint32_t pgn, + CANPriority priority, + std::uint8_t destinationAddress, + std::uint8_t sourceAddress); + + /// @brief Copy constructor for a CAN Identifier + /// @param[in] copiedObject The object to copy + CANIdentifier(const CANIdentifier &copiedObject); + + /// @brief Destructor for the CANIdentifier + ~CANIdentifier(); + + /// @brief Assignment operator for a CAN identifier + /// @param[in] obj rhs of the operator + CANIdentifier &operator=(const CANIdentifier &obj); + + /// @brief Returns the raw encoded ID of the CAN identifier + /// @returns The raw encoded ID of the CAN identifier + std::uint32_t get_identifier() const; + + /// @brief Returns the identifier type (standard vs extended) + /// @returns The identifier type (standard vs extended) + Type get_identifier_type() const; + + /// @brief Returns the PGN encoded in the identifier + /// @returns The PGN encoded in the identifier + std::uint32_t get_parameter_group_number() const; + + /// @brief Returns the priority of the frame encoded in the identifier + /// @returns The priority of the frame encoded in the identifier + CANPriority get_priority() const; + + /// @brief Returns the destination address of the frame encoded in the identifier + /// @returns The destination address of the frame encoded in the identifier + std::uint8_t get_destination_address() const; + + /// @brief Returns the source address of the frame encoded in the identifier + /// @returns The source address of the frame encoded in the identifier + std::uint8_t get_source_address() const; + + /// @brief Returns if the ID is valid based on some range checking + /// @returns Frame valid status + bool get_is_valid() const; + + static constexpr std::uint32_t IDENTIFIER_TYPE_BIT_MASK = 0x80000000; ///< This bit denotes if the frame is standard or extended format + static constexpr std::uint32_t UNDEFINED_PARAMETER_GROUP_NUMBER = 0xFFFFFFFF; ///< A fake PGN used internally to denote a NULL PGN + static constexpr std::uint8_t GLOBAL_ADDRESS = 0xFF; ///< The broadcast CAN address + static constexpr std::uint8_t NULL_ADDRESS = 0xFE; ///< The NULL CAN address as defined by ISO11783 + + private: + static constexpr std::uint32_t BROADCAST_PGN_MASK = 0x0003FFFF; ///< Broadcast PGNs don't mask off the bits used for destination in the PGN + static constexpr std::uint32_t DESTINATION_SPECIFIC_PGN_MASK = 0x0003FF00; ///< Destination specific PGNs mask the destination out of the PGN itself + static constexpr std::uint32_t PDU2_FORMAT_MASK = 0x00F00000; ///< Mask that denotes the ID as being PDU2 format + static constexpr std::uint8_t PARAMTER_GROUP_NUMBER_OFFSET = 8; ///< PGN is offset 8 bits into the ID + static constexpr std::uint8_t PRIORITY_DATA_BIT_OFFSET = 26; ///< Priority is offset 26 bits into the ID + + std::uint32_t m_RawIdentifier; ///< The raw encoded 29 bit ID + }; + +} // namespace isobus + +#endif // CAN_IDENTIFIER_HPP diff --git a/src/can_internal_control_function.cpp b/src/can_internal_control_function.cpp new file mode 100644 index 0000000..b2fed6f --- /dev/null +++ b/src/can_internal_control_function.cpp @@ -0,0 +1,64 @@ +//================================================================================================ +/// @file can_internal_control_function.cpp +/// +/// @brief A representation of an ISOBUS ECU that we can send from. Use this class +/// when defining your own control functions that will claim an address within your program. +/// @author Adrian Del Grosso +/// @author Daan Steenbergen +/// +/// @copyright 2022 Adrian Del Grosso +//================================================================================================ +#include "can_internal_control_function.hpp" + +#include "can_constants.hpp" +#include "can_network_manager.hpp" +#include "can_parameter_group_number_request_protocol.hpp" + +#include + +namespace isobus +{ + InternalControlFunction::InternalControlFunction(NAME desiredName, std::uint8_t preferredAddress, std::uint8_t CANPort, CANLibBadge) : + ControlFunction(desiredName, NULL_CAN_ADDRESS, CANPort, Type::Internal), + stateMachine(preferredAddress, desiredName, CANPort) + { + } + + std::shared_ptr InternalControlFunction::create(NAME desiredName, std::uint8_t preferredAddress, std::uint8_t CANPort) + { + // Unfortunately, we can't use `std::make_shared` here because the constructor is private + CANLibBadge badge; // This badge is used to allow creation of the PGN request protocol only from within this class + auto controlFunction = std::shared_ptr(new InternalControlFunction(desiredName, preferredAddress, CANPort, badge)); + controlFunction->pgnRequestProtocol = std::make_unique(controlFunction, badge); + CANNetworkManager::CANNetwork.on_control_function_created(controlFunction, badge); + return controlFunction; + } + + bool InternalControlFunction::destroy(std::uint32_t expectedRefCount) + { + // We need to destroy the PGN request protocol before we destroy the control function + pgnRequestProtocol.reset(); + + return ControlFunction::destroy(expectedRefCount); + } + + void InternalControlFunction::process_commanded_address(std::uint8_t commandedAddress, CANLibBadge) + { + stateMachine.process_commanded_address(commandedAddress); + } + + bool InternalControlFunction::update_address_claiming(CANLibBadge) + { + std::uint8_t previousAddress = address; + stateMachine.update(); + address = stateMachine.get_claimed_address(); + + return previousAddress != address; + } + + std::weak_ptr InternalControlFunction::get_pgn_request_protocol() const + { + return pgnRequestProtocol; + } + +} // namespace isobus diff --git a/src/can_internal_control_function.hpp b/src/can_internal_control_function.hpp new file mode 100644 index 0000000..64293c3 --- /dev/null +++ b/src/can_internal_control_function.hpp @@ -0,0 +1,73 @@ +//================================================================================================ +/// @file can_internal_control_function.hpp +/// +/// @brief A representation of an ISOBUS ECU that we can send from. Use this class +/// when defining your own control functions that will claim an address within your program. +/// @author Adrian Del Grosso +/// @author Daan Steenbergen +/// +/// @copyright 2022 Adrian Del Grosso +//================================================================================================ + +#ifndef CAN_INTERNAL_CONTROL_FUNCTION_HPP +#define CAN_INTERNAL_CONTROL_FUNCTION_HPP + +#include "can_address_claim_state_machine.hpp" +#include "can_badge.hpp" +#include "can_control_function.hpp" + +#include + +namespace isobus +{ + class CANNetworkManager; + class ParameterGroupNumberRequestProtocol; + + //================================================================================================ + /// @class InternalControlFunction + /// + /// @brief Describes an internal ECU's NAME and address data. Used to send CAN messages. + /// @details This class is used to define your own ECU's NAME, and is used to transmit messages. + /// Each instance of this class will claim a unique address on the bus, and can be used to + /// send messages. + //================================================================================================ + class InternalControlFunction : public ControlFunction + { + public: + /// @brief The factory function to construct an internal control function + /// @param[in] desiredName The NAME for this control function to claim as + /// @param[in] preferredAddress The preferred NAME for this control function + /// @param[in] CANPort The CAN channel index for this control function to use + static std::shared_ptr create(NAME desiredName, std::uint8_t preferredAddress, std::uint8_t CANPort); + + /// @brief Destroys this control function, by removing it from the network manager + /// @param[in] expectedRefCount The expected number of shared pointers to this control function after removal + /// @returns true if the control function was successfully removed from everywhere in the stack, otherwise false + bool destroy(std::uint32_t expectedRefCount = 1) override; + + /// @brief The protected constructor for the internal control function, which is called by the (inherited) factory function + /// @param[in] desiredName The NAME for this control function to claim as + /// @param[in] preferredAddress The preferred NAME for this control function + /// @param[in] CANPort The CAN channel index for this control function to use + InternalControlFunction(NAME desiredName, std::uint8_t preferredAddress, std::uint8_t CANPort, CANLibBadge); + + /// @brief Used by the network manager to tell the ICF that the address claim state machine needs to process + /// a J1939 command to move address. + void process_commanded_address(std::uint8_t commandedAddress, CANLibBadge); + + /// @brief Updates the internal control function together with it's associated address claim state machine + /// @returns Wether the control function has changed address by the end of the update + bool update_address_claiming(CANLibBadge); + + /// @brief Gets the PGN request protocol for this ICF + /// @returns The PGN request protocol for this ICF + std::weak_ptr get_pgn_request_protocol() const; + + private: + AddressClaimStateMachine stateMachine; ///< The address claimer for this ICF + std::shared_ptr pgnRequestProtocol; ///< The PGN request protocol for this ICF + }; + +} // namespace isobus + +#endif // CAN_INTERNAL_CONTROL_FUNCTION_HPP diff --git a/src/can_message.cpp b/src/can_message.cpp new file mode 100644 index 0000000..8ff536c --- /dev/null +++ b/src/can_message.cpp @@ -0,0 +1,185 @@ +//================================================================================================ +/// @file can_message.cpp +/// +/// @brief An abstraction of a CAN message, could be > 8 data bytes. +/// @author Adrian Del Grosso +/// @author Daan Steenbergen +/// +/// @copyright 2022 Adrian Del Grosso +//================================================================================================ +#include "can_message.hpp" +#include "can_stack_logger.hpp" + +#include + +namespace isobus +{ + CANMessage::CANMessage(std::uint8_t CANPort) : + CANPortIndex(CANPort) + { + } + + CANMessage::Type CANMessage::get_type() const + { + return messageType; + } + + const std::vector &CANMessage::get_data() const + { + return data; + } + + std::uint32_t CANMessage::get_data_length() const + { + return data.size(); + } + + std::shared_ptr CANMessage::get_source_control_function() const + { + return source; + } + + std::shared_ptr CANMessage::get_destination_control_function() const + { + return destination; + } + + CANIdentifier CANMessage::get_identifier() const + { + return identifier; + } + + std::uint8_t CANMessage::get_can_port_index() const + { + return CANPortIndex; + } + + void CANMessage::set_data(const std::uint8_t *dataBuffer, std::uint32_t length) + { + assert(length <= ABSOLUTE_MAX_MESSAGE_LENGTH && "CANMessage::set_data() called with length greater than maximum supported"); + assert(nullptr != dataBuffer && "CANMessage::set_data() called with nullptr dataBuffer"); + + data.insert(data.end(), dataBuffer, dataBuffer + length); + } + + void CANMessage::set_data(std::uint8_t dataByte, const std::uint32_t insertPosition) + { + assert(insertPosition <= ABSOLUTE_MAX_MESSAGE_LENGTH && "CANMessage::set_data() called with insertPosition greater than maximum supported"); + + data[insertPosition] = dataByte; + } + + void CANMessage::set_data_size(std::uint32_t length) + { + data.resize(length); + } + + void CANMessage::set_source_control_function(std::shared_ptr value) + { + source = value; + } + + void CANMessage::set_destination_control_function(std::shared_ptr value) + { + destination = value; + } + + void CANMessage::set_identifier(CANIdentifier value) + { + identifier = value; + } + + std::uint8_t CANMessage::get_uint8_at(const std::uint32_t index) const + { + return data.at(index); + } + + std::uint16_t CANMessage::get_uint16_at(const std::uint32_t index, const ByteFormat format) const + { + std::uint16_t retVal; + if (ByteFormat::LittleEndian == format) + { + retVal = data.at(index); + retVal |= static_cast(data.at(index + 1)) << 8; + } + else + { + retVal = static_cast(data.at(index)) << 8; + retVal |= data.at(index + 1); + } + return retVal; + } + + std::uint32_t CANMessage::get_uint24_at(const std::uint32_t index, const ByteFormat format) const + { + std::uint32_t retVal; + if (ByteFormat::LittleEndian == format) + { + retVal = data.at(index); + retVal |= static_cast(data.at(index + 1)) << 8; + retVal |= static_cast(data.at(index + 2)) << 16; + } + else + { + retVal = static_cast(data.at(index + 2)) << 16; + retVal |= static_cast(data.at(index + 1)) << 8; + retVal |= data.at(index + 2); + } + return retVal; + } + + std::uint32_t CANMessage::get_uint32_at(const std::uint32_t index, const ByteFormat format) const + { + std::uint32_t retVal; + if (ByteFormat::LittleEndian == format) + { + retVal = data.at(index); + retVal |= static_cast(data.at(index + 1)) << 8; + retVal |= static_cast(data.at(index + 2)) << 16; + retVal |= static_cast(data.at(index + 3)) << 24; + } + else + { + retVal = static_cast(data.at(index)) << 24; + retVal |= static_cast(data.at(index + 1)) << 16; + retVal |= static_cast(data.at(index + 2)) << 8; + retVal |= data.at(index + 3); + } + return retVal; + } + + std::uint64_t CANMessage::get_uint64_at(const std::uint32_t index, const ByteFormat format) const + { + std::uint64_t retVal; + if (ByteFormat::LittleEndian == format) + { + retVal = data.at(index); + retVal |= static_cast(data.at(index + 1)) << 8; + retVal |= static_cast(data.at(index + 2)) << 16; + retVal |= static_cast(data.at(index + 3)) << 24; + retVal |= static_cast(data.at(index + 4)) << 32; + retVal |= static_cast(data.at(index + 5)) << 40; + retVal |= static_cast(data.at(index + 6)) << 48; + retVal |= static_cast(data.at(index + 7)) << 56; + } + else + { + retVal = static_cast(data.at(index)) << 56; + retVal |= static_cast(data.at(index + 1)) << 48; + retVal |= static_cast(data.at(index + 2)) << 40; + retVal |= static_cast(data.at(index + 3)) << 32; + retVal |= static_cast(data.at(index + 4)) << 24; + retVal |= static_cast(data.at(index + 5)) << 16; + retVal |= static_cast(data.at(index + 6)) << 8; + retVal |= data.at(index + 7); + } + return retVal; + } + bool isobus::CANMessage::get_bool_at(const std::uint32_t byteIndex, const std::uint8_t bitIndex, const std::uint8_t length) const + { + assert(length <= 8 - bitIndex && "length must be less than or equal to 8 - bitIndex"); + std::uint8_t mask = ((1 << length) - 1) << bitIndex; + return (get_uint8_at(byteIndex) & mask) == mask; + } + +} // namespace isobus diff --git a/src/can_message.hpp b/src/can_message.hpp new file mode 100644 index 0000000..e8b217b --- /dev/null +++ b/src/can_message.hpp @@ -0,0 +1,169 @@ +//================================================================================================ +/// @file can_message.hpp +/// +/// @brief An abstraction of a CAN message, could be > 8 data bytes. +/// @author Adrian Del Grosso +/// @author Daan Steenbergen +/// +/// @copyright 2022 Adrian Del Grosso +//================================================================================================ + +#ifndef CAN_MESSAGE_HPP +#define CAN_MESSAGE_HPP + +#include "can_control_function.hpp" +#include "can_identifier.hpp" + +#include + +namespace isobus +{ + //================================================================================================ + /// @class CANMessage + /// + /// @brief A class that represents a generic CAN message of arbitrary length. + //================================================================================================ + class CANMessage + { + public: + /// @brief The internal message type + enum class Type + { + Transmit, ///< Message is to be transmitted from the stack + Receive, ///< Message is being received + Internal ///< Message is being used internally as data storage for a protocol + }; + + /// @brief The different byte formats that can be used when reading bytes from the buffer. + enum class ByteFormat + { + LittleEndian, + BigEndian + }; + + /// @brief ISO11783-3 defines this: The maximum number of packets that can be sent in a single connection + /// with extended transport protocol is restricted by the extended data packet offset (3 bytes). + /// This yields a maximum message size of (2^24-1 packets) x (7 bytes/packet) = 117440505 bytes + /// @returns The maximum length of any CAN message as defined by ETP in ISO11783 + static const std::uint32_t ABSOLUTE_MAX_MESSAGE_LENGTH = 117440505; + + /// @brief Constructor for a CAN message + /// @param[in] CANPort The can channel index the message uses + explicit CANMessage(std::uint8_t CANPort); + + /// @brief Destructor for a CAN message + virtual ~CANMessage() = default; + + /// @brief Returns the CAN message type + /// @returns The type of the CAN message + Type get_type() const; + + /// @brief Gets a reference to the data in the CAN message + /// @returns A reference to the data in the CAN message + const std::vector &get_data() const; + + /// @brief Returns the length of the data in the CAN message + /// @returns The message data payload length + virtual std::uint32_t get_data_length() const; + + /// @brief Gets the source control function that the message is from + /// @returns The source control function that the message is from + std::shared_ptr get_source_control_function() const; + + /// @brief Gets the destination control function that the message is to + /// @returns The destination control function that the message is to + std::shared_ptr get_destination_control_function() const; + + /// @brief Returns the identifier of the message + /// @returns The identifier of the message + CANIdentifier get_identifier() const; + + /// @brief Returns the CAN channel index associated with the message + /// @returns The CAN channel index associated with the message + std::uint8_t get_can_port_index() const; + + /// @brief Sets the message data to the value supplied. Creates a copy. + /// @param[in] dataBuffer The data payload + /// @param[in] length the length of the data payload in bytes + void set_data(const std::uint8_t *dataBuffer, std::uint32_t length); + + /// @brief Sets one byte of data in the message data payload + /// @param[in] dataByte One byte of data + /// @param[in] insertPosition The position in the message at which to insert the data byte + void set_data(std::uint8_t dataByte, const std::uint32_t insertPosition); + + /// @brief Sets the size of the data payload + /// @param[in] length The desired length of the data payload + void set_data_size(std::uint32_t length); + + /// @brief Sets the source control function for the message + /// @param[in] value The source control function + void set_source_control_function(std::shared_ptr value); + + /// @brief Sets the destination control function for the message + /// @param[in] value The destination control function + void set_destination_control_function(std::shared_ptr value); + + /// @brief Sets the CAN ID of the message + /// @param[in] value The CAN ID for the message + void set_identifier(CANIdentifier value); + + /// @brief Get a 8-bit unsigned byte from the buffer at a specific index. + /// A 8-bit unsigned byte can hold a value between 0 and 255. + /// @details This function will return the byte at the specified index in the buffer. + /// @param[in] index The index to get the byte from + /// @return The 8-bit unsigned byte + std::uint8_t get_uint8_at(const std::uint32_t index) const; + + /// @brief Get a 16-bit unsigned integer from the buffer at a specific index. + /// A 16-bit unsigned integer can hold a value between 0 and 65535. + /// @details This function will return the 2 bytes at the specified index in the buffer. + /// @param[in] index The index to get the 16-bit unsigned integer from + /// @param[in] format The byte format to use when reading the integer + /// @return The 16-bit unsigned integer + std::uint16_t get_uint16_at(const std::uint32_t index, const ByteFormat format = ByteFormat::LittleEndian) const; + + /// @brief Get a right-aligned 24-bit integer from the buffer (returned as a uint32_t) at a specific index. + /// A 24-bit number can hold a value between 0 and 16,777,215. + /// @details This function will return the 3 bytes at the specified index in the buffer. + /// @param[in] index The index to get the 24-bit unsigned integer from + /// @param[in] format The byte format to use when reading the integer + /// @return The 24-bit unsigned integer, right aligned into a uint32_t + std::uint32_t get_uint24_at(const std::uint32_t index, const ByteFormat format = ByteFormat::LittleEndian) const; + + /// @brief Get a 32-bit unsigned integer from the buffer at a specific index. + /// A 32-bit unsigned integer can hold a value between 0 and 4294967295. + /// @details This function will return the 4 bytes at the specified index in the buffer. + /// @param[in] index The index to get the 32-bit unsigned integer from + /// @param[in] format The byte format to use when reading the integer + /// @return The 32-bit unsigned integer + std::uint32_t get_uint32_at(const std::uint32_t index, const ByteFormat format = ByteFormat::LittleEndian) const; + + /// @brief Get a 64-bit unsigned integer from the buffer at a specific index. + /// A 64-bit unsigned integer can hold a value between 0 and 18446744073709551615. + /// @details This function will return the 8 bytes at the specified index in the buffer. + /// @param[in] index The index to get the 64-bit unsigned integer from + /// @param[in] format The byte format to use when reading the integer + /// @return The 64-bit unsigned integer + std::uint64_t get_uint64_at(const std::uint32_t index, const ByteFormat format = ByteFormat::LittleEndian) const; + + /// @brief Get a bit-boolean from the buffer at a specific index. + /// @details This function will return whether the bit(s) at the specified index in the buffer is/are (all) equal to 1. + /// @param[in] byteIndex The byte index to start reading the boolean from + /// @param[in] bitIndex The bit index to start reading the boolean from, ranging from 0 to 7 + /// @param[in] length The number of bits to read, maximum of (8 - bitIndex) + /// @return True if (all) the bit(s) are set, false otherwise + bool get_bool_at(const std::uint32_t byteIndex, const std::uint8_t bitIndex, const std::uint8_t length = 1) const; + + private: + Type messageType = Type::Receive; ///< The internal message type associated with the message + CANIdentifier identifier = CANIdentifier(0); ///< The CAN ID of the message + std::vector data; ///< A data buffer for the message, used when not using data chunk callbacks + std::shared_ptr source = nullptr; ///< The source control function of the message + std::shared_ptr destination = nullptr; ///< The destination control function of the message + const std::uint8_t CANPortIndex; ///< The CAN channel index associated with the message + }; + +} // namespace isobus + +#endif // CAN_MESSAGE_HPP diff --git a/src/can_message_frame.cpp b/src/can_message_frame.cpp new file mode 100644 index 0000000..b5516e3 --- /dev/null +++ b/src/can_message_frame.cpp @@ -0,0 +1,35 @@ +//================================================================================================ +/// @file can_message_frame.cpp +/// +/// @brief Implements helper functions for CANMessageFrame +/// @author Adrian Del Grosso +/// +/// @copyright 2023 Adrian Del Grosso +//================================================================================================ +#include "can_message_frame.hpp" +#include "can_constants.hpp" +#include "can_identifier.hpp" + +namespace isobus +{ + std::uint32_t CANMessageFrame::get_number_bits_in_message() const + { + constexpr std::uint32_t MAX_CONSECUTIVE_SAME_BITS = 5; // After 5 consecutive bits, 6th will be opposite + const std::uint32_t dataLengthBits = CAN_DATA_LENGTH * dataLength; + std::uint32_t retVal = 0; + + if (isExtendedFrame) + { + constexpr std::uint32_t EXTENDED_ID_BEST_NON_DATA_LENGTH = 67; // SOF, ID, Control, CRC, ACK, EOF, and interframe space + constexpr std::uint32_t EXTENDED_ID_WORST_NON_DATA_LENGTH = 78; + retVal = ((dataLengthBits + EXTENDED_ID_BEST_NON_DATA_LENGTH) + (dataLengthBits + (dataLengthBits / MAX_CONSECUTIVE_SAME_BITS) + EXTENDED_ID_WORST_NON_DATA_LENGTH)); + } + else + { + constexpr std::uint32_t STANDARD_ID_BEST_NON_DATA_LENGTH = 47; // SOF, ID, Control, CRC, ACK, EOF, and interframe space + constexpr std::uint32_t STANDARD_ID_WORST_NON_DATA_LENGTH = 54; + retVal = ((dataLengthBits + STANDARD_ID_BEST_NON_DATA_LENGTH) + (dataLengthBits + (dataLengthBits / MAX_CONSECUTIVE_SAME_BITS) + STANDARD_ID_WORST_NON_DATA_LENGTH)); + } + return retVal / 2; + } +} // namespace isobus diff --git a/src/can_message_frame.hpp b/src/can_message_frame.hpp new file mode 100644 index 0000000..f03710b --- /dev/null +++ b/src/can_message_frame.hpp @@ -0,0 +1,40 @@ +//================================================================================================ +/// @file can_message_frame.hpp +/// +/// @brief A classical CAN frame, with 8 data bytes +/// @author Adrian Del Grosso +/// @author Daan Steenbergen +/// +/// @copyright 2022 Adrian Del Grosso +//================================================================================================ + +#ifndef CAN_MESSAGE_FRAME_HPP +#define CAN_MESSAGE_FRAME_HPP + +#include + +namespace isobus +{ + //================================================================================================ + /// @class CANMessageFrame + /// + /// @brief A CAN frame for interfacing with a hardware layer, like socket CAN or other interface + //================================================================================================ + class CANMessageFrame + { + public: + /// Returns the number of bits in a CAN message with averaged bit stuffing + /// @returns The number of bits in the message (with average bit stuffing) + std::uint32_t get_number_bits_in_message() const; + + std::uint64_t timestamp_us; ///< A microsecond timestamp + std::uint32_t identifier; ///< The 32 bit identifier of the frame + std::uint8_t channel; ///< The CAN channel index associated with the frame + std::uint8_t data[8]; ///< The data payload of the frame + std::uint8_t dataLength; ///< The length of the data used in the frame + bool isExtendedFrame; ///< Denotes if the frame is extended format + }; + +} // namespace isobus + +#endif // CAN_MESSAGE_FRAME_HPP diff --git a/src/can_network_configuration.cpp b/src/can_network_configuration.cpp new file mode 100644 index 0000000..84f9a42 --- /dev/null +++ b/src/can_network_configuration.cpp @@ -0,0 +1,60 @@ +//================================================================================================ +/// @file can_network_configuration.cpp +/// +/// @brief This is a class for changing stack settings. +/// @author Adrian Del Grosso +/// +/// @copyright 2022 Adrian Del Grosso +//================================================================================================ + +#include "can_network_configuration.hpp" + +namespace isobus +{ + void CANNetworkConfiguration::set_max_number_transport_protocol_sessions(std::uint32_t value) + { + maxNumberTransportProtocolSessions = value; + } + + std::uint32_t CANNetworkConfiguration::get_max_number_transport_protocol_sessions() const + { + return maxNumberTransportProtocolSessions; + } + + void CANNetworkConfiguration::set_minimum_time_between_transport_protocol_bam_frames(std::uint32_t value) + { + constexpr std::uint32_t MAX_BAM_FRAME_DELAY_MS = 200; + constexpr std::uint32_t MIN_BAM_FRAME_DELAY_MS = 10; + + if ((value <= MAX_BAM_FRAME_DELAY_MS) && + (value >= MIN_BAM_FRAME_DELAY_MS)) + { + minimumTimeBetweenTransportProtocolBAMFrames = value; + } + } + + std::uint32_t CANNetworkConfiguration::get_minimum_time_between_transport_protocol_bam_frames() const + { + return minimumTimeBetweenTransportProtocolBAMFrames; + } + + void CANNetworkConfiguration::set_max_number_of_etp_frames_per_edpo(std::uint8_t numberFrames) + { + extendedTransportProtocolMaxNumberOfFramesPerEDPO = numberFrames; + } + + std::uint8_t CANNetworkConfiguration::get_max_number_of_etp_frames_per_edpo() const + { + return extendedTransportProtocolMaxNumberOfFramesPerEDPO; + } + + void CANNetworkConfiguration::set_max_number_of_network_manager_protocol_frames_per_update(std::uint8_t numberFrames) + { + networkManagerMaxFramesToSendPerUpdate = numberFrames; + } + + std::uint8_t CANNetworkConfiguration::get_max_number_of_network_manager_protocol_frames_per_update() const + { + return networkManagerMaxFramesToSendPerUpdate; + } +} diff --git a/src/can_network_configuration.hpp b/src/can_network_configuration.hpp new file mode 100644 index 0000000..8b6cd12 --- /dev/null +++ b/src/can_network_configuration.hpp @@ -0,0 +1,86 @@ +//================================================================================================ +/// @file can_network_configuration.hpp +/// +/// @brief This is a class for changing stack settings. +/// @author Adrian Del Grosso +/// +/// @copyright 2022 Adrian Del Grosso +//================================================================================================ + +#ifndef CAN_NETWORK_CONFIGURATION_HPP +#define CAN_NETWORK_CONFIGURATION_HPP + +#include + +namespace isobus +{ + //================================================================================================ + /// @class CANNetworkConfiguration + /// + /// @brief A class that defines stack-wide configuration data. You can set the values in there + /// to suit your specific memory constraints. + //================================================================================================ + class CANNetworkConfiguration + { + public: + /// @brief The constructor for the configuration object + CANNetworkConfiguration() = default; + + /// @brief The destructor for the configuration object + ~CANNetworkConfiguration() = default; + + /// @brief Configures the max number of concurrent TP sessions to provide a RAM limit for TP sessions + /// @param[in] value The max allowable number of TP sessions + void set_max_number_transport_protocol_sessions(std::uint32_t value); + + /// @brief Returns the max number of concurrent TP sessions + /// @returns The max number of concurrent TP sessions + std::uint32_t get_max_number_transport_protocol_sessions() const; + + /// @brief Sets the minimum time to wait between sending BAM frames + /// @details The acceptable range as defined by ISO-11783 is 10 to 200 ms. + /// This is a minumum time, so if you set it to some value, like 10 ms, the + /// stack will attempt to transmit it as close to that time as it can, but it is + /// not possible to 100% ensure it. + /// @param[in] value The minimum time to wait between sending BAM frames + void set_minimum_time_between_transport_protocol_bam_frames(std::uint32_t value); + + /// @brief Returns the minimum time to wait between sending BAM frames + /// @returns The minimum time to wait between sending BAM frames + std::uint32_t get_minimum_time_between_transport_protocol_bam_frames() const; + + /// @brief Sets the max number of data frames the stack will use when + /// in an ETP session, between EDPO phases. The default is 255, + /// but decreasing it may reduce bus load at the expense of transfer time. + /// @param[in] numberFrames The max number of data frames to use + void set_max_number_of_etp_frames_per_edpo(std::uint8_t numberFrames); + + /// @brief Returns the max number of data frames the stack will use when + /// in an ETP session, between EDPO phases. The default is 255, + /// but decreasing it may reduce bus load at the expense of transfer time. + /// @returns The number of data frames the stack will use when sending ETP messages between EDPOs + std::uint8_t get_max_number_of_etp_frames_per_edpo() const; + + /// @brief Sets the max number of data frames the stack will send from each + /// transport layer protocol, per update. The default is 255, + /// but decreasing it may reduce bus load at the expense of transfer time. + /// @param[in] numberFrames The max number of frames to use + void set_max_number_of_network_manager_protocol_frames_per_update(std::uint8_t numberFrames); + + /// @brief Returns the max number of data frames the stack will send from each + /// transport layer protocol, per update. The default is 255, + /// but decreasing it may reduce bus load at the expense of transfer time. + /// @returns The max number of frames to use in transport protocols in each network manager update + std::uint8_t get_max_number_of_network_manager_protocol_frames_per_update() const; + + private: + static constexpr std::uint8_t DEFAULT_BAM_PACKET_DELAY_TIME_MS = 50; ///< The default time between BAM frames, as defined by J1939 + + std::uint32_t maxNumberTransportProtocolSessions = 4; ///< The max number of TP sessions allowed + std::uint32_t minimumTimeBetweenTransportProtocolBAMFrames = DEFAULT_BAM_PACKET_DELAY_TIME_MS; ///< The configurable time between BAM frames + std::uint8_t extendedTransportProtocolMaxNumberOfFramesPerEDPO = 0xFF; ///< Used to control throttling of ETP sessions. + std::uint8_t networkManagerMaxFramesToSendPerUpdate = 0xFF; ///< Used to control the max number of transport layer frames added to the driver queue per network manager update + }; +} // namespace isobus + +#endif // CAN_NETWORK_CONFIGURATION_HPP diff --git a/src/can_network_manager.cpp b/src/can_network_manager.cpp new file mode 100644 index 0000000..5584f55 --- /dev/null +++ b/src/can_network_manager.cpp @@ -0,0 +1,1001 @@ +//================================================================================================ +/// @file can_network_manager.cpp +/// +/// @brief The main class that manages the ISOBUS stack including: callbacks, Name to Address +/// management, making control functions, and driving the various protocols. +/// @author Adrian Del Grosso +/// @author Daan Steenbergen +/// +/// @copyright 2022 Adrian Del Grosso +//================================================================================================ + +#include "can_network_manager.hpp" +#include "can_constants.hpp" +#include "can_general_parameter_group_numbers.hpp" +#include "can_hardware_abstraction.hpp" +#include "can_message.hpp" +#include "can_partnered_control_function.hpp" +#include "can_protocol.hpp" +#include "can_stack_logger.hpp" +#include "system_timing.hpp" +#include "to_string.hpp" + +#include +#include +#include +#include + +namespace isobus +{ + CANNetworkManager CANNetworkManager::CANNetwork; + + void CANNetworkManager::initialize() + { + receiveMessageList.clear(); + initialized = true; + transportProtocol.initialize({}); + extendedTransportProtocol.initialize({}); + } + + std::shared_ptr CANNetworkManager::get_control_function(std::uint8_t channelIndex, std::uint8_t address, CANLibBadge) const + { + return get_control_function(channelIndex, address); + } + + void CANNetworkManager::add_global_parameter_group_number_callback(std::uint32_t parameterGroupNumber, CANLibCallback callback, void *parent) + { + globalParameterGroupNumberCallbacks.emplace_back(parameterGroupNumber, callback, parent, nullptr); + } + + void CANNetworkManager::remove_global_parameter_group_number_callback(std::uint32_t parameterGroupNumber, CANLibCallback callback, void *parent) + { + ParameterGroupNumberCallbackData tempObject(parameterGroupNumber, callback, parent, nullptr); + auto callbackLocation = std::find(globalParameterGroupNumberCallbacks.begin(), globalParameterGroupNumberCallbacks.end(), tempObject); + if (globalParameterGroupNumberCallbacks.end() != callbackLocation) + { + globalParameterGroupNumberCallbacks.erase(callbackLocation); + } + } + + std::size_t CANNetworkManager::get_number_global_parameter_group_number_callbacks() const + { + return globalParameterGroupNumberCallbacks.size(); + } + + void CANNetworkManager::add_any_control_function_parameter_group_number_callback(std::uint32_t parameterGroupNumber, CANLibCallback callback, void *parent) + { +#if !defined CAN_STACK_DISABLE_THREADS && !defined ARDUINO + std::lock_guard lock(anyControlFunctionCallbacksMutex); +#endif + anyControlFunctionParameterGroupNumberCallbacks.emplace_back(parameterGroupNumber, callback, parent, nullptr); + } + + void CANNetworkManager::remove_any_control_function_parameter_group_number_callback(std::uint32_t parameterGroupNumber, CANLibCallback callback, void *parent) + { + ParameterGroupNumberCallbackData tempObject(parameterGroupNumber, callback, parent, nullptr); +#if !defined CAN_STACK_DISABLE_THREADS && !defined ARDUINO + std::lock_guard lock(anyControlFunctionCallbacksMutex); +#endif + auto callbackLocation = std::find(anyControlFunctionParameterGroupNumberCallbacks.begin(), anyControlFunctionParameterGroupNumberCallbacks.end(), tempObject); + if (anyControlFunctionParameterGroupNumberCallbacks.end() != callbackLocation) + { + anyControlFunctionParameterGroupNumberCallbacks.erase(callbackLocation); + } + } + + std::shared_ptr CANNetworkManager::get_internal_control_function(std::shared_ptr controlFunction) + { + std::shared_ptr retVal = nullptr; + + if ((nullptr != controlFunction) && + (ControlFunction::Type::Internal == controlFunction->get_type())) + { + retVal = std::static_pointer_cast(controlFunction); + } + return retVal; + } + + float CANNetworkManager::get_estimated_busload(std::uint8_t canChannel) + { +#if !defined CAN_STACK_DISABLE_THREADS && !defined ARDUINO + const std::lock_guard lock(busloadUpdateMutex); +#endif + constexpr float ISOBUS_BAUD_RATE_BPS = 250000.0f; + float retVal = 0.0f; + + if (canChannel < CAN_PORT_MAXIMUM) + { + float totalTimeInAccumulatorWindow = (busloadMessageBitsHistory.at(canChannel).size() * BUSLOAD_UPDATE_FREQUENCY_MS) / 1000.0f; + std::uint32_t totalBitCount = std::accumulate(busloadMessageBitsHistory.at(canChannel).begin(), busloadMessageBitsHistory.at(canChannel).end(), 0); + retVal = (0 != totalTimeInAccumulatorWindow) ? ((totalBitCount / (totalTimeInAccumulatorWindow * ISOBUS_BAUD_RATE_BPS)) * 100.0f) : 0.0f; + } + return retVal; + } + + bool CANNetworkManager::send_can_message(std::uint32_t parameterGroupNumber, + const std::uint8_t *dataBuffer, + std::uint32_t dataLength, + std::shared_ptr sourceControlFunction, + std::shared_ptr destinationControlFunction, + CANIdentifier::CANPriority priority, + TransmitCompleteCallback transmitCompleteCallback, + void *parentPointer, + DataChunkCallback frameChunkCallback) + { + bool retVal = false; + + if (((nullptr != dataBuffer) || + (nullptr != frameChunkCallback)) && + (dataLength > 0) && + (dataLength <= CANMessage::ABSOLUTE_MAX_MESSAGE_LENGTH) && + (nullptr != sourceControlFunction) && + ((parameterGroupNumber == static_cast(CANLibParameterGroupNumber::AddressClaim)) || + (sourceControlFunction->get_address_valid()))) + { + CANLibProtocol *currentProtocol; + + // See if any transport layer protocol can handle this message + for (std::uint32_t i = 0; i < CANLibProtocol::get_number_protocols(); i++) + { + if (CANLibProtocol::get_protocol(i, currentProtocol)) + { + retVal = currentProtocol->protocol_transmit_message(parameterGroupNumber, + dataBuffer, + dataLength, + sourceControlFunction, + destinationControlFunction, + transmitCompleteCallback, + parentPointer, + frameChunkCallback); + + if (retVal) + { + break; + } + } + } + + //! @todo Allow sending 8 byte message with the frameChunkCallback + if ((!retVal) && + (nullptr != dataBuffer)) + { + if (nullptr == destinationControlFunction) + { + // Todo move binding of dest address to hardware layer + retVal = send_can_message_raw(sourceControlFunction->get_can_port(), sourceControlFunction->get_address(), 0xFF, parameterGroupNumber, priority, dataBuffer, dataLength); + } + else if (destinationControlFunction->get_address_valid()) + { + retVal = send_can_message_raw(sourceControlFunction->get_can_port(), sourceControlFunction->get_address(), destinationControlFunction->get_address(), parameterGroupNumber, priority, dataBuffer, dataLength); + } + + if ((retVal) && + (nullptr != transmitCompleteCallback)) + { + // Message was not sent via a protocol, so handle the tx callback now + transmitCompleteCallback(parameterGroupNumber, dataLength, sourceControlFunction, destinationControlFunction, retVal, parentPointer); + } + } + } + return retVal; + } + + void CANNetworkManager::receive_can_message(const CANMessage &message) + { + if (initialized) + { +#if !defined CAN_STACK_DISABLE_THREADS && !defined ARDUINO + std::lock_guard lock(receiveMessageMutex); +#endif + + receiveMessageList.push_back(message); + } + } + + void CANNetworkManager::update() + { +#if !defined CAN_STACK_DISABLE_THREADS && !defined ARDUINO + const std::lock_guard lock(ControlFunction::controlFunctionProcessingMutex); +#endif + + if (!initialized) + { + initialize(); + } + + update_new_partners(); + + process_rx_messages(); + + update_internal_cfs(); + + prune_inactive_control_functions(); + + for (std::size_t i = 0; i < CANLibProtocol::get_number_protocols(); i++) + { + CANLibProtocol *currentProtocol = nullptr; + + if (CANLibProtocol::get_protocol(i, currentProtocol)) + { + if (!currentProtocol->get_is_initialized()) + { + currentProtocol->initialize({}); + } + currentProtocol->update({}); + } + } + update_busload_history(); + updateTimestamp_ms = SystemTiming::get_timestamp_ms(); + } + + bool CANNetworkManager::send_can_message_raw(std::uint32_t portIndex, + std::uint8_t sourceAddress, + std::uint8_t destAddress, + std::uint32_t parameterGroupNumber, + std::uint8_t priority, + const void *data, + std::uint32_t size, + CANLibBadge) const + { + return send_can_message_raw(portIndex, sourceAddress, destAddress, parameterGroupNumber, priority, data, size); + } + + ParameterGroupNumberCallbackData CANNetworkManager::get_global_parameter_group_number_callback(std::uint32_t index) const + { + ParameterGroupNumberCallbackData retVal(0, nullptr, nullptr, nullptr); + + if (index < get_number_global_parameter_group_number_callbacks()) + { + retVal = globalParameterGroupNumberCallbacks[index]; + } + return retVal; + } + + void receive_can_message_frame_from_hardware(const CANMessageFrame &rxFrame) + { + CANNetworkManager::process_receive_can_message_frame(rxFrame); + } + + void on_transmit_can_message_frame_from_hardware(const CANMessageFrame &txFrame) + { + CANNetworkManager::process_transmitted_can_message_frame(txFrame); + } + + void periodic_update_from_hardware() + { + CANNetworkManager::CANNetwork.update(); + } + + void CANNetworkManager::process_receive_can_message_frame(const CANMessageFrame &rxFrame) + { + CANMessage tempCANMessage(rxFrame.channel); + + CANNetworkManager::CANNetwork.update_control_functions(rxFrame); + + tempCANMessage.set_identifier(CANIdentifier(rxFrame.identifier)); + + tempCANMessage.set_source_control_function(CANNetworkManager::CANNetwork.get_control_function(rxFrame.channel, tempCANMessage.get_identifier().get_source_address())); + tempCANMessage.set_destination_control_function(CANNetworkManager::CANNetwork.get_control_function(rxFrame.channel, tempCANMessage.get_identifier().get_destination_address())); + tempCANMessage.set_data(rxFrame.data, rxFrame.dataLength); + + CANNetworkManager::CANNetwork.update_busload(rxFrame.channel, rxFrame.get_number_bits_in_message()); + + CANNetworkManager::CANNetwork.receive_can_message(tempCANMessage); + } + + void CANNetworkManager::process_transmitted_can_message_frame(const CANMessageFrame &txFrame) + { + CANNetworkManager::CANNetwork.update_busload(txFrame.channel, txFrame.get_number_bits_in_message()); + } + + void CANNetworkManager::on_control_function_destroyed(std::shared_ptr controlFunction, CANLibBadge) + { + if (ControlFunction::Type::Internal == controlFunction->get_type()) + { + internalControlFunctions.erase(std::remove(internalControlFunctions.begin(), internalControlFunctions.end(), controlFunction), internalControlFunctions.end()); + } + else if (ControlFunction::Type::Partnered == controlFunction->get_type()) + { + partneredControlFunctions.erase(std::remove(partneredControlFunctions.begin(), partneredControlFunctions.end(), controlFunction), partneredControlFunctions.end()); + } + + auto result = std::find(inactiveControlFunctions.begin(), inactiveControlFunctions.end(), controlFunction); + if (result != inactiveControlFunctions.end()) + { + inactiveControlFunctions.erase(result); + } + + for (std::uint8_t i = 0; i < NULL_CAN_ADDRESS; i++) + { + if (controlFunctionTable[controlFunction->get_can_port()][i] == controlFunction) + { + if (i != controlFunction->get_address()) + { + CANStackLogger::warn("[NM]: %s control function with address '%d' was at incorrect address '%d' in the lookup table prior to deletion.", controlFunction->get_type_string().c_str(), controlFunction->get_address(), i); + } + + if (initialized) + { + // The control function was active, replace it with an new external control function + controlFunctionTable[controlFunction->get_can_port()][controlFunction->address] = ControlFunction::create(controlFunction->get_NAME(), controlFunction->get_address(), controlFunction->get_can_port()); + } + else + { + // The network manager is not initialized yet, just remove the control function from the table + controlFunctionTable[controlFunction->get_can_port()][i] = nullptr; + } + } + } + CANStackLogger::info("[NM]: %s control function with address '%d' is deleted.", controlFunction->get_type_string().c_str(), controlFunction->get_address()); + } + + void CANNetworkManager::on_control_function_created(std::shared_ptr controlFunction, CANLibBadge) + { + on_control_function_created(controlFunction); + } + + void CANNetworkManager::on_control_function_created(std::shared_ptr controlFunction, CANLibBadge) + { + on_control_function_created(controlFunction); + } + + void CANNetworkManager::on_control_function_created(std::shared_ptr controlFunction, CANLibBadge) + { + on_control_function_created(controlFunction); + } + + void CANNetworkManager::add_control_function_status_change_callback(ControlFunctionStateCallback callback) + { + if (nullptr != callback) + { +#if !defined CAN_STACK_DISABLE_THREADS && !defined ARDUINO + const std::lock_guard lock(controlFunctionStatusCallbacksMutex); +#endif + controlFunctionStateCallbacks.emplace_back(callback); + } + } + + void CANNetworkManager::remove_control_function_status_change_callback(ControlFunctionStateCallback callback) + { + if (nullptr != callback) + { +#if !defined CAN_STACK_DISABLE_THREADS && !defined ARDUINO + const std::lock_guard lock(controlFunctionStatusCallbacksMutex); +#endif + ControlFunctionStateCallback targetCallback(callback); + auto callbackLocation = std::find(controlFunctionStateCallbacks.begin(), controlFunctionStateCallbacks.end(), targetCallback); + + if (controlFunctionStateCallbacks.end() != callbackLocation) + { + controlFunctionStateCallbacks.erase(callbackLocation); + } + } + } + + const std::list> &CANNetworkManager::get_internal_control_functions() const + { + return internalControlFunctions; + } + + const std::list> &CANNetworkManager::get_partnered_control_functions() const + { + return partneredControlFunctions; + } + + FastPacketProtocol &CANNetworkManager::get_fast_packet_protocol() + { + return fastPacketProtocol; + } + + CANNetworkConfiguration &CANNetworkManager::get_configuration() + { + return configuration; + } + + bool CANNetworkManager::add_protocol_parameter_group_number_callback(std::uint32_t parameterGroupNumber, CANLibCallback callback, void *parentPointer) + { + bool retVal = false; + ParameterGroupNumberCallbackData callbackInfo(parameterGroupNumber, callback, parentPointer, nullptr); +#if !defined CAN_STACK_DISABLE_THREADS && !defined ARDUINO + const std::lock_guard lock(protocolPGNCallbacksMutex); +#endif + if ((nullptr != callback) && (protocolPGNCallbacks.end() == find(protocolPGNCallbacks.begin(), protocolPGNCallbacks.end(), callbackInfo))) + { + protocolPGNCallbacks.push_back(callbackInfo); + retVal = true; + } + return retVal; + } + + bool CANNetworkManager::remove_protocol_parameter_group_number_callback(std::uint32_t parameterGroupNumber, CANLibCallback callback, void *parentPointer) + { + bool retVal = false; + ParameterGroupNumberCallbackData callbackInfo(parameterGroupNumber, callback, parentPointer, nullptr); +#if !defined CAN_STACK_DISABLE_THREADS && !defined ARDUINO + const std::lock_guard lock(protocolPGNCallbacksMutex); +#endif + if (nullptr != callback) + { + std::list::iterator callbackLocation; + callbackLocation = find(protocolPGNCallbacks.begin(), protocolPGNCallbacks.end(), callbackInfo); + + if (protocolPGNCallbacks.end() != callbackLocation) + { + protocolPGNCallbacks.erase(callbackLocation); + retVal = true; + } + } + return retVal; + } + + CANNetworkManager::CANNetworkManager() + { + currentBusloadBitAccumulator.fill(0); + lastAddressClaimRequestTimestamp_ms.fill(0); + controlFunctionTable.fill({ nullptr }); + } + + void CANNetworkManager::update_address_table(const CANMessage &message) + { + std::uint8_t channelIndex = message.get_can_port_index(); + + if ((static_cast(CANLibParameterGroupNumber::AddressClaim) == message.get_identifier().get_parameter_group_number()) && + (channelIndex < CAN_PORT_MAXIMUM)) + { + std::uint8_t claimedAddress = message.get_identifier().get_source_address(); + auto targetControlFunction = controlFunctionTable[channelIndex][claimedAddress]; + if ((nullptr != targetControlFunction) && + (CANIdentifier::NULL_ADDRESS == targetControlFunction->get_address())) + { + // Someone is at that spot in the table, but their address was stolen + // Need to evict them from the table and move them to the inactive list + targetControlFunction->address = NULL_CAN_ADDRESS; + inactiveControlFunctions.push_back(targetControlFunction); + CANStackLogger::info("[NM]: %s CF '%016llx' is evicted from address '%d' on channel '%d', as their address is probably stolen.", + targetControlFunction->get_type_string().c_str(), + targetControlFunction->get_NAME().get_full_name(), + claimedAddress, + channelIndex); + targetControlFunction = nullptr; + } + + if (targetControlFunction != nullptr) + { + targetControlFunction->claimedAddressSinceLastAddressClaimRequest = true; + } + else + { + // Look through all inactive CFs, maybe one of them has freshly claimed the address + for (auto currentControlFunction : inactiveControlFunctions) + { + if ((currentControlFunction->get_address() == claimedAddress) && + (currentControlFunction->get_can_port() == channelIndex)) + { + controlFunctionTable[channelIndex][claimedAddress] = currentControlFunction; + CANStackLogger::debug("[NM]: %s CF '%016llx' is now active at address '%d' on channel '%d'.", + currentControlFunction->get_type_string().c_str(), + currentControlFunction->get_NAME().get_full_name(), + claimedAddress, + channelIndex); + process_control_function_state_change_callback(currentControlFunction, ControlFunctionState::Online); + break; + } + } + } + } + else if ((static_cast(CANLibParameterGroupNumber::ParameterGroupNumberRequest) == message.get_identifier().get_parameter_group_number()) && + (channelIndex < CAN_PORT_MAXIMUM)) + { + auto requestedPGN = message.get_uint24_at(0); + + if (static_cast(CANLibParameterGroupNumber::AddressClaim) == requestedPGN) + { + lastAddressClaimRequestTimestamp_ms.at(channelIndex) = SystemTiming::get_timestamp_ms(); + + // Reset the claimedAddressSinceLastAddressClaimRequest flag for all control functions on the port + auto result = std::find_if(inactiveControlFunctions.begin(), inactiveControlFunctions.end(), [channelIndex](std::shared_ptr controlFunction) { + return (channelIndex == controlFunction->get_can_port()); + }); + if (result != inactiveControlFunctions.end()) + { + (*result)->claimedAddressSinceLastAddressClaimRequest = true; + } + std::for_each(controlFunctionTable[channelIndex].begin(), controlFunctionTable[channelIndex].end(), [](std::shared_ptr controlFunction) { + if (nullptr != controlFunction) + { + controlFunction->claimedAddressSinceLastAddressClaimRequest = false; + } + }); + } + } + } + + void CANNetworkManager::update_internal_cfs() + { + for (const auto ¤tInternalControlFunction : internalControlFunctions) + { + if (currentInternalControlFunction->update_address_claiming({})) + { + std::uint8_t channelIndex = currentInternalControlFunction->get_can_port(); + std::uint8_t claimedAddress = currentInternalControlFunction->get_address(); + + // Check if the internal control function switched addresses, and therefore needs to be moved in the table + for (std::uint8_t address = 0; address < NULL_CAN_ADDRESS; address++) + { + if (controlFunctionTable[channelIndex][address] == currentInternalControlFunction) + { + controlFunctionTable[channelIndex][address] = nullptr; + break; + } + } + + if (nullptr != controlFunctionTable[channelIndex][claimedAddress]) + { + // Someone is at that spot in the table, but their address was stolen by an internal control function + // Need to evict them from the table + controlFunctionTable[channelIndex][claimedAddress]->address = NULL_CAN_ADDRESS; + controlFunctionTable[channelIndex][claimedAddress] = nullptr; + } + + // ECU has claimed since the last update, add it to the table + controlFunctionTable[channelIndex][claimedAddress] = currentInternalControlFunction; + } + } + } + + void CANNetworkManager::update_busload(std::uint8_t channelIndex, std::uint32_t numberOfBitsProcessed) + { +#if !defined CAN_STACK_DISABLE_THREADS && !defined ARDUINO + const std::lock_guard lock(CANNetworkManager::CANNetwork.busloadUpdateMutex); +#endif + currentBusloadBitAccumulator.at(channelIndex) += numberOfBitsProcessed; + } + + void CANNetworkManager::update_busload_history() + { +#if !defined CAN_STACK_DISABLE_THREADS && !defined ARDUINO + const std::lock_guard lock(busloadUpdateMutex); +#endif + if (SystemTiming::time_expired_ms(busloadUpdateTimestamp_ms, BUSLOAD_UPDATE_FREQUENCY_MS)) + { + for (std::size_t i = 0; i < busloadMessageBitsHistory.size(); i++) + { + busloadMessageBitsHistory.at(i).push_back(currentBusloadBitAccumulator.at(i)); + + while (busloadMessageBitsHistory.at(i).size() > (BUSLOAD_SAMPLE_WINDOW_MS / BUSLOAD_UPDATE_FREQUENCY_MS)) + { + busloadMessageBitsHistory.at(i).pop_front(); + } + currentBusloadBitAccumulator.at(i) = 0; + } + busloadUpdateTimestamp_ms = SystemTiming::get_timestamp_ms(); + } + } + + void CANNetworkManager::update_control_functions(const CANMessageFrame &rxFrame) + { + if ((static_cast(CANLibParameterGroupNumber::AddressClaim) == CANIdentifier(rxFrame.identifier).get_parameter_group_number()) && + (CAN_DATA_LENGTH == rxFrame.dataLength) && + (rxFrame.channel < CAN_PORT_MAXIMUM)) + { + std::uint64_t claimedNAME; + std::shared_ptr foundControlFunction = nullptr; + uint8_t claimedAddress = CANIdentifier(rxFrame.identifier).get_source_address(); + + claimedNAME = rxFrame.data[0]; + claimedNAME |= (static_cast(rxFrame.data[1]) << 8); + claimedNAME |= (static_cast(rxFrame.data[2]) << 16); + claimedNAME |= (static_cast(rxFrame.data[3]) << 24); + claimedNAME |= (static_cast(rxFrame.data[4]) << 32); + claimedNAME |= (static_cast(rxFrame.data[5]) << 40); + claimedNAME |= (static_cast(rxFrame.data[6]) << 48); + claimedNAME |= (static_cast(rxFrame.data[7]) << 56); + + // Check if the claimed NAME is someone we already know about + auto activeResult = std::find_if(controlFunctionTable[rxFrame.channel].begin(), + controlFunctionTable[rxFrame.channel].end(), + [claimedNAME](const std::shared_ptr &cf) { + return (nullptr != cf) && (cf->controlFunctionNAME.get_full_name() == claimedNAME); + }); + if (activeResult != controlFunctionTable[rxFrame.channel].end()) + { + foundControlFunction = *activeResult; + } + else + { + auto inActiveResult = std::find_if(inactiveControlFunctions.begin(), + inactiveControlFunctions.end(), + [claimedNAME, &rxFrame](const std::shared_ptr &cf) { + return (cf->controlFunctionNAME.get_full_name() == claimedNAME) && (cf->get_can_port() == rxFrame.channel); + }); + if (inActiveResult != inactiveControlFunctions.end()) + { + foundControlFunction = *inActiveResult; + } + } + + if (nullptr == foundControlFunction) + { + // If we still haven't found it, it might be a partner. Check the list of partners. + for (const auto &partner : partneredControlFunctions) + { + if ((partner->get_can_port() == rxFrame.channel) && + (partner->check_matches_name(NAME(claimedNAME))) && + (0 == partner->get_NAME().get_full_name())) + { + partner->controlFunctionNAME = NAME(claimedNAME); + foundControlFunction = partner; + controlFunctionTable[rxFrame.channel][claimedAddress] = foundControlFunction; + break; + } + } + } + + // Remove any CF that has the same address as the one claiming + std::for_each(controlFunctionTable[rxFrame.channel].begin(), + controlFunctionTable[rxFrame.channel].end(), + [&foundControlFunction, &claimedAddress](const std::shared_ptr &cf) { + if ((nullptr != cf) && (foundControlFunction != cf) && (cf->address == claimedAddress)) + cf->address = CANIdentifier::NULL_ADDRESS; + }); + + std::for_each(inactiveControlFunctions.begin(), + inactiveControlFunctions.end(), + [&rxFrame, &foundControlFunction, &claimedAddress](const std::shared_ptr &cf) { + if ((foundControlFunction != cf) && (cf->address == claimedAddress) && (cf->get_can_port() == rxFrame.channel)) + cf->address = CANIdentifier::NULL_ADDRESS; + }); + + if (nullptr == foundControlFunction) + { + // New device, need to start keeping track of it + foundControlFunction = ControlFunction::create(NAME(claimedNAME), claimedAddress, rxFrame.channel); + controlFunctionTable[rxFrame.channel][foundControlFunction->get_address()] = foundControlFunction; + CANStackLogger::debug("[NM]: A control function claimed address %u on channel %u", foundControlFunction->get_address(), foundControlFunction->get_can_port()); + } + else if (foundControlFunction->address != claimedAddress) + { + if (foundControlFunction->get_address_valid()) + { + controlFunctionTable[rxFrame.channel][claimedAddress] = foundControlFunction; + controlFunctionTable[rxFrame.channel][foundControlFunction->get_address()] = nullptr; + CANStackLogger::info("[NM]: The %s control function at address %d changed it's address to %d on channel %u.", + foundControlFunction->get_type_string().c_str(), + foundControlFunction->get_address(), + claimedAddress, + foundControlFunction->get_can_port()); + } + else + { + CANStackLogger::info("[NM]: %s control function with name %016llx has claimed address %u on channel %u.", + foundControlFunction->get_type_string().c_str(), + foundControlFunction->get_NAME().get_full_name(), + claimedAddress, + foundControlFunction->get_can_port()); + process_control_function_state_change_callback(foundControlFunction, ControlFunctionState::Online); + } + foundControlFunction->address = claimedAddress; + } + } + } + + void CANNetworkManager::update_new_partners() + { + for (const auto &partner : partneredControlFunctions) + { + if (!partner->initialized) + { + // Remove any inactive CF that matches the partner's name + for (auto currentInactiveControlFunction = inactiveControlFunctions.begin(); currentInactiveControlFunction != inactiveControlFunctions.end(); currentInactiveControlFunction++) + { + if ((partner->check_matches_name((*currentInactiveControlFunction)->get_NAME())) && + (partner->get_can_port() == (*currentInactiveControlFunction)->get_can_port()) && + (ControlFunction::Type::External == (*currentInactiveControlFunction)->get_type())) + { + inactiveControlFunctions.erase(currentInactiveControlFunction); + break; + } + } + + for (const auto ¤tActiveControlFunction : controlFunctionTable[partner->get_can_port()]) + { + if ((nullptr != currentActiveControlFunction) && + (partner->check_matches_name(currentActiveControlFunction->get_NAME())) && + (ControlFunction::Type::External == currentActiveControlFunction->get_type())) + { + // This CF matches the filter and is not an internal or already partnered CF + CANStackLogger::info("[NM]: A partner with name %016llx has claimed address %u on channel %u.", + partner->get_NAME().get_full_name(), + partner->get_address(), + partner->get_can_port()); + + // Populate the partner's data + partner->address = currentActiveControlFunction->get_address(); + partner->controlFunctionNAME = currentActiveControlFunction->get_NAME(); + partner->initialized = true; + controlFunctionTable[partner->get_can_port()][partner->address] = std::shared_ptr(partner); + process_control_function_state_change_callback(partner, ControlFunctionState::Online); + break; + } + } + partner->initialized = true; + } + } + } + + CANMessageFrame CANNetworkManager::construct_frame(std::uint32_t portIndex, + std::uint8_t sourceAddress, + std::uint8_t destAddress, + std::uint32_t parameterGroupNumber, + std::uint8_t priority, + const void *data, + std::uint32_t size) const + { + CANMessageFrame txFrame; + txFrame.identifier = DEFAULT_IDENTIFIER; + + if ((NULL_CAN_ADDRESS != destAddress) && (priority <= static_cast(CANIdentifier::CANPriority::PriorityLowest7)) && (size <= CAN_DATA_LENGTH) && (nullptr != data)) + { + std::uint32_t identifier = 0; + + // Manually encode an identifier + identifier |= ((priority & 0x07) << 26); + identifier |= (sourceAddress & 0xFF); + + if (BROADCAST_CAN_ADDRESS == destAddress) + { + if ((parameterGroupNumber & 0xF000) >= 0xF000) + { + identifier |= ((parameterGroupNumber & 0x3FFFF) << 8); + } + else + { + identifier |= (destAddress << 8); + identifier |= ((parameterGroupNumber & 0x3FF00) << 8); + } + } + else + { + if ((parameterGroupNumber & 0xF000) < 0xF000) + { + identifier |= (destAddress << 8); + identifier |= ((parameterGroupNumber & 0x3FF00) << 8); + } + else + { + CANStackLogger::warn("[NM]: Cannot send a message with PGN " + + isobus::to_string(static_cast(parameterGroupNumber)) + + " as a destination specific message. " + + "Try resending it using nullptr as your destination control function."); + identifier = DEFAULT_IDENTIFIER; + } + } + + if (DEFAULT_IDENTIFIER != identifier) + { + txFrame.channel = portIndex; + memcpy(reinterpret_cast(txFrame.data), data, size); + txFrame.dataLength = size; + txFrame.isExtendedFrame = true; + txFrame.identifier = identifier & 0x1FFFFFFF; + } + } + return txFrame; + } + + std::shared_ptr CANNetworkManager::get_control_function(std::uint8_t channelIndex, std::uint8_t address) const + { + std::shared_ptr retVal = nullptr; + + if ((address < NULL_CAN_ADDRESS) && (channelIndex < CAN_PORT_MAXIMUM)) + { + retVal = controlFunctionTable[channelIndex][address]; + } + return retVal; + } + + CANMessage CANNetworkManager::get_next_can_message_from_rx_queue() + { +#if !defined CAN_STACK_DISABLE_THREADS && !defined ARDUINO + std::lock_guard lock(receiveMessageMutex); +#endif + CANMessage retVal = receiveMessageList.front(); + receiveMessageList.pop_front(); + return retVal; + } + + std::size_t CANNetworkManager::get_number_can_messages_in_rx_queue() + { +#if !defined CAN_STACK_DISABLE_THREADS && !defined ARDUINO + std::lock_guard lock(receiveMessageMutex); +#endif + return receiveMessageList.size(); + } + + void CANNetworkManager::on_control_function_created(std::shared_ptr controlFunction) + { + if (ControlFunction::Type::Internal == controlFunction->get_type()) + { + internalControlFunctions.push_back(std::static_pointer_cast(controlFunction)); + } + else if (ControlFunction::Type::Partnered == controlFunction->get_type()) + { + partneredControlFunctions.push_back(std::static_pointer_cast(controlFunction)); + } + } + + void CANNetworkManager::process_any_control_function_pgn_callbacks(const CANMessage ¤tMessage) + { +#if !defined CAN_STACK_DISABLE_THREADS && !defined ARDUINO + const std::lock_guard lock(anyControlFunctionCallbacksMutex); +#endif + for (const auto ¤tCallback : anyControlFunctionParameterGroupNumberCallbacks) + { + if ((currentCallback.get_parameter_group_number() == currentMessage.get_identifier().get_parameter_group_number()) && + ((nullptr == currentMessage.get_destination_control_function()) || + (ControlFunction::Type::Internal == currentMessage.get_destination_control_function()->get_type()))) + { + currentCallback.get_callback()(currentMessage, currentCallback.get_parent()); + } + } + } + + void CANNetworkManager::process_control_function_state_change_callback(std::shared_ptr controlFunction, ControlFunctionState state) + { +#if !defined CAN_STACK_DISABLE_THREADS && !defined ARDUINO + const std::lock_guard lock(controlFunctionStatusCallbacksMutex); +#endif + for (const auto &callback : controlFunctionStateCallbacks) + { + callback(controlFunction, state); + } + } + + void CANNetworkManager::process_protocol_pgn_callbacks(const CANMessage ¤tMessage) + { +#if !defined CAN_STACK_DISABLE_THREADS && !defined ARDUINO + const std::lock_guard lock(protocolPGNCallbacksMutex); +#endif + for (const auto ¤tCallback : protocolPGNCallbacks) + { + if (currentCallback.get_parameter_group_number() == currentMessage.get_identifier().get_parameter_group_number()) + { + currentCallback.get_callback()(currentMessage, currentCallback.get_parent()); + } + } + } + + void CANNetworkManager::process_can_message_for_global_and_partner_callbacks(const CANMessage &message) + { + std::shared_ptr messageDestination = message.get_destination_control_function(); + if ((nullptr == messageDestination) && + ((nullptr != message.get_source_control_function()) || + ((static_cast(CANLibParameterGroupNumber::ParameterGroupNumberRequest) == message.get_identifier().get_parameter_group_number()) && + (NULL_CAN_ADDRESS == message.get_identifier().get_source_address())))) + { + // Message destined to global + for (std::size_t i = 0; i < get_number_global_parameter_group_number_callbacks(); i++) + { + if ((message.get_identifier().get_parameter_group_number() == get_global_parameter_group_number_callback(i).get_parameter_group_number()) && + (nullptr != get_global_parameter_group_number_callback(i).get_callback())) + { + // We have a callback that matches this PGN + get_global_parameter_group_number_callback(i).get_callback()(message, get_global_parameter_group_number_callback(i).get_parent()); + } + } + } + else if ((messageDestination != nullptr) && (messageDestination->get_type() == ControlFunction::Type::Internal)) + { + // Message is destined to us + for (const auto &partner : partneredControlFunctions) + { + if ((nullptr != partner) && + (partner->get_can_port() == message.get_can_port_index())) + { + // Message matches CAN port for a partnered control function + for (std::size_t k = 0; k < partner->get_number_parameter_group_number_callbacks(); k++) + { + if ((message.get_identifier().get_parameter_group_number() == partner->get_parameter_group_number_callback(k).get_parameter_group_number()) && + (nullptr != partner->get_parameter_group_number_callback(k).get_callback()) && + ((nullptr == partner->get_parameter_group_number_callback(k).get_internal_control_function()) || + (partner->get_parameter_group_number_callback(k).get_internal_control_function()->get_address() == message.get_identifier().get_destination_address()))) + { + // We have a callback matching this message + partner->get_parameter_group_number_callback(k).get_callback()(message, partner->get_parameter_group_number_callback(k).get_parent()); + } + } + } + } + } + } + + void CANNetworkManager::process_can_message_for_commanded_address(const CANMessage &message) + { + constexpr std::uint8_t COMMANDED_ADDRESS_LENGTH = 9; + + if ((nullptr == message.get_destination_control_function()) && + (static_cast(CANLibParameterGroupNumber::CommandedAddress) == message.get_identifier().get_parameter_group_number()) && + (COMMANDED_ADDRESS_LENGTH == message.get_data_length())) + { + std::uint64_t targetNAME = message.get_uint64_at(0); + + for (const auto ¤tICF : internalControlFunctions) + { + if ((message.get_can_port_index() == currentICF->get_can_port()) && + (currentICF->get_NAME().get_full_name() == targetNAME)) + { + currentICF->process_commanded_address(message.get_uint8_at(8), {}); + } + } + } + } + + void CANNetworkManager::process_rx_messages() + { + while (0 != get_number_can_messages_in_rx_queue()) + { + CANMessage currentMessage = get_next_can_message_from_rx_queue(); + + update_address_table(currentMessage); + + // Update Special Callbacks, like protocols and non-cf specific ones + process_protocol_pgn_callbacks(currentMessage); + process_any_control_function_pgn_callbacks(currentMessage); + + // Update Others + process_can_message_for_global_and_partner_callbacks(currentMessage); + } + } + + void CANNetworkManager::prune_inactive_control_functions() + { + for (std::uint_fast8_t channelIndex = 0; channelIndex < CAN_PORT_MAXIMUM; channelIndex++) + { + constexpr std::uint32_t MAX_ADDRESS_CLAIM_RESOLUTION_TIME = 755; // This is 250ms + RTxD + 250ms + if ((0 != lastAddressClaimRequestTimestamp_ms.at(channelIndex)) && + (SystemTiming::time_expired_ms(lastAddressClaimRequestTimestamp_ms.at(channelIndex), MAX_ADDRESS_CLAIM_RESOLUTION_TIME))) + { + for (std::uint_fast8_t i = 0; i < NULL_CAN_ADDRESS; i++) + { + auto controlFunction = controlFunctionTable[channelIndex][i]; + if ((nullptr != controlFunction) && + (!controlFunction->claimedAddressSinceLastAddressClaimRequest) && + (ControlFunction::Type::Internal != controlFunction->get_type())) + { + inactiveControlFunctions.push_back(controlFunction); + CANStackLogger::info("[NM]: Control function with address %u and NAME %016llx is now offline on channel %u.", controlFunction->get_address(), controlFunction->get_NAME(), channelIndex); + controlFunctionTable[channelIndex][i] = nullptr; + controlFunction->address = NULL_CAN_ADDRESS; + process_control_function_state_change_callback(controlFunction, ControlFunctionState::Offline); + } + else if ((nullptr != controlFunction) && + (!controlFunction->claimedAddressSinceLastAddressClaimRequest)) + { + process_control_function_state_change_callback(controlFunction, ControlFunctionState::Offline); + } + } + lastAddressClaimRequestTimestamp_ms.at(channelIndex) = 0; + } + } + } + + bool CANNetworkManager::send_can_message_raw(std::uint32_t portIndex, std::uint8_t sourceAddress, std::uint8_t destAddress, std::uint32_t parameterGroupNumber, std::uint8_t priority, const void *data, std::uint32_t size) const + { + CANMessageFrame tempFrame = construct_frame(portIndex, sourceAddress, destAddress, parameterGroupNumber, priority, data, size); + bool retVal = false; + + if ((DEFAULT_IDENTIFIER != tempFrame.identifier) && + (portIndex < CAN_PORT_MAXIMUM)) + { + retVal = send_can_message_frame_to_hardware(tempFrame); + } + return retVal; + } + + void CANNetworkManager::protocol_message_callback(const CANMessage &message) + { + process_can_message_for_global_and_partner_callbacks(message); + process_can_message_for_commanded_address(message); + } + +} // namespace isobus diff --git a/src/can_network_manager.hpp b/src/can_network_manager.hpp new file mode 100644 index 0000000..3c1ec54 --- /dev/null +++ b/src/can_network_manager.hpp @@ -0,0 +1,378 @@ +//================================================================================================ +/// @file can_network_manager.hpp +/// +/// @brief The main class that manages the ISOBUS stack including: callbacks, Name to Address +/// management, making control functions, and driving the various protocols. +/// @author Adrian Del Grosso +/// @author Daan Steenbergen +/// +/// @copyright 2022 Adrian Del Grosso +//================================================================================================ + +#ifndef CAN_NETWORK_MANAGER_HPP +#define CAN_NETWORK_MANAGER_HPP + +#include "can_address_claim_state_machine.hpp" +#include "can_badge.hpp" +#include "can_callbacks.hpp" +#include "can_constants.hpp" +#include "can_extended_transport_protocol.hpp" +#include "can_identifier.hpp" +#include "can_internal_control_function.hpp" +#include "can_message.hpp" +#include "can_message_frame.hpp" +#include "can_network_configuration.hpp" +#include "can_transport_protocol.hpp" +#include "nmea2000_fast_packet_protocol.hpp" + +#include +#include +#include +#include + +#if !defined CAN_STACK_DISABLE_THREADS && !defined ARDUINO +#include +#endif + +/// @brief This namespace encompasses all of the ISO11783 stack's functionality to reduce global namespace pollution +namespace isobus +{ + class PartneredControlFunction; + + //================================================================================================ + /// @class CANNetworkManager + /// + /// @brief The main CAN network manager object, handles protocol management and updating other + /// stack components. Provides an interface for sending CAN messages. + //================================================================================================ + class CANNetworkManager + { + public: + static CANNetworkManager CANNetwork; ///< Static singleton of the one network manager. Use this to access stack functionality. + + /// @brief Initializer function for the network manager + void initialize(); + + /// @brief Called only by the stack, returns a control function based on certain port and address + /// @param[in] channelIndex CAN Channel index of the control function + /// @param[in] address Address of the control function + /// @returns A control function that matches the parameters, or nullptr if no match was found + std::shared_ptr get_control_function(std::uint8_t channelIndex, std::uint8_t address, CANLibBadge) const; + + /// @brief This is how you register a callback for any PGN destined for the global address (0xFF) + /// @param[in] parameterGroupNumber The PGN you want to register for + /// @param[in] callback The callback that will be called when parameterGroupNumber is received from the global address (0xFF) + /// @param[in] parent A generic context variable that helps identify what object the callback is destined for. Can be nullptr if you don't want to use it. + void add_global_parameter_group_number_callback(std::uint32_t parameterGroupNumber, CANLibCallback callback, void *parent); + + /// @brief This is how you remove a callback for any PGN destined for the global address (0xFF) + /// @param[in] parameterGroupNumber The PGN of the callback to remove + /// @param[in] callback The callback that will be removed + /// @param[in] parent A generic context variable that helps identify what object the callback was destined for + void remove_global_parameter_group_number_callback(std::uint32_t parameterGroupNumber, CANLibCallback callback, void *parent); + + /// @brief Returns the number of global PGN callbacks that have been registered with the network manager + /// @returns The number of global PGN callbacks that have been registered with the network manager + std::size_t get_number_global_parameter_group_number_callbacks() const; + + /// @brief Registers a callback for ANY control function sending the associated PGN + /// @param[in] parameterGroupNumber The PGN you want to register for + /// @param[in] callback The callback that will be called when parameterGroupNumber is received from any control function + /// @param[in] parent A generic context variable that helps identify what object the callback is destined for. Can be nullptr if you don't want to use it. + void add_any_control_function_parameter_group_number_callback(std::uint32_t parameterGroupNumber, CANLibCallback callback, void *parent); + + /// @brief This is how you remove a callback added with add_any_control_function_parameter_group_number_callback + /// @param[in] parameterGroupNumber The PGN of the callback to remove + /// @param[in] callback The callback that will be removed + /// @param[in] parent A generic context variable that helps identify what object the callback was destined for + void remove_any_control_function_parameter_group_number_callback(std::uint32_t parameterGroupNumber, CANLibCallback callback, void *parent); + + /// @brief Returns an internal control function if the passed-in control function is an internal type + /// @returns An internal control function casted from the passed in control function + std::shared_ptr get_internal_control_function(std::shared_ptr controlFunction); + + /// @brief Returns an estimated busload between 0.0f and 100.0f + /// @details This calculates busload over a 1 second window. + /// @note This function averages between best and worst case bit-stuffing. + /// This may be more or less aggressive than the actual amount of bit stuffing. Knowing + /// the actual amount of bit stuffing is impossible, so this should only be used as an estimate. + /// @param[in] canChannel The channel to estimate the bus load for + /// @returns Estimated busload over the last 1 second + float get_estimated_busload(std::uint8_t canChannel); + + /// @brief This is the main way to send a CAN message of any length. + /// @details This function will automatically choose an appropriate transport protocol if needed. + /// If you don't specify a destination (or use nullptr) you message will be sent as a broadcast + /// if it is valid to do so. + /// You can also get a callback on success or failure of the transmit. + /// @returns `true` if the message was sent, otherwise `false` + bool send_can_message(std::uint32_t parameterGroupNumber, + const std::uint8_t *dataBuffer, + std::uint32_t dataLength, + std::shared_ptr sourceControlFunction, + std::shared_ptr destinationControlFunction = nullptr, + CANIdentifier::CANPriority priority = CANIdentifier::CANPriority::PriorityDefault6, + TransmitCompleteCallback txCompleteCallback = nullptr, + void *parentPointer = nullptr, + DataChunkCallback frameChunkCallback = nullptr); + + /// @brief This is the main function used by the stack to receive CAN messages and add them to a queue. + /// @details This function is called by the stack itself when you call can_lib_process_rx_message. + /// @param[in] message The message to be received + void receive_can_message(const CANMessage &message); + + /// @brief The main update function for the network manager. Updates all protocols. + void update(); + + /// @brief Process the CAN Rx queue + /// @param[in] rxFrame Frame to process + static void process_receive_can_message_frame(const CANMessageFrame &rxFrame); + + /// @brief Used to tell the network manager when frames are emitted on the bus, so that they can be + /// added to the internal bus load calculations. + /// @param[in] txFrame The frame that was just emitted onto the bus + static void process_transmitted_can_message_frame(const CANMessageFrame &txFrame); + + /// @brief Informs the network manager that a control function object has been destroyed, so that it can be purged from the network manager + /// @param[in] controlFunction The control function that was destroyed + void on_control_function_destroyed(std::shared_ptr controlFunction, CANLibBadge); + + /// @brief Informs the network manager that a control function object has been created, so that it can be added to the network manager + /// @param[in] controlFunction The control function that was created + void on_control_function_created(std::shared_ptr controlFunction, CANLibBadge); + + /// @brief Informs the network manager that a control function object has been created, so that it can be added to the network manager + /// @param[in] controlFunction The control function that was created + void on_control_function_created(std::shared_ptr controlFunction, CANLibBadge); + + /// @brief Informs the network manager that a control function object has been created, so that it can be added to the network manager + /// @param[in] controlFunction The control function that was created + void on_control_function_created(std::shared_ptr controlFunction, CANLibBadge); + + /// @brief Use this to get a callback when a control function goes online or offline. + /// This could be useful if you want event driven notifications for when your partners are disconnected from the bus. + /// @param[in] callback The callback you want to be called when the any control function changes state + void add_control_function_status_change_callback(ControlFunctionStateCallback callback); + + /// @brief Used to remove callbacks added with add_control_function_status_change_callback + /// @param[in] callback The callback you want to remove + void remove_control_function_status_change_callback(ControlFunctionStateCallback callback); + + /// @brief Gets all the internal control functions that are currently registered in the network manager + /// @returns A list of all the internal control functions + const std::list> &get_internal_control_functions() const; + + /// @brief Gets all the partnered control functions that are currently registered in the network manager + /// @returns A list of all the partnered control functions + const std::list> &get_partnered_control_functions() const; + + /// @brief Returns the class instance of the NMEA2k fast packet protocol. + /// Use this to register for FP multipacket messages + /// @returns The class instance of the NMEA2k fast packet protocol. + FastPacketProtocol &get_fast_packet_protocol(); + + /// @brief Returns the configuration of this network manager + /// @returns The configuration class for this network manager + CANNetworkConfiguration &get_configuration(); + + protected: + // Using protected region to allow protocols use of special functions from the network manager + friend class AddressClaimStateMachine; ///< Allows the network manager to work closely with the address claiming process + friend class ExtendedTransportProtocolManager; ///< Allows the network manager to access the ETP manager + friend class TransportProtocolManager; ///< Allows the network manager to work closely with the transport protocol manager object + friend class DiagnosticProtocol; ///< Allows the diagnostic protocol to access the protected functions on the network manager + friend class ParameterGroupNumberRequestProtocol; ///< Allows the PGN request protocol to access the network manager protected functions + friend class FastPacketProtocol; ///< Allows the FP protocol to access the network manager protected functions + friend class CANLibProtocol; + + /// @brief Adds a PGN callback for a protocol class + /// @param[in] parameterGroupNumber The PGN to register for + /// @param[in] callback The callback to call when the PGN is received + /// @param[in] parentPointer A generic context variable that helps identify what object the callback was destined for + /// @returns `true` if the callback was added, otherwise `false` + bool add_protocol_parameter_group_number_callback(std::uint32_t parameterGroupNumber, CANLibCallback callback, void *parentPointer); + + /// @brief Removes a PGN callback for a protocol class + /// @param[in] parameterGroupNumber The PGN to register for + /// @param[in] callback The callback to call when the PGN is received + /// @param[in] parentPointer A generic context variable that helps identify what object the callback was destined for + /// @returns `true` if the callback was removed, otherwise `false` + bool remove_protocol_parameter_group_number_callback(std::uint32_t parameterGroupNumber, CANLibCallback callback, void *parentPointer); + + /// @brief Sends a CAN message using raw addresses. Used only by the stack. + /// @param[in] portIndex The CAN channel index to send the message from + /// @param[in] sourceAddress The source address to send the CAN message from + /// @param[in] destAddress The destination address to send the message to + /// @param[in] parameterGroupNumber The PGN to use when sending the message + /// @param[in] priority The CAN priority of the message being sent + /// @param[in] data A pointer to the data buffer to send from + /// @param[in] size The size of the message to send + /// @returns `true` if the message was sent, otherwise `false` + bool send_can_message_raw(std::uint32_t portIndex, + std::uint8_t sourceAddress, + std::uint8_t destAddress, + std::uint32_t parameterGroupNumber, + std::uint8_t priority, + const void *data, + std::uint32_t size, + CANLibBadge) const; + + /// @brief Processes completed protocol messages. Causes PGN callbacks to trigger. + /// @param[in] message The completed protocol message + void protocol_message_callback(const CANMessage &message); + + std::vector protocolList; ///< A list of all created protocol classes + + private: + /// @brief Constructor for the network manager. Sets default values for members + CANNetworkManager(); + + /// @brief Updates the internal address table based on a received CAN message + /// @param[in] message A message being received by the stack + void update_address_table(const CANMessage &message); + + /// @brief Updates the internal address table based on updates to internal cfs addresses + void update_internal_cfs(); + + /// @brief Processes a CAN message's contribution to the current busload + /// @param[in] channelIndex The CAN channel index associated to the message being processed + /// @param[in] numberOfBitsProcessed The number of bits to add to the busload calculation + void update_busload(std::uint8_t channelIndex, std::uint32_t numberOfBitsProcessed); + + /// @brief Updates the stored bit accumulators for calculating the bus load over a multiple sample windows + void update_busload_history(); + + /// @brief Creates new control function classes based on the frames coming in from the bus + /// @param[in] rxFrame Raw frames coming in from the bus + void update_control_functions(const CANMessageFrame &rxFrame); + + /// @brief Checks if new partners have been created and matches them to existing control functions + void update_new_partners(); + + /// @brief Builds a CAN frame from a frame's discrete components + /// @param[in] portIndex The CAN channel index of the CAN message being processed + /// @param[in] sourceAddress The source address to send the CAN message from + /// @param[in] destAddress The destination address to send the message to + /// @param[in] parameterGroupNumber The PGN to use when sending the message + /// @param[in] priority The CAN priority of the message being sent + /// @param[in] data A pointer to the data buffer to send from + /// @param[in] size The size of the message to send + /// @returns The constructed frame based on the inputs + CANMessageFrame construct_frame(std::uint32_t portIndex, + std::uint8_t sourceAddress, + std::uint8_t destAddress, + std::uint32_t parameterGroupNumber, + std::uint8_t priority, + const void *data, + std::uint32_t size) const; + + /// @brief Returns a control function based on a CAN address and channel index + /// @param[in] channelIndex The CAN channel index of the CAN message being processed + /// @param[in] address The CAN address associated with a control function + /// @returns A control function matching the address and CAN port passed in + std::shared_ptr get_control_function(std::uint8_t channelIndex, std::uint8_t address) const; + + /// @brief Gets a message from the Rx Queue. + /// @note This will only ever get an 8 byte message. Long messages are handled elsewhere. + /// @returns The can message that was at the front of the buffer + CANMessage get_next_can_message_from_rx_queue(); + + /// @brief Returns the number of messages in the rx queue that need to be processed + /// @returns The number of messages in the rx queue that need to be processed + std::size_t get_number_can_messages_in_rx_queue(); + + /// @brief Informs the network manager that a control function object has been created + /// @param[in] controlFunction The control function that was created + void on_control_function_created(std::shared_ptr controlFunction); + + /// @brief Processes a can message for callbacks added with add_any_control_function_parameter_group_number_callback + /// @param[in] currentMessage The message to process + void process_any_control_function_pgn_callbacks(const CANMessage ¤tMessage); + + /// @brief Checks the control function state callback list to see if we need to call + /// a control function state callback. + /// @param[in] controlFunction The controlFunction whose state has changed + /// @param[in] state The new state of the control function + void process_control_function_state_change_callback(std::shared_ptr controlFunction, ControlFunctionState state); + + /// @brief Processes a can message for callbacks added with add_protocol_parameter_group_number_callback + /// @param[in] currentMessage The message to process + void process_protocol_pgn_callbacks(const CANMessage ¤tMessage); + + /// @brief Matches a CAN message to any matching PGN callback, and calls that callback + /// @param[in] message A pointer to a CAN message to be processed + void process_can_message_for_global_and_partner_callbacks(const CANMessage &message); + + /// @brief Processes a CAN message to see if it's a commanded address message, and + /// if it is, it attempts to set the relevant CF's address to the new value. + /// @note Changing the address will resend the address claim message if + /// the target was an internal control function. + /// @param[in] message The message to process + void process_can_message_for_commanded_address(const CANMessage &message); + + /// @brief Processes the internal receive message queue + void process_rx_messages(); + + /// @brief Checks to see if any control function didn't claim during a round of + /// address claiming and removes it if needed. + void prune_inactive_control_functions(); + + /// @brief Sends a CAN message using raw addresses. Used only by the stack. + /// @param[in] portIndex The CAN channel index to send the message from + /// @param[in] sourceAddress The source address to send the CAN message from + /// @param[in] destAddress The destination address to send the message to + /// @param[in] parameterGroupNumber The PGN to use when sending the message + /// @param[in] priority The CAN priority of the message being sent + /// @param[in] data A pointer to the data buffer to send from + /// @param[in] size The size of the message to send + /// @returns `true` if the message was sent, otherwise `false` + bool send_can_message_raw(std::uint32_t portIndex, + std::uint8_t sourceAddress, + std::uint8_t destAddress, + std::uint32_t parameterGroupNumber, + std::uint8_t priority, + const void *data, + std::uint32_t size) const; + + /// @brief Gets a PGN callback for the global address by index + /// @param[in] index The index of the callback to get + /// @returns A structure containing the global PGN callback data + ParameterGroupNumberCallbackData get_global_parameter_group_number_callback(std::uint32_t index) const; + + static constexpr std::uint32_t BUSLOAD_SAMPLE_WINDOW_MS = 1000; ///< Using a 1s window to average the bus load, otherwise it's very erratic + static constexpr std::uint32_t BUSLOAD_UPDATE_FREQUENCY_MS = 100; ///< Bus load bit accumulation happens over a 100ms window + + CANNetworkConfiguration configuration; ///< The configuration for this network manager + ExtendedTransportProtocolManager extendedTransportProtocol; ///< Static instance of the protocol manager + FastPacketProtocol fastPacketProtocol; ///< Instance of the fast packet protocol + TransportProtocolManager transportProtocol; ///< Static instance of the transport protocol manager + + std::array, CAN_PORT_MAXIMUM> busloadMessageBitsHistory; ///< Stores the approximate number of bits processed on each channel over multiple previous time windows + std::array currentBusloadBitAccumulator; ///< Accumulates the approximate number of bits processed on each channel during the current time window + std::array lastAddressClaimRequestTimestamp_ms; ///< Stores timestamps for when the last request for the address claim PGN was received. Used to prune stale CFs. + + std::array, NULL_CAN_ADDRESS>, CAN_PORT_MAXIMUM> controlFunctionTable; ///< Table to maintain address to NAME mappings + std::list> inactiveControlFunctions; ///< A list of the control function that currently don't have a valid address + std::list> internalControlFunctions; ///< A list of the internal control functions + std::list> partneredControlFunctions; ///< A list of the partnered control functions + + std::list protocolPGNCallbacks; ///< A list of PGN callback registered by CAN protocols + std::list receiveMessageList; ///< A queue of Rx messages to process + std::list controlFunctionStateCallbacks; ///< List of all control function state callbacks + std::vector globalParameterGroupNumberCallbacks; ///< A list of all global PGN callbacks + std::vector anyControlFunctionParameterGroupNumberCallbacks; ///< A list of all global PGN callbacks +#if !defined CAN_STACK_DISABLE_THREADS && !defined ARDUINO + std::mutex receiveMessageMutex; ///< A mutex for receive messages thread safety + std::mutex protocolPGNCallbacksMutex; ///< A mutex for PGN callback thread safety + std::mutex anyControlFunctionCallbacksMutex; ///< Mutex to protect the "any CF" callbacks + std::mutex busloadUpdateMutex; ///< A mutex that protects the busload metrics since we calculate it on our own thread + std::mutex controlFunctionStatusCallbacksMutex; ///< A Mutex that protects access to the control function status callback list +#endif + std::uint32_t busloadUpdateTimestamp_ms = 0; ///< Tracks a time window for determining approximate busload + std::uint32_t updateTimestamp_ms = 0; ///< Keeps track of the last time the CAN stack was update in milliseconds + bool initialized = false; ///< True if the network manager has been initialized by the update function + }; + +} // namespace isobus + +#endif // CAN_NETWORK_MANAGER_HPP diff --git a/src/can_parameter_group_number_request_protocol.cpp b/src/can_parameter_group_number_request_protocol.cpp new file mode 100644 index 0000000..97de49a --- /dev/null +++ b/src/can_parameter_group_number_request_protocol.cpp @@ -0,0 +1,306 @@ +//================================================================================================ +/// @file can_parameter_group_number_request_protocol.cpp +/// +/// @brief A protocol that handles PGN requests +/// @details The purpose of this protocol is to simplify and standardize how PGN requests +/// are made and responded to. +/// @author Adrian Del Grosso +/// +/// @copyright 2022 Adrian Del Grosso +//================================================================================================ +#include "can_parameter_group_number_request_protocol.hpp" +#include "can_general_parameter_group_numbers.hpp" +#include "can_stack_logger.hpp" +#include "to_string.hpp" + +#include +#include + +namespace isobus +{ + + ParameterGroupNumberRequestProtocol::ParameterGroupNumberRequestProtocol(std::shared_ptr internalControlFunction, CANLibBadge) : + myControlFunction(internalControlFunction) + { + assert(nullptr != myControlFunction && "ParameterGroupNumberRequestProtocol::ParameterGroupNumberRequestProtocol() called with nullptr internalControlFunction"); + CANNetworkManager::CANNetwork.add_protocol_parameter_group_number_callback(static_cast(CANLibParameterGroupNumber::ParameterGroupNumberRequest), process_message, this); + CANNetworkManager::CANNetwork.add_protocol_parameter_group_number_callback(static_cast(CANLibParameterGroupNumber::RequestForRepetitionRate), process_message, this); + } + + ParameterGroupNumberRequestProtocol::~ParameterGroupNumberRequestProtocol() + { + CANNetworkManager::CANNetwork.remove_protocol_parameter_group_number_callback(static_cast(CANLibParameterGroupNumber::ParameterGroupNumberRequest), process_message, this); + CANNetworkManager::CANNetwork.remove_protocol_parameter_group_number_callback(static_cast(CANLibParameterGroupNumber::RequestForRepetitionRate), process_message, this); + } + + bool ParameterGroupNumberRequestProtocol::request_parameter_group_number(std::uint32_t pgn, std::shared_ptr source, std::shared_ptr destination) + { + std::array buffer; + + buffer[0] = static_cast(pgn & 0xFF); + buffer[1] = static_cast((pgn >> 8) & 0xFF); + buffer[2] = static_cast((pgn >> 16) & 0xFF); + + return CANNetworkManager::CANNetwork.send_can_message(static_cast(CANLibParameterGroupNumber::ParameterGroupNumberRequest), + buffer.data(), + PGN_REQUEST_LENGTH, + source, + destination); + } + + bool ParameterGroupNumberRequestProtocol::request_repetition_rate(std::uint32_t pgn, std::uint16_t repetitionRate_ms, std::shared_ptr source, std::shared_ptr destination) + { + std::array buffer; + + buffer[0] = static_cast(pgn & 0xFF); + buffer[1] = static_cast((pgn >> 8) & 0xFF); + buffer[2] = static_cast((pgn >> 16) & 0xFF); + buffer[3] = static_cast(repetitionRate_ms & 0xFF); + buffer[4] = static_cast((repetitionRate_ms >> 8) & 0xFF); + buffer[5] = 0xFF; + buffer[6] = 0xFF; + buffer[7] = 0xFF; + + return CANNetworkManager::CANNetwork.send_can_message(static_cast(CANLibParameterGroupNumber::RequestForRepetitionRate), + buffer.data(), + CAN_DATA_LENGTH, + source, + destination); + } + + bool ParameterGroupNumberRequestProtocol::register_pgn_request_callback(std::uint32_t pgn, PGNRequestCallback callback, void *parentPointer) + { + PGNRequestCallbackInfo pgnCallback(callback, pgn, parentPointer); + bool retVal = false; +#if !defined CAN_STACK_DISABLE_THREADS && !defined ARDUINO + const std::lock_guard lock(pgnRequestMutex); +#endif + + if ((nullptr != callback) && (pgnRequestCallbacks.end() == std::find(pgnRequestCallbacks.begin(), pgnRequestCallbacks.end(), pgnCallback))) + { + pgnRequestCallbacks.push_back(pgnCallback); + retVal = true; + } + return retVal; + } + + bool ParameterGroupNumberRequestProtocol::register_request_for_repetition_rate_callback(std::uint32_t pgn, PGNRequestForRepetitionRateCallback callback, void *parentPointer) + { + PGNRequestForRepetitionRateCallbackInfo repetitionRateCallback(callback, pgn, parentPointer); + bool retVal = false; +#if !defined CAN_STACK_DISABLE_THREADS && !defined ARDUINO + const std::lock_guard lock(pgnRequestMutex); +#endif + + if ((nullptr != callback) && (repetitionRateCallbacks.end() == std::find(repetitionRateCallbacks.begin(), repetitionRateCallbacks.end(), repetitionRateCallback))) + { + repetitionRateCallbacks.push_back(repetitionRateCallback); + retVal = true; + } + return retVal; + } + + bool ParameterGroupNumberRequestProtocol::remove_pgn_request_callback(std::uint32_t pgn, PGNRequestCallback callback, void *parentPointer) + { + PGNRequestCallbackInfo repetitionRateCallback(callback, pgn, parentPointer); + bool retVal = false; +#if !defined CAN_STACK_DISABLE_THREADS && !defined ARDUINO + const std::lock_guard lock(pgnRequestMutex); +#endif + + auto callbackLocation = find(pgnRequestCallbacks.begin(), pgnRequestCallbacks.end(), repetitionRateCallback); + + if (pgnRequestCallbacks.end() != callbackLocation) + { + pgnRequestCallbacks.erase(callbackLocation); + retVal = true; + } + return retVal; + } + + bool ParameterGroupNumberRequestProtocol::remove_request_for_repetition_rate_callback(std::uint32_t pgn, PGNRequestForRepetitionRateCallback callback, void *parentPointer) + { + PGNRequestForRepetitionRateCallbackInfo repetitionRateCallback(callback, pgn, parentPointer); + bool retVal = false; +#if !defined CAN_STACK_DISABLE_THREADS && !defined ARDUINO + const std::lock_guard lock(pgnRequestMutex); +#endif + + auto callbackLocation = find(repetitionRateCallbacks.begin(), repetitionRateCallbacks.end(), repetitionRateCallback); + + if (repetitionRateCallbacks.end() != callbackLocation) + { + repetitionRateCallbacks.erase(callbackLocation); + retVal = true; + } + return retVal; + } + + std::size_t ParameterGroupNumberRequestProtocol::get_number_registered_pgn_request_callbacks() const + { + return pgnRequestCallbacks.size(); + } + + std::size_t ParameterGroupNumberRequestProtocol::get_number_registered_request_for_repetition_rate_callbacks() const + { + return repetitionRateCallbacks.size(); + } + + ParameterGroupNumberRequestProtocol::PGNRequestCallbackInfo::PGNRequestCallbackInfo(PGNRequestCallback callback, std::uint32_t parameterGroupNumber, void *parentPointer) : + callbackFunction(callback), + pgn(parameterGroupNumber), + parent(parentPointer) + { + } + + ParameterGroupNumberRequestProtocol::PGNRequestForRepetitionRateCallbackInfo::PGNRequestForRepetitionRateCallbackInfo(PGNRequestForRepetitionRateCallback callback, std::uint32_t parameterGroupNumber, void *parentPointer) : + callbackFunction(callback), + pgn(parameterGroupNumber), + parent(parentPointer) + { + } + + bool ParameterGroupNumberRequestProtocol::PGNRequestCallbackInfo::operator==(const PGNRequestCallbackInfo &obj) const + { + return ((obj.callbackFunction == this->callbackFunction) && (obj.pgn == this->pgn) && (obj.parent == this->parent)); + } + + bool ParameterGroupNumberRequestProtocol::PGNRequestForRepetitionRateCallbackInfo::operator==(const PGNRequestForRepetitionRateCallbackInfo &obj) const + { + return ((obj.callbackFunction == this->callbackFunction) && (obj.pgn == this->pgn) && (obj.parent == this->parent)); + } + + void ParameterGroupNumberRequestProtocol::process_message(const CANMessage &message) + { + if (((nullptr == message.get_destination_control_function()) && + (BROADCAST_CAN_ADDRESS == message.get_identifier().get_destination_address())) || + (message.get_destination_control_function() == myControlFunction)) + { + switch (message.get_identifier().get_parameter_group_number()) + { + case static_cast(CANLibParameterGroupNumber::RequestForRepetitionRate): + { + // Can't send this request to global, and must be 8 bytes. Ignore illegal message formats + if ((CAN_DATA_LENGTH == message.get_data_length()) && (nullptr != message.get_destination_control_function())) + { + std::uint32_t requestedPGN = message.get_uint24_at(0); + std::uint16_t requestedRate = message.get_uint16_at(3); + +#if !defined CAN_STACK_DISABLE_THREADS && !defined ARDUINO + const std::lock_guard lock(pgnRequestMutex); +#endif + for (const auto &repetitionRateCallback : repetitionRateCallbacks) + { + if (((repetitionRateCallback.pgn == requestedPGN) || + (static_cast(isobus::CANLibParameterGroupNumber::Any) == repetitionRateCallback.pgn)) && + (repetitionRateCallback.callbackFunction(requestedPGN, message.get_source_control_function(), requestedRate, repetitionRateCallback.parent))) + { + // If the callback was able to process the PGN request, stop processing more. + break; + } + } + + // We can just ignore requests for repetition rate if we don't support them for this PGN. No need to NACK. + // From isobus.net: + // "CFs are not required to monitor the bus for this message." + // "If another CF cannot or does not want to use the requested repetition rate, which is necessary for systems with fixed timing control loops, it may ignore this message." + } + else + { + CANStackLogger::warn("[PR]: Received a malformed or broadcast request for repetition rate message. The message will not be processed."); + } + } + break; + + case static_cast(CANLibParameterGroupNumber::ParameterGroupNumberRequest): + { + if (message.get_data_length() >= PGN_REQUEST_LENGTH) + { + bool shouldAck = false; + AcknowledgementType ackType = AcknowledgementType::Negative; + bool anyCallbackProcessed = false; + + std::uint32_t requestedPGN = message.get_uint24_at(0); + +#if !defined CAN_STACK_DISABLE_THREADS && !defined ARDUINO + const std::lock_guard lock(pgnRequestMutex); +#endif + for (const auto &pgnRequestCallback : pgnRequestCallbacks) + { + if (((pgnRequestCallback.pgn == requestedPGN) || + (static_cast(isobus::CANLibParameterGroupNumber::Any) == pgnRequestCallback.pgn)) && + (pgnRequestCallback.callbackFunction(requestedPGN, message.get_source_control_function(), shouldAck, ackType, pgnRequestCallback.parent))) + { + // If we're here, the callback was able to process the PGN request. + anyCallbackProcessed = true; + + // Now we need to know if we shoulc ACK it. + // We should not ACK messages that send the actual PGN as a result of requesting it. This behavior is up to + // the application layer to do properly. + if (shouldAck && (nullptr != message.get_destination_control_function())) + { + send_acknowledgement(ackType, + requestedPGN, + message.get_source_control_function()); + } + // If this callback was able to process the PGN request, stop processing more. + break; + } + } + + if ((!anyCallbackProcessed) && (nullptr != message.get_destination_control_function())) + { + send_acknowledgement(AcknowledgementType::Negative, + requestedPGN, + message.get_source_control_function()); + CANStackLogger::warn("[PR]: NACK-ing PGN request for PGN " + isobus::to_string(requestedPGN) + " because no callback could handle it."); + } + } + else + { + CANStackLogger::warn("[PR]: Received a malformed PGN request message. The message will not be processed."); + } + } + break; + + default: + break; + } + } + } + + void ParameterGroupNumberRequestProtocol::process_message(const CANMessage &message, void *parent) + { + if (nullptr != parent) + { + reinterpret_cast(parent)->process_message(message); + } + } + + bool ParameterGroupNumberRequestProtocol::send_acknowledgement(AcknowledgementType type, std::uint32_t parameterGroupNumber, std::shared_ptr destination) const + { + bool retVal = false; + + if (nullptr != destination) + { + std::array buffer; + + buffer[0] = static_cast(type); + buffer[1] = 0xFF; + buffer[2] = 0xFF; + buffer[3] = 0xFF; + buffer[4] = destination->get_address(); + buffer[5] = static_cast(parameterGroupNumber & 0xFF); + buffer[6] = static_cast((parameterGroupNumber >> 8) & 0xFF); + buffer[7] = static_cast((parameterGroupNumber >> 16) & 0xFF); + + retVal = CANNetworkManager::CANNetwork.send_can_message(static_cast(CANLibParameterGroupNumber::Acknowledge), + buffer.data(), + CAN_DATA_LENGTH, + myControlFunction, + nullptr); + } + return retVal; + } + +} // namespace isobus diff --git a/src/can_parameter_group_number_request_protocol.hpp b/src/can_parameter_group_number_request_protocol.hpp new file mode 100644 index 0000000..8eee2a0 --- /dev/null +++ b/src/can_parameter_group_number_request_protocol.hpp @@ -0,0 +1,168 @@ +//================================================================================================ +/// @file can_parameter_group_number_request_protocol.hpp +/// +/// @brief A protocol that handles PGN requests +/// @details The purpose of this protocol is to simplify and standardize how PGN requests +/// are made and responded to. +/// @author Adrian Del Grosso +/// +/// @copyright 2022 Adrian Del Grosso +//================================================================================================ +#ifndef CAN_PARAMETER_GROUP_NUMBER_REQUEST_PROTOCOL_HPP +#define CAN_PARAMETER_GROUP_NUMBER_REQUEST_PROTOCOL_HPP + +#include "can_badge.hpp" +#include "can_control_function.hpp" +#include "can_network_manager.hpp" + +#include + +namespace isobus +{ + //================================================================================================ + /// @class ParameterGroupNumberRequestProtocol + /// + /// @brief A protocol that handles PGN requests + /// @details The purpose of this protocol is to simplify and standardize how PGN requests + /// are made and responded to. It provides a way to easily send a PGN request or a request for + /// repetition rate, as well as methods to receive PGN requests. + //================================================================================================ + class ParameterGroupNumberRequestProtocol + { + public: + /// @brief The constructor for this protocol + /// @param[in] internalControlFunction The internal control function that owns this protocol and will be used to send messages + ParameterGroupNumberRequestProtocol(std::shared_ptr internalControlFunction, CANLibBadge); + + /// @brief The destructor for this protocol + ~ParameterGroupNumberRequestProtocol(); + + /// @brief Remove the copy constructor + ParameterGroupNumberRequestProtocol(const ParameterGroupNumberRequestProtocol &) = delete; + + /// @brief Remove the copy assignment operator + ParameterGroupNumberRequestProtocol &operator=(const ParameterGroupNumberRequestProtocol &) = delete; + + /// @brief Sends a PGN request to the specified control function + /// @param[in] pgn The PGN to request + /// @param[in] source The internal control function to send from + /// @param[in] destination The control function to request `pgn` from + /// @returns `true` if the request was successfully sent + static bool request_parameter_group_number(std::uint32_t pgn, std::shared_ptr source, std::shared_ptr destination); + + /// @brief Sends a PGN request for repetition rate + /// @details Use this if you want the destination CF to send you the specified PGN at some fixed interval + /// @param[in] pgn The PGN to request + /// @param[in] repetitionRate_ms The repetition rate to request in milliseconds + /// @param[in] source The internal control function to send from + /// @param[in] destination The control function to send the request to + /// @returns `true` if the request was sent + static bool request_repetition_rate(std::uint32_t pgn, std::uint16_t repetitionRate_ms, std::shared_ptr source, std::shared_ptr destination); + + /// @brief Registers for a callback on receipt of a PGN request + /// @param[in] pgn The PGN you want to handle in the callback + /// @param[in] callback The callback function to register + /// @param[in] parentPointer Generic context variable, usually the `this` pointer of the class registering the callback + /// @returns true if the callback was registered, false if the callback is nullptr or is already registered for the same PGN + bool register_pgn_request_callback(std::uint32_t pgn, PGNRequestCallback callback, void *parentPointer); + + /// @brief Registers for a callback on receipt of a request for repetition rate + /// @param[in] pgn The PGN you want to handle in the callback + /// @param[in] callback The callback function to register + /// @param[in] parentPointer Generic context variable, usually the `this` pointer of the class registering the callback + /// @returns true if the callback was registered, false if the callback is nullptr or is already registered for the same PGN + bool register_request_for_repetition_rate_callback(std::uint32_t pgn, PGNRequestForRepetitionRateCallback callback, void *parentPointer); + + /// @brief Removes a previously registered PGN request callback + /// @param[in] pgn The PGN associated with the callback + /// @param[in] callback The callback function to remove + /// @param[in] parentPointer Generic context variable, usually the `this` pointer of the class that registered the callback + /// @returns true if the callback was removed, false if no callback matched the parameters + bool remove_pgn_request_callback(std::uint32_t pgn, PGNRequestCallback callback, void *parentPointer); + + /// @brief Removes a callback for repetition rate requests + /// @param[in] pgn The PGN associated with the callback + /// @param[in] callback The callback function to remove + /// @param[in] parentPointer Generic context variable, usually the `this` pointer of the class that registered the callback + /// @returns true if the callback was registered, false if the callback is nullptr or is already registered for the same PGN + bool remove_request_for_repetition_rate_callback(std::uint32_t pgn, PGNRequestForRepetitionRateCallback callback, void *parentPointer); + + /// @brief Returns the number of PGN request callbacks that have been registered with this protocol instance + /// @returns The number of PGN request callbacks that have been registered with this protocol instance + std::size_t get_number_registered_pgn_request_callbacks() const; + + /// @brief Returns the number of PGN request for repetition rate callbacks that have been registered with this protocol instance + /// @returns The number of PGN request for repetition rate callbacks that have been registered with this protocol instance + std::size_t get_number_registered_request_for_repetition_rate_callbacks() const; + + private: + /// @brief A storage class for holding PGN callbacks and their associated PGN + class PGNRequestCallbackInfo + { + public: + /// @brief Constructor for PGNRequestCallbackInfo + /// @param[in] callback A PGNRequestCallback + /// @param[in] parameterGroupNumber The PGN associated with the callback + /// @param[in] parentPointer Pointer to the class that registered the callback, or `nullptr` + PGNRequestCallbackInfo(PGNRequestCallback callback, std::uint32_t parameterGroupNumber, void *parentPointer); + + /// @brief A utility function for determining if the data in the object is equal to another object + /// @details The objects are the same if the pgn and callbackFunction both match + /// @param[in] obj The object to compare against + /// @returns true if the objects have identical data + bool operator==(const PGNRequestCallbackInfo &obj) const; + + PGNRequestCallback callbackFunction; ///< The actual callback + std::uint32_t pgn; ///< The PGN associated with the callback + void *parent; ///< Pointer to the class that registered the callback, or `nullptr` + }; + + /// @brief A storage class for holding PGN callbacks and their associated PGN + class PGNRequestForRepetitionRateCallbackInfo + { + public: + /// @brief Constructor for PGNRequestCallbackInfo + /// @param[in] callback A PGNRequestCallback + /// @param[in] parameterGroupNumber The PGN associated with the callback + /// @param[in] parentPointer Pointer to the class that registered the callback, or `nullptr` + PGNRequestForRepetitionRateCallbackInfo(PGNRequestForRepetitionRateCallback callback, std::uint32_t parameterGroupNumber, void *parentPointer); + + /// @brief A utility function for determining if the data in the object is equal to another object + /// @details The objects are the same if the pgn and callbackFunction both match + /// @param[in] obj The object to compare against + /// @returns true if the objects have identical data + bool operator==(const PGNRequestForRepetitionRateCallbackInfo &obj) const; + + PGNRequestForRepetitionRateCallback callbackFunction; ///< The actual callback + std::uint32_t pgn; ///< The PGN associated with the callback + void *parent; ///< Pointer to the class that registered the callback, or `nullptr` + }; + + static constexpr std::uint8_t PGN_REQUEST_LENGTH = 3; ///< The CAN data length of a PGN request + + /// @brief A generic way for a protocol to process a received message + /// @param[in] message A received CAN message + void process_message(const CANMessage &message); + + /// @brief A generic way for a protocol to process a received message + /// @param[in] message A received CAN message + /// @param[in] parent Provides the context to the actual TP manager object + static void process_message(const CANMessage &message, void *parent); + + /// @brief Sends a message using the acknowledgement PGN + /// @param[in] type The type of acknowledgement to send (Ack, vs Nack, etc) + /// @param[in] parameterGroupNumber The PGN to acknowledge + /// @param[in] destination The destination control function to send the acknowledgement to + /// @returns true if the message was sent, false otherwise + bool send_acknowledgement(AcknowledgementType type, std::uint32_t parameterGroupNumber, std::shared_ptr destination) const; + + std::shared_ptr myControlFunction; ///< The internal control function that this protocol will send from + std::vector pgnRequestCallbacks; ///< A list of all registered PGN callbacks and the PGN associated with each callback + std::vector repetitionRateCallbacks; ///< A list of all registered request for repetition rate callbacks and the PGN associated with the callback +#if !defined CAN_STACK_DISABLE_THREADS && !defined ARDUINO + std::mutex pgnRequestMutex; ///< A mutex to protect the callback lists +#endif + }; +} + +#endif // CAN_PARAMETER_GROUP_NUMBER_REQUEST_PROTOCOL_HPP diff --git a/src/can_partnered_control_function.cpp b/src/can_partnered_control_function.cpp new file mode 100644 index 0000000..4d3d359 --- /dev/null +++ b/src/can_partnered_control_function.cpp @@ -0,0 +1,182 @@ +//================================================================================================ +/// @file can_partnered_control_function.cpp +/// +/// @brief A class that describes a control function on the bus that the stack should communicate +/// with. Use these to describe ECUs you want to send messages to. +/// @author Adrian Del Grosso +/// @author Daan Steenbergen +/// +/// @copyright 2022 Adrian Del Grosso +//================================================================================================ +#include "can_partnered_control_function.hpp" + +#include "can_constants.hpp" +#include "can_network_manager.hpp" + +#include +#include + +namespace isobus +{ + PartneredControlFunction::PartneredControlFunction(std::uint8_t CANPort, const std::vector NAMEFilters, CANLibBadge) : + ControlFunction(NAME(0), NULL_CAN_ADDRESS, CANPort, Type::Partnered), + NAMEFilterList(NAMEFilters) + { +#if !defined CAN_STACK_DISABLE_THREADS && !defined ARDUINO + const std::lock_guard lock(ControlFunction::controlFunctionProcessingMutex); +#endif + } + + std::shared_ptr PartneredControlFunction::create(std::uint8_t CANPort, const std::vector NAMEFilters) + { + // Unfortunately, we can't use `std::make_shared` here because the constructor is meant to be protected + auto controlFunction = std::shared_ptr(new PartneredControlFunction(CANPort, NAMEFilters, {})); + CANNetworkManager::CANNetwork.on_control_function_created(controlFunction, CANLibBadge()); + return controlFunction; + } + + void PartneredControlFunction::add_parameter_group_number_callback(std::uint32_t parameterGroupNumber, CANLibCallback callback, void *parent, std::shared_ptr internalControlFunction) + { + parameterGroupNumberCallbacks.emplace_back(parameterGroupNumber, callback, parent, internalControlFunction); + } + + void PartneredControlFunction::remove_parameter_group_number_callback(std::uint32_t parameterGroupNumber, CANLibCallback callback, void *parent, std::shared_ptr internalControlFunction) + { + ParameterGroupNumberCallbackData tempObject(parameterGroupNumber, callback, parent, internalControlFunction); + auto callbackLocation = std::find(parameterGroupNumberCallbacks.begin(), parameterGroupNumberCallbacks.end(), tempObject); + if (parameterGroupNumberCallbacks.end() != callbackLocation) + { + parameterGroupNumberCallbacks.erase(callbackLocation); + } + } + + std::size_t PartneredControlFunction::get_number_parameter_group_number_callbacks() const + { + return parameterGroupNumberCallbacks.size(); + } + + std::size_t PartneredControlFunction::get_number_name_filters() const + { + return NAMEFilterList.size(); + } + + bool PartneredControlFunction::get_name_filter_parameter(std::size_t index, NAME::NAMEParameters ¶meter, std::uint32_t &filterValue) const + { + bool retVal = false; + + if (index < get_number_name_filters()) + { + parameter = NAMEFilterList[index].get_parameter(); + filterValue = NAMEFilterList[index].get_value(); + retVal = true; + } + return retVal; + } + + std::size_t PartneredControlFunction::get_number_name_filters_with_parameter_type(NAME::NAMEParameters parameter) + { + std::size_t retVal = 0; + + for (std::size_t i = 0; i < NAMEFilterList.size(); i++) + { + if (parameter == NAMEFilterList[i].get_parameter()) + { + retVal++; + } + } + return retVal; + } + + bool PartneredControlFunction::check_matches_name(NAME NAMEToCheck) const + { + std::uint32_t currentFilterValue; + NAME::NAMEParameters currentFilterParameter; + bool retVal = true; + + if (0 == get_number_name_filters()) + { + retVal = false; + } + + for (std::uint32_t i = 0; i < get_number_name_filters(); i++) + { + get_name_filter_parameter(i, currentFilterParameter, currentFilterValue); + + switch (currentFilterParameter) + { + case NAME::NAMEParameters::IdentityNumber: + { + retVal = (currentFilterValue == NAMEToCheck.get_identity_number()); + } + break; + + case NAME::NAMEParameters::ManufacturerCode: + { + retVal = (currentFilterValue == NAMEToCheck.get_manufacturer_code()); + } + break; + + case NAME::NAMEParameters::EcuInstance: + { + retVal = (currentFilterValue == NAMEToCheck.get_ecu_instance()); + } + break; + + case NAME::NAMEParameters::FunctionInstance: + { + retVal = (currentFilterValue == NAMEToCheck.get_function_instance()); + } + break; + + case NAME::NAMEParameters::FunctionCode: + { + retVal = (currentFilterValue == NAMEToCheck.get_function_code()); + } + break; + + case NAME::NAMEParameters::DeviceClass: + { + retVal = (currentFilterValue == NAMEToCheck.get_device_class()); + } + break; + + case NAME::NAMEParameters::DeviceClassInstance: + { + retVal = (currentFilterValue == NAMEToCheck.get_device_class_instance()); + } + break; + + case NAME::NAMEParameters::IndustryGroup: + { + retVal = (currentFilterValue == NAMEToCheck.get_industry_group()); + } + break; + + case NAME::NAMEParameters::ArbitraryAddressCapable: + { + retVal = ((0 != currentFilterValue) == NAMEToCheck.get_arbitrary_address_capable()); + } + break; + + default: + { + retVal = false; + } + break; + } + + if (false == retVal) + { + break; + } + } + return retVal; + } + + ParameterGroupNumberCallbackData &PartneredControlFunction::get_parameter_group_number_callback(std::size_t index) + { + assert(index < get_number_parameter_group_number_callbacks()); + return parameterGroupNumberCallbacks[index]; + } + +} // namespace isobus diff --git a/src/can_partnered_control_function.hpp b/src/can_partnered_control_function.hpp new file mode 100644 index 0000000..536873a --- /dev/null +++ b/src/can_partnered_control_function.hpp @@ -0,0 +1,119 @@ +//================================================================================================ +/// @file can_partnered_control_function.hpp +/// +/// @brief A class that describes a control function on the bus that the stack should communicate +/// with. Use these to describe ECUs you want to send messages to. +/// @author Adrian Del Grosso +/// @author Daan Steenbergen +/// +/// @copyright 2022 Adrian Del Grosso +//================================================================================================ + +#ifndef CAN_PARTNERED_CONTROL_FUNCTION_HPP +#define CAN_PARTNERED_CONTROL_FUNCTION_HPP + +#include "can_NAME_filter.hpp" +#include "can_address_claim_state_machine.hpp" +#include "can_badge.hpp" +#include "can_callbacks.hpp" +#include "can_control_function.hpp" + +#include + +namespace isobus +{ + class CANNetworkManager; + class InternalControlFunction; + + //================================================================================================ + /// @class PartneredControlFunction + /// + /// @brief This represents any device on the bus you want to talk to. + /// @details To communicate with a device on the bus, create one of these objects and tell it + /// via the constructor what the identity of that device is using NAME fields like + /// manufacturer code, function, and device class. The stack will take care of locating the + /// device on the bus that matches that description, and will allow you to talk to it through + /// passing this object to the appropriate send function in the network manager. + //================================================================================================ + class PartneredControlFunction : public ControlFunction + { + public: + /// @brief The factory function to construct a partnered control function + /// @param[in] CANPort The CAN channel associated with this control function definition + /// @param[in] NAMEFilters A list of filters that describe the identity of the CF based on NAME components + static std::shared_ptr create(std::uint8_t CANPort, const std::vector NAMEFilters); + + /// @brief the constructor for a PartneredControlFunction, which is called by the factory function + /// @param[in] CANPort The CAN channel associated with this control function definition + /// @param[in] NAMEFilters A list of filters that describe the identity of the CF based on NAME components + PartneredControlFunction(std::uint8_t CANPort, const std::vector NAMEFilters, CANLibBadge); + + /// @brief Deleted copy constructor for PartneredControlFunction to avoid slicing + PartneredControlFunction(PartneredControlFunction &) = delete; + + /// @brief This is how you get notified that this control function has sent you a destination specific message. + /// @details Add a callback function here to be notified when this device has sent you a message with the specified PGN. + /// You can also get callbacks for any/all PGNs if you pass in `CANLibParameterGroupNumber::Any` as the PGN. + /// Optionally you can use the parent pointer to get context inside your callback as to what C++ object the callback is + /// destined for. Whatever you pass in `parent` will be passed back to you in the callback. In theory, you could use + /// that variable for passing any arbitrary data through the callback also. + /// You can add as many callbacks as you want, and can use the same function for multiple PGNs if you want. + /// @param[in] parameterGroupNumber The PGN you want to use to communicate, or `CANLibParameterGroupNumber::Any` + /// @param[in] callback The function you want to get called when a message is received with parameterGroupNumber from this CF + /// @param[in] parent A generic context variable that helps identify what object the callback was destined for + /// @param[in] internalControlFunction An internal control function to filter based on. If you supply this + /// parameter the callback will only be called when messages from the partner are received with the + /// specified ICF as the destination. + void add_parameter_group_number_callback(std::uint32_t parameterGroupNumber, CANLibCallback callback, void *parent, std::shared_ptr internalControlFunction = nullptr); + + /// @brief Removes a callback matching *exactly* the parameters passed in + /// @param[in] parameterGroupNumber The PGN associated with the callback being removed + /// @param[in] callback The callback function being removed + /// @param[in] parent A generic context variable that helps identify what object the callback was destined for + /// @param[in] internalControlFunction The ICF being used to filter messages against + void remove_parameter_group_number_callback(std::uint32_t parameterGroupNumber, CANLibCallback callback, void *parent, std::shared_ptr internalControlFunction = nullptr); + + /// @brief Returns the number of parameter group number callbacks associated with this control function + /// @returns The number of parameter group number callbacks associated with this control function + std::size_t get_number_parameter_group_number_callbacks() const; + + /// @brief Returns the number of NAME filter objects that describe the identity of this control function + /// @returns The number of NAME filter objects that describe the identity of this control function + std::size_t get_number_name_filters() const; + + /// @brief Returns the number of NAME filters with a specific NAME parameter component, like manufacturer code + /// @param[in] parameter The NAME parameter to check against + /// @returns The number of NAME filters with a specific NAME parameter component + std::size_t get_number_name_filters_with_parameter_type(NAME::NAMEParameters parameter); + + /// @brief Returns a NAME filter by index + /// @param[in] index The index of the filter to get + /// @param[out] parameter The returned parameter type + /// @param[out] filterValue The raw value of the filter associated with the `parameter` + /// @returns true if a filter was returned successfully, false if the index was out of range + bool get_name_filter_parameter(std::size_t index, NAME::NAMEParameters ¶meter, std::uint32_t &filterValue) const; + + /// @brief Checks to see if a NAME matches this CF's NAME filters + /// @param[in] NAMEToCheck The NAME to check against this control function's filters + /// @returns true if this control function matches the NAME that was passed in, false otherwise + bool check_matches_name(NAME NAMEToCheck) const; + + private: + friend class CANNetworkManager; ///< Allows the network manager to use get_parameter_group_number_callback + + /// @brief Make inherited factory function private so that it can't be called + static std::shared_ptr create(NAME, std::uint8_t, std::uint8_t) = delete; + + /// @brief Returns a parameter group number associated with this control function by index + /// @param[in] index The index from which to get the PGN callback data object + /// @returns A reference to the PGN callback data object at the index specified + ParameterGroupNumberCallbackData &get_parameter_group_number_callback(std::size_t index); + + const std::vector NAMEFilterList; ///< A list of NAME parameters that describe this control function's identity + std::vector parameterGroupNumberCallbacks; ///< A list of all parameter group number callbacks associated with this control function + bool initialized = false; ///< A way to track if the network manager has processed this CF against existing CFs + }; + +} // namespace isobus + +#endif // CAN_PARTNERED_CONTROL_FUNCTION diff --git a/src/can_protocol.cpp b/src/can_protocol.cpp new file mode 100644 index 0000000..1392677 --- /dev/null +++ b/src/can_protocol.cpp @@ -0,0 +1,60 @@ +//================================================================================================ +/// @file can_protocol.cpp +/// +/// @brief A base class for all protocol classes. Allows the network manager to update them +/// in a generic, dynamic way. +/// @author Adrian Del Grosso +/// +/// @copyright 2022 Adrian Del Grosso +//================================================================================================ +#include "can_protocol.hpp" + +#include "can_network_manager.hpp" + +#include + +namespace isobus +{ + CANLibProtocol::CANLibProtocol() : + initialized(false) + { + CANNetworkManager::CANNetwork.protocolList.push_back(this); + } + + CANLibProtocol::~CANLibProtocol() + { + auto protocolLocation = find(CANNetworkManager::CANNetwork.protocolList.begin(), CANNetworkManager::CANNetwork.protocolList.end(), this); + + if (CANNetworkManager::CANNetwork.protocolList.end() != protocolLocation) + { + CANNetworkManager::CANNetwork.protocolList.erase(protocolLocation); + } + } + + bool CANLibProtocol::get_is_initialized() const + { + return initialized; + } + + bool CANLibProtocol::get_protocol(std::uint32_t index, CANLibProtocol *&returnedProtocol) + { + returnedProtocol = nullptr; + + if (index < CANNetworkManager::CANNetwork.protocolList.size()) + { + returnedProtocol = CANNetworkManager::CANNetwork.protocolList[index]; + } + return (nullptr != returnedProtocol); + } + + std::uint32_t CANLibProtocol::get_number_protocols() + { + return CANNetworkManager::CANNetwork.protocolList.size(); + } + + void CANLibProtocol::initialize(CANLibBadge) + { + initialized = true; + } + +} // namespace isobus diff --git a/src/can_protocol.hpp b/src/can_protocol.hpp new file mode 100644 index 0000000..066f380 --- /dev/null +++ b/src/can_protocol.hpp @@ -0,0 +1,95 @@ +//================================================================================================ +/// @file can_protocol.hpp +/// +/// @brief A base class for all protocol classes. Allows the network manager to update them +/// in a generic, dynamic way. +/// @author Adrian Del Grosso +/// +/// @copyright 2022 Adrian Del Grosso +//================================================================================================ + +#ifndef CAN_PROTOCOL_HPP +#define CAN_PROTOCOL_HPP + +#include "can_badge.hpp" +#include "can_callbacks.hpp" +#include "can_control_function.hpp" +#include "can_message.hpp" + +#include + +namespace isobus +{ + class CANNetworkManager; + + //================================================================================================ + /// @class CANLibProtocol + /// + /// @brief A base class for a CAN protocol + /// @details CANLibProtocols are objects that manage different statful CAN protocols defined by + /// ISO11783 and/or J1939. They could also be used for abitrary processing inside the CAN stack. + //================================================================================================ + class CANLibProtocol + { + public: + /// @brief The base class constructor for a CANLibProtocol + CANLibProtocol(); + + /// @brief Deleted copy constructor for a CANLibProtocol + CANLibProtocol(CANLibProtocol &) = delete; + + /// @brief The base class destructor for a CANLibProtocol + virtual ~CANLibProtocol(); + + /// @brief Returns whether or not the protocol has been initialized by the network manager + /// @returns true if the protocol has been initialized by the network manager + bool get_is_initialized() const; + + /// @brief Gets a CAN protocol by index from the list of all protocols + /// @param[in] index The index of the protocol to get from the list of protocols + /// @param[out] returnedProtocol The returned protocol + /// @returns true if a protocol was successfully returned, false if index was out of range + static bool get_protocol(std::uint32_t index, CANLibProtocol *&returnedProtocol); + + /// @brief Returns the number of all created protocols + /// @returns The number of all created protocols + static std::uint32_t get_number_protocols(); + + /// @brief A generic way to initialize a protocol + /// @details The network manager will call a protocol's initialize function + /// when it is first updated, if it has yet to be initialized. + virtual void initialize(CANLibBadge); + + /// @brief A generic way for a protocol to process a received message + /// @param[in] message A received CAN message + virtual void process_message(const CANMessage &message) = 0; + + /// @brief The network manager calls this to see if the protocol can accept a non-raw CAN message for processing + /// @param[in] parameterGroupNumber The PGN of the message + /// @param[in] data The data to be sent + /// @param[in] messageLength The length of the data to be sent + /// @param[in] source The source control function + /// @param[in] destination The destination control function + /// @param[in] transmitCompleteCallback A callback for when the protocol completes its work + /// @param[in] parentPointer A generic context object for the tx complete and chunk callbacks + /// @param[in] frameChunkCallback A callback to get some data to send + /// @returns true if the message was accepted by the protocol for processing + virtual bool protocol_transmit_message(std::uint32_t parameterGroupNumber, + const std::uint8_t *data, + std::uint32_t messageLength, + std::shared_ptr source, + std::shared_ptr destination, + TransmitCompleteCallback transmitCompleteCallback, + void *parentPointer, + DataChunkCallback frameChunkCallback) = 0; + + /// @brief This will be called by the network manager on every cyclic update of the stack + virtual void update(CANLibBadge) = 0; + + protected: + bool initialized; ///< Keeps track of if the protocol has been initialized by the network manager + }; + +} // namespace isobus + +#endif // CAN_PROTOCOL_HPP diff --git a/src/can_stack_logger.cpp b/src/can_stack_logger.cpp new file mode 100644 index 0000000..e20617e --- /dev/null +++ b/src/can_stack_logger.cpp @@ -0,0 +1,86 @@ +//================================================================================================ +/// @file can_stack_logger.cpp +/// +/// @brief A class that acts as a logging sink. The intent is that someone could make their own +/// derived class of logger and inject it into the CAN stack to get helpful debug logging. +/// @author Adrian Del Grosso +/// +/// @copyright 2022 Adrian Del Grosso +//================================================================================================ +#include "can_stack_logger.hpp" + +#include + +namespace isobus +{ + CANStackLogger *CANStackLogger::logger = nullptr; + CANStackLogger::LoggingLevel CANStackLogger::currentLogLevel = LoggingLevel::Info; +#if !defined CAN_STACK_DISABLE_THREADS && !defined ARDUINO + std::mutex CANStackLogger::loggerMutex; +#endif + + void CANStackLogger::CAN_stack_log(LoggingLevel level, const std::string &logText) + { +#if !defined CAN_STACK_DISABLE_THREADS && !defined ARDUINO + const std::lock_guard lock(loggerMutex); +#endif + CANStackLogger *canStackLogger = nullptr; + + if ((get_can_stack_logger(canStackLogger)) && + (level >= get_log_level())) + { + canStackLogger->sink_CAN_stack_log(level, logText); + } + } + + void CANStackLogger::debug(const std::string &logText) + { + CAN_stack_log(LoggingLevel::Debug, logText); + } + + void CANStackLogger::info(const std::string &logText) + { + CAN_stack_log(LoggingLevel::Info, logText); + } + + void CANStackLogger::warn(const std::string &logText) + { + CAN_stack_log(LoggingLevel::Warning, logText); + } + + void CANStackLogger::error(const std::string &logText) + { + CAN_stack_log(LoggingLevel::Error, logText); + } + + void CANStackLogger::critical(const std::string &logText) + { + CAN_stack_log(LoggingLevel::Critical, logText); + } + + void CANStackLogger::set_can_stack_logger_sink(CANStackLogger *logSink) + { + logger = logSink; + } + + CANStackLogger::LoggingLevel CANStackLogger::get_log_level() + { + return currentLogLevel; + } + + void CANStackLogger::set_log_level(LoggingLevel newLogLevel) + { + currentLogLevel = newLogLevel; + } + + void CANStackLogger::sink_CAN_stack_log(LoggingLevel, const std::string &) + { + // Override this function to use the log sink + } + + bool CANStackLogger::get_can_stack_logger(CANStackLogger *&canStackLogger) + { + canStackLogger = logger; + return (nullptr != canStackLogger); + } +} // namespace isobus diff --git a/src/can_stack_logger.hpp b/src/can_stack_logger.hpp new file mode 100644 index 0000000..addf572 --- /dev/null +++ b/src/can_stack_logger.hpp @@ -0,0 +1,175 @@ +//================================================================================================ +/// @file can_stack_logger.hpp +/// +/// @brief A class that acts as a logging sink. The intent is that someone could make their own +/// derived class of logger and inject it into the CAN stack to get helpful debug logging. +/// @author Adrian Del Grosso +/// +/// @copyright 2022 Adrian Del Grosso +//================================================================================================ +#ifndef CAN_STACK_LOGGER_HPP +#define CAN_STACK_LOGGER_HPP + +#include +#include + +#if !defined CAN_STACK_DISABLE_THREADS && !defined ARDUINO +#include +#endif + +namespace isobus +{ + //================================================================================================ + /// @class CANStackLogger + /// + /// @brief A base class for a CAN logger, used to get diagnostic info from the CAN stack + /// @details The CAN stack prints helpful text that may inform you of issues in either the stack + /// or your application. You can override a function in this class to begin consuming this + /// logging text. + //================================================================================================ + class CANStackLogger + { + public: + /// @brief Enumerates the various log message severities + enum class LoggingLevel + { + Debug = 0, ///< Verbose information + Info, ///< General status info messages and messages about how things are working under normal conditions + Warning, ///< Warnings indicate issues that do not stop normal operation, but should be noted for troubleshooting + Error, ///< Errors are issues that interrupt normal operation + Critical ///< Critical issues are fundamental problems that must be solved for the stack to work properly + }; + + /// @brief The constructor for a CANStackLogger + CANStackLogger() = default; + + /// @brief The destructor for a CANStackLogger + ~CANStackLogger() = default; + + /// @brief Gets called from the CAN stack to log information. Wraps sink_CAN_stack_log. + /// @param[in] level The log level for this text + /// @param[in] logText The text to be logged + static void CAN_stack_log(LoggingLevel level, const std::string &logText); + + /// @brief Gets called from the CAN stack to log information. Wraps sink_CAN_stack_log. + /// @param[in] level The log level for this text + /// @param[in] format A format string of text to log, similar to printf + /// @param[in] args A list of printf style arguments to use with the format string when logging + template + static void CAN_stack_log(LoggingLevel level, const std::string &format, Args... args) + { + int size_s = std::snprintf(nullptr, 0, format.c_str(), args...) + 1; // Extra space for '\0' + if (size_s > 0) + { + auto size = static_cast(size_s); + std::unique_ptr buf(new char[size]); + std::snprintf(buf.get(), size, format.c_str(), args...); + CAN_stack_log(level, std::string(buf.get(), buf.get() + size - 1)); // We don't want the '\0' inside + } + else if (size_s < 0) + { + CAN_stack_log(level, format); // If snprintf had some error, at least print the format string + } + } + + /// @brief Logs a string to the log sink with `Debug` severity. Wraps sink_CAN_stack_log. + /// @param[in] logText The text to be logged at `Debug` severity + static void debug(const std::string &logText); + + /// @brief Logs a printf formatted string to the log sink with `Debug` severity. Wraps sink_CAN_stack_log. + /// @param[in] format The format string, similar to printf + /// @param[in] args The variadic arguments to format, similar to printf + template + static void debug(const std::string &format, Args... args) + { + CAN_stack_log(LoggingLevel::Debug, format, args...); + } + + /// @brief Logs a string to the log sink with `Info` severity. Wraps sink_CAN_stack_log. + /// @param[in] logText The text to be logged at `Info` severity + static void info(const std::string &logText); + + /// @brief Logs a printf formatted string to the log sink with `Info` severity. Wraps sink_CAN_stack_log. + /// @param[in] format The format string, similar to printf + /// @param[in] args The variadic arguments to format, similar to printf + template + static void info(const std::string &format, Args... args) + { + CAN_stack_log(LoggingLevel::Info, format, args...); + } + + /// @brief Logs a string to the log sink with `Warning` severity. Wraps sink_CAN_stack_log. + /// @param[in] logText The text to be logged at `Warning` severity + static void warn(const std::string &logText); + + /// @brief Logs a printf formatted string to the log sink with `Warning` severity. Wraps sink_CAN_stack_log. + /// @param[in] format The format string, similar to printf + /// @param[in] args The variadic arguments to format, similar to printf + template + static void warn(const std::string &format, Args... args) + { + CAN_stack_log(LoggingLevel::Warning, format, args...); + } + + /// @brief Logs a string to the log sink with `Error` severity. Wraps sink_CAN_stack_log. + /// @param[in] logText The text to be logged at `Error` severity + static void error(const std::string &logText); + + /// @brief Logs a printf formatted string to the log sink with `Error` severity. Wraps sink_CAN_stack_log. + /// @param[in] format The format string, similar to printf + /// @param[in] args The variadic arguments to format, similar to printf + template + static void error(const std::string &format, Args... args) + { + CAN_stack_log(LoggingLevel::Error, format, args...); + } + + /// @brief Logs a string to the log sink with `Critical` severity. Wraps sink_CAN_stack_log. + /// @param[in] logText The text to be logged at `Critical` severity + static void critical(const std::string &logText); + + /// @brief Logs a printf formatted string to the log sink with `Critical` severity. Wraps sink_CAN_stack_log. + /// @param[in] format The format string, similar to printf + /// @param[in] args The variadic arguments to format, similar to printf + template + static void critical(const std::string &format, Args... args) + { + CAN_stack_log(LoggingLevel::Critical, format, args...); + } + + /// @brief Assigns a derived logger class to be used as the log sink + /// @param[in] logSink A pointer to a derived CANStackLogger class + static void set_can_stack_logger_sink(CANStackLogger *logSink); + + /// @brief Returns the current logging level + /// @details Log statements below the current level will be dropped + /// and not passed to the log sink + /// @returns The current log level + static LoggingLevel get_log_level(); + + /// @brief Sets the current logging level + /// @details Log statements below the new level will be dropped + /// and not passed to the log sink + /// @param[in] newLogLevel The new logging level + static void set_log_level(LoggingLevel newLogLevel); + + /// @brief Override this to make a log sink for your application + /// @param[in] level The severity level of the log text + /// @param[in] logText The information being logged + virtual void sink_CAN_stack_log(LoggingLevel level, const std::string &logText); + + private: + /// @brief Provides a pointer to the static instance of the logger, and returns if the pointer is valid + /// @param[out] canStackLogger The static logger instance + /// @returns true if the logger is not `nullptr` or false if it is `nullptr` + static bool get_can_stack_logger(CANStackLogger *&canStackLogger); + + static CANStackLogger *logger; ///< A static pointer to an instance of a logger + static LoggingLevel currentLogLevel; ///< The current log level. Logs for levels below the current one will be dropped. +#if !defined CAN_STACK_DISABLE_THREADS && !defined ARDUINO + static std::mutex loggerMutex; ///< A mutex that protects the logger so it can be used from multiple threads +#endif + }; +} // namespace isobus + +#endif // CAN_STACK_LOGGER_HPP diff --git a/src/can_transport_protocol.cpp b/src/can_transport_protocol.cpp new file mode 100644 index 0000000..86fcf0d --- /dev/null +++ b/src/can_transport_protocol.cpp @@ -0,0 +1,870 @@ +//================================================================================================ +/// @file can_transport_protocol.cpp +/// +/// @brief A protocol that handles the ISO11783/J1939 transport protocol. +/// It handles both the broadcast version (BAM) and and the connection mode version. +/// @author Adrian Del Grosso +/// +/// @copyright 2022 Adrian Del Grosso +//================================================================================================ + +#include "can_transport_protocol.hpp" + +#include "can_general_parameter_group_numbers.hpp" +#include "can_network_configuration.hpp" +#include "can_network_manager.hpp" +#include "can_stack_logger.hpp" +#include "system_timing.hpp" +#include "to_string.hpp" + +#include + +namespace isobus +{ + TransportProtocolManager::TransportProtocolSession::TransportProtocolSession(Direction sessionDirection, std::uint8_t canPortIndex) : + sessionMessage(canPortIndex), + sessionDirection(sessionDirection) + { + } + + bool TransportProtocolManager::TransportProtocolSession::operator==(const TransportProtocolSession &obj) + { + return ((sessionMessage.get_source_control_function() == obj.sessionMessage.get_source_control_function()) && + (sessionMessage.get_destination_control_function() == obj.sessionMessage.get_destination_control_function()) && + (sessionMessage.get_identifier().get_parameter_group_number() == obj.sessionMessage.get_identifier().get_parameter_group_number())); + } + + std::uint32_t TransportProtocolManager::TransportProtocolSession::get_message_data_length() const + { + if (nullptr != frameChunkCallback) + { + return frameChunkCallbackMessageLength; + } + return sessionMessage.get_data_length(); + } + + TransportProtocolManager::~TransportProtocolManager() + { + // No need to clean up, as this object is a member of the network manager + // so its callbacks will be cleared at destruction time + } + + void TransportProtocolManager::initialize(CANLibBadge) + { + if (!initialized) + { + initialized = true; + CANNetworkManager::CANNetwork.add_protocol_parameter_group_number_callback(static_cast(CANLibParameterGroupNumber::TransportProtocolCommand), process_message, this); + CANNetworkManager::CANNetwork.add_protocol_parameter_group_number_callback(static_cast(CANLibParameterGroupNumber::TransportProtocolData), process_message, this); + } + } + + void TransportProtocolManager::process_message(const CANMessage &message) + { + if ((nullptr != message.get_source_control_function()) && + ((nullptr == message.get_destination_control_function()) || + (nullptr != CANNetworkManager::CANNetwork.get_internal_control_function(message.get_destination_control_function())))) + { + switch (message.get_identifier().get_parameter_group_number()) + { + case static_cast(CANLibParameterGroupNumber::TransportProtocolCommand): + { + TransportProtocolSession *session; + const auto &data = message.get_data(); + const std::uint32_t pgn = (static_cast(data[5]) | (static_cast(data[6]) << 8) | (static_cast(data[7]) << 16)); + switch (data[0]) + { + case BROADCAST_ANNOUNCE_MESSAGE_MULTIPLEXOR: + { + if (CAN_DATA_LENGTH == message.get_data_length()) + { + if ((nullptr == message.get_destination_control_function()) && + (activeSessions.size() < CANNetworkManager::CANNetwork.get_configuration().get_max_number_transport_protocol_sessions()) && + (!get_session(session, message.get_source_control_function(), message.get_destination_control_function(), pgn))) + { + TransportProtocolSession *newSession = new TransportProtocolSession(TransportProtocolSession::Direction::Receive, message.get_can_port_index()); + CANIdentifier tempIdentifierData(CANIdentifier::Type::Extended, pgn, CANIdentifier::CANPriority::PriorityLowest7, BROADCAST_CAN_ADDRESS, message.get_source_control_function()->get_address()); + newSession->sessionMessage.set_data_size(static_cast(data[1]) | static_cast(data[2] << 8)); + newSession->sessionMessage.set_source_control_function(message.get_source_control_function()); + newSession->sessionMessage.set_destination_control_function(nullptr); + newSession->packetCount = data[3]; + newSession->sessionMessage.set_identifier(tempIdentifierData); + newSession->state = StateMachineState::RxDataSession; + newSession->timestamp_ms = SystemTiming::get_timestamp_ms(); + activeSessions.push_back(newSession); + CANStackLogger::CAN_stack_log(CANStackLogger::LoggingLevel::Debug, + "[TP]: New Rx BAM Session. Source: " + + isobus::to_string(static_cast(newSession->sessionMessage.get_source_control_function()->get_address()))); + } + else + { + // Don't send an abort, they're probably expecting a CTS so it'll timeout + // Or maybe if we already had a session they sent a second BAM? Also bad + CANStackLogger::CAN_stack_log(CANStackLogger::LoggingLevel::Error, "[TP]: Can't Create an Rx BAM session"); + } + } + else + { + CANStackLogger::CAN_stack_log(CANStackLogger::LoggingLevel::Error, "[TP]: Bad BAM Message Length"); + } + } + break; + + case REQUEST_TO_SEND_MULTIPLEXOR: + { + if (CAN_DATA_LENGTH == message.get_data_length()) + { + if ((nullptr != message.get_destination_control_function()) && + (activeSessions.size() < CANNetworkManager::CANNetwork.get_configuration().get_max_number_transport_protocol_sessions()) && + (!get_session(session, message.get_source_control_function(), message.get_destination_control_function(), pgn))) + { + TransportProtocolSession *newSession = new TransportProtocolSession(TransportProtocolSession::Direction::Receive, message.get_can_port_index()); + CANIdentifier tempIdentifierData(CANIdentifier::Type::Extended, pgn, CANIdentifier::CANPriority::PriorityLowest7, message.get_destination_control_function()->get_address(), message.get_source_control_function()->get_address()); + newSession->sessionMessage.set_data_size(static_cast(data[1]) | static_cast(data[2] << 8)); + newSession->sessionMessage.set_source_control_function(message.get_source_control_function()); + newSession->sessionMessage.set_destination_control_function(message.get_destination_control_function()); + newSession->packetCount = data[3]; + newSession->clearToSendPacketMax = data[4]; + newSession->sessionMessage.set_identifier(tempIdentifierData); + newSession->state = StateMachineState::ClearToSend; + newSession->timestamp_ms = SystemTiming::get_timestamp_ms(); + activeSessions.push_back(newSession); + } + else if ((get_session(session, message.get_source_control_function(), message.get_destination_control_function(), pgn)) && + (nullptr != message.get_destination_control_function()) && + (ControlFunction::Type::Internal == message.get_destination_control_function()->get_type())) + { + abort_session(pgn, ConnectionAbortReason::AlreadyInCMSession, std::static_pointer_cast(message.get_destination_control_function()), message.get_source_control_function()); + CANStackLogger::CAN_stack_log(CANStackLogger::LoggingLevel::Error, "[TP]: Sent abort, RTS when already in CM session"); + } + else if ((activeSessions.size() >= CANNetworkManager::CANNetwork.get_configuration().get_max_number_transport_protocol_sessions()) && + (nullptr != message.get_destination_control_function()) && + (ControlFunction::Type::Internal == message.get_destination_control_function()->get_type())) + { + abort_session(pgn, ConnectionAbortReason::SystemResourcesNeeded, std::static_pointer_cast(message.get_destination_control_function()), message.get_source_control_function()); + CANStackLogger::CAN_stack_log(CANStackLogger::LoggingLevel::Error, "[TP]: Sent abort, No Sessions Available"); + } + } + else + { + // Bad RTS message length. Can't really abort? Not sure what the PGN is if length < 8 + CANStackLogger::CAN_stack_log(CANStackLogger::LoggingLevel::Error, "[TP]: Received Bad Message Length for an RTS"); + } + } + break; + + case CLEAR_TO_SEND_MULTIPLEXOR: + { + // Can't happen when doing a BAM session, make sure the session type is correct + if ((CAN_DATA_LENGTH == message.get_data_length()) && + (nullptr != message.get_destination_control_function()) && + (nullptr != message.get_source_control_function())) + { + const std::uint8_t packetsToBeSent = data[1]; + + if (get_session(session, message.get_destination_control_function(), message.get_source_control_function(), pgn)) + { + if (StateMachineState::WaitForClearToSend == session->state) + { + session->packetCount = packetsToBeSent; + session->timestamp_ms = SystemTiming::get_timestamp_ms(); + // If 0 was sent as the packet number, they want us to wait. + // Just sit here in this state until we get a non-zero packet count + if (0 != packetsToBeSent) + { + session->lastPacketNumber = 0; + session->state = StateMachineState::TxDataSession; + } + } + else + { + // The session exists, but we're probably already in the TxDataSession state. Need to abort + // In the case of Rx'ing a CTS, we're the source in the session + abort_session(pgn, ConnectionAbortReason::ClearToSendReceivedWhileTransferInProgress, std::static_pointer_cast(message.get_destination_control_function()), message.get_source_control_function()); + CANStackLogger::CAN_stack_log(CANStackLogger::LoggingLevel::Error, "[TP]: Sent abort, CTS while in data session, PGN: " + isobus::to_string(pgn)); + } + } + else + { + // We got a CTS but no session exists. Aborting clears up the situation faster than waiting for them to timeout + // In the case of Rx'ing a CTS, we're the source in the session + abort_session(pgn, ConnectionAbortReason::AnyOtherError, std::static_pointer_cast(message.get_destination_control_function()), message.get_source_control_function()); + CANStackLogger::CAN_stack_log(CANStackLogger::LoggingLevel::Error, "[TP]: Sent abort, CTS With no matching session, PGN: " + isobus::to_string(pgn)); + } + } + else + { + CANStackLogger::CAN_stack_log(CANStackLogger::LoggingLevel::Warning, "[TP]: Received an Invalid CTS"); + } + } + break; + + case END_OF_MESSAGE_ACKNOWLEDGE_MULTIPLEXOR: + { + // Can't happen when doing a BAM session, make sure the session type is correct + if ((CAN_DATA_LENGTH == message.get_data_length()) && + (nullptr != message.get_destination_control_function()) && + (nullptr != message.get_source_control_function())) + { + if (get_session(session, message.get_destination_control_function(), message.get_source_control_function(), pgn)) + { + if (StateMachineState::WaitForEndOfMessageAcknowledge == session->state) + { + // We completed our Tx session! + session->state = StateMachineState::None; + close_session(session, true); + } + else + { + abort_session(pgn, ConnectionAbortReason::AnyOtherError, std::static_pointer_cast(message.get_destination_control_function()), message.get_source_control_function()); + close_session(session, false); + CANStackLogger::CAN_stack_log(CANStackLogger::LoggingLevel::Error, "[TP]: Sent abort, received EOM in wrong session state, PGN: " + isobus::to_string(pgn)); + } + } + else + { + abort_session(pgn, ConnectionAbortReason::AnyOtherError, std::static_pointer_cast(message.get_destination_control_function()), message.get_source_control_function()); + CANStackLogger::CAN_stack_log(CANStackLogger::LoggingLevel::Error, "[TP]: Sent abort, received EOM without matching session, PGN: " + isobus::to_string(pgn)); + } + } + else + { + CANStackLogger::CAN_stack_log(CANStackLogger::LoggingLevel::Warning, "[TP]: Bad EOM received"); + } + } + break; + + case CONNECTION_ABORT_MULTIPLEXOR: + { + if (get_session(session, message.get_destination_control_function(), message.get_source_control_function(), pgn)) + { + CANStackLogger::CAN_stack_log(CANStackLogger::LoggingLevel::Error, "[TP]: Received an abort for an session with PGN: " + isobus::to_string(pgn)); + close_session(session, false); + } + else + { + CANStackLogger::CAN_stack_log(CANStackLogger::LoggingLevel::Warning, "[TP]: Received an abort with no matching session with PGN: " + isobus::to_string(pgn)); + } + } + break; + + default: + { + CANStackLogger::CAN_stack_log(CANStackLogger::LoggingLevel::Warning, "[TP]: Bad Mux in Transport Protocol Command"); + } + break; + } + } + break; + + case static_cast(CANLibParameterGroupNumber::TransportProtocolData): + { + TransportProtocolSession *tempSession = nullptr; + + if ((CAN_DATA_LENGTH == message.get_data_length()) && + (get_session(tempSession, message.get_source_control_function(), message.get_destination_control_function())) && + (StateMachineState::RxDataSession == tempSession->state)) + { + // Check for valid sequence number + if (message.get_data()[SEQUENCE_NUMBER_DATA_INDEX] == (tempSession->lastPacketNumber + 1)) + { + for (std::uint8_t i = SEQUENCE_NUMBER_DATA_INDEX; (i < PROTOCOL_BYTES_PER_FRAME) && (static_cast((PROTOCOL_BYTES_PER_FRAME * tempSession->lastPacketNumber) + i) < tempSession->get_message_data_length()); i++) + { + std::uint16_t currentDataIndex = (PROTOCOL_BYTES_PER_FRAME * tempSession->lastPacketNumber) + i; + tempSession->sessionMessage.set_data(message.get_data()[1 + SEQUENCE_NUMBER_DATA_INDEX + i], currentDataIndex); + } + tempSession->lastPacketNumber++; + tempSession->processedPacketsThisSession++; + if ((tempSession->lastPacketNumber * PROTOCOL_BYTES_PER_FRAME) >= tempSession->get_message_data_length()) + { + // Send EOM Ack for CM sessions only + if (nullptr != tempSession->sessionMessage.get_destination_control_function()) + { + send_end_of_session_acknowledgement(tempSession); + } + CANNetworkManager::CANNetwork.process_any_control_function_pgn_callbacks(tempSession->sessionMessage); + CANNetworkManager::CANNetwork.protocol_message_callback(tempSession->sessionMessage); + close_session(tempSession, true); + } + tempSession->timestamp_ms = SystemTiming::get_timestamp_ms(); + } + else if (message.get_data()[SEQUENCE_NUMBER_DATA_INDEX] == (tempSession->lastPacketNumber)) + { + // Sequence number is duplicate of the last one + CANStackLogger::CAN_stack_log(CANStackLogger::LoggingLevel::Error, "[TP]: Aborting session due to duplciate sequence number"); + abort_session(tempSession, ConnectionAbortReason::DuplicateSequenceNumber); + close_session(tempSession, false); + } + else + { + CANStackLogger::CAN_stack_log(CANStackLogger::LoggingLevel::Error, "[TP]: Aborting session due to bad sequence number"); + abort_session(tempSession, ConnectionAbortReason::BadSequenceNumber); + close_session(tempSession, false); + } + } + else + { + CANStackLogger::CAN_stack_log(CANStackLogger::LoggingLevel::Warning, "[TP]: Invalid BAM TP Data Received"); + if (get_session(tempSession, message.get_source_control_function(), message.get_destination_control_function())) + { + // If a session matches and ther was an error, get rid of the session + close_session(tempSession, false); + } + } + } + break; + + default: + { + // This is not a runtime error, should never happen. + // Bad PGN passed to protocol. Check PGN registrations. + CANStackLogger::CAN_stack_log(CANStackLogger::LoggingLevel::Warning, "[TP]: Received an unexpected PGN"); + } + break; + } + } + } + + void TransportProtocolManager::process_message(const CANMessage &message, void *parent) + { + if (nullptr != parent) + { + reinterpret_cast(parent)->process_message(message); + } + } + + bool TransportProtocolManager::protocol_transmit_message(std::uint32_t parameterGroupNumber, + const std::uint8_t *dataBuffer, + std::uint32_t messageLength, + std::shared_ptr source, + std::shared_ptr destination, + TransmitCompleteCallback sessionCompleteCallback, + void *parentPointer, + DataChunkCallback frameChunkCallback) + { + TransportProtocolSession *session; + bool retVal = false; + + if ((messageLength <= MAX_PROTOCOL_DATA_LENGTH) && + (messageLength > CAN_DATA_LENGTH) && + ((nullptr != dataBuffer) || + (nullptr != frameChunkCallback)) && + (nullptr != source) && + (true == source->get_address_valid()) && + ((nullptr == destination) || + (destination->get_address_valid())) && + (!get_session(session, source, destination, parameterGroupNumber)) && + ((nullptr != destination) || + ((nullptr == destination) && + (!get_session(session, source, destination))))) + { + TransportProtocolSession *newSession = new TransportProtocolSession(TransportProtocolSession::Direction::Transmit, + source->get_can_port()); + std::uint8_t destinationAddress; + + if (dataBuffer != nullptr) + { + newSession->sessionMessage.set_data(dataBuffer, messageLength); + } + else + { + newSession->frameChunkCallback = frameChunkCallback; + newSession->frameChunkCallbackMessageLength = messageLength; + } + newSession->sessionMessage.set_source_control_function(source); + newSession->sessionMessage.set_destination_control_function(destination); + newSession->packetCount = (messageLength / PROTOCOL_BYTES_PER_FRAME); + newSession->lastPacketNumber = 0; + newSession->processedPacketsThisSession = 0; + newSession->sessionCompleteCallback = sessionCompleteCallback; + newSession->parent = parentPointer; + if (0 != (messageLength % PROTOCOL_BYTES_PER_FRAME)) + { + newSession->packetCount++; + } + + if (nullptr != destination) + { + // CM Message + destinationAddress = destination->get_address(); + set_state(newSession, StateMachineState::RequestToSend); + } + else + { + // BAM message + destinationAddress = BROADCAST_CAN_ADDRESS; + set_state(newSession, StateMachineState::BroadcastAnnounce); + } + + CANIdentifier messageVirtualID(CANIdentifier::Type::Extended, + parameterGroupNumber, + CANIdentifier::CANPriority::PriorityDefault6, + destinationAddress, + source->get_address()); + + newSession->sessionMessage.set_identifier(messageVirtualID); + + activeSessions.push_back(newSession); + retVal = true; + } + return retVal; + } + + void TransportProtocolManager::update(CANLibBadge) + { + for (auto i : activeSessions) + { + update_state_machine(i); + } + } + + bool TransportProtocolManager::abort_session(TransportProtocolSession *session, ConnectionAbortReason reason) + { + bool retVal = false; + + if (nullptr != session) + { + std::shared_ptr myControlFunction; + std::shared_ptr partnerControlFunction; + std::array data; + std::uint32_t pgn = session->sessionMessage.get_identifier().get_parameter_group_number(); + + if (TransportProtocolSession::Direction::Transmit == session->sessionDirection) + { + myControlFunction = CANNetworkManager::CANNetwork.get_internal_control_function(session->sessionMessage.get_source_control_function()); + partnerControlFunction = session->sessionMessage.get_destination_control_function(); + } + else + { + myControlFunction = CANNetworkManager::CANNetwork.get_internal_control_function(session->sessionMessage.get_destination_control_function()); + partnerControlFunction = session->sessionMessage.get_source_control_function(); + } + + data[0] = CONNECTION_ABORT_MULTIPLEXOR; + data[1] = static_cast(reason); + data[2] = 0xFF; + data[3] = 0xFF; + data[4] = 0xFF; + data[5] = static_cast(pgn & 0xFF); + data[6] = static_cast((pgn >> 8) & 0xFF); + data[7] = static_cast((pgn >> 16) & 0xFF); + retVal = CANNetworkManager::CANNetwork.send_can_message(static_cast(CANLibParameterGroupNumber::TransportProtocolCommand), + data.data(), + 8, + myControlFunction, + partnerControlFunction, + CANIdentifier::CANPriority::PriorityDefault6); + } + return retVal; + } + + bool TransportProtocolManager::abort_session(std::uint32_t parameterGroupNumber, ConnectionAbortReason reason, std::shared_ptr source, std::shared_ptr destination) + { + std::array data; + + data[0] = CONNECTION_ABORT_MULTIPLEXOR; + data[1] = static_cast(reason); + data[2] = 0xFF; + data[3] = 0xFF; + data[4] = 0xFF; + data[5] = static_cast(parameterGroupNumber & 0xFF); + data[6] = static_cast((parameterGroupNumber >> 8) & 0xFF); + data[7] = static_cast((parameterGroupNumber >> 16) & 0xFF); + return CANNetworkManager::CANNetwork.send_can_message(static_cast(CANLibParameterGroupNumber::TransportProtocolCommand), + data.data(), + 8, + source, + destination, + CANIdentifier::CANPriority::PriorityDefault6); + } + + void TransportProtocolManager::close_session(TransportProtocolSession *session, bool successfull) + { + if (nullptr != session) + { + process_session_complete_callback(session, successfull); + auto sessionLocation = std::find(activeSessions.begin(), activeSessions.end(), session); + if (activeSessions.end() != sessionLocation) + { + activeSessions.erase(sessionLocation); + delete session; + CANStackLogger::CAN_stack_log(CANStackLogger::LoggingLevel::Debug, "[TP]: Session Closed"); + } + } + } + + void TransportProtocolManager::process_session_complete_callback(TransportProtocolSession *session, bool success) + { + if ((nullptr != session) && + (nullptr != session->sessionCompleteCallback) && + (nullptr != session->sessionMessage.get_source_control_function()) && + (ControlFunction::Type::Internal == session->sessionMessage.get_source_control_function()->get_type())) + { + session->sessionCompleteCallback(session->sessionMessage.get_identifier().get_parameter_group_number(), + session->get_message_data_length(), + std::static_pointer_cast(session->sessionMessage.get_source_control_function()), + session->sessionMessage.get_destination_control_function(), + success, + session->parent); + } + } + + bool TransportProtocolManager::send_broadcast_announce_message(TransportProtocolSession *session) const + { + bool retVal = false; + + if (nullptr != session) + { + const std::uint8_t dataBuffer[CAN_DATA_LENGTH] = { BROADCAST_ANNOUNCE_MESSAGE_MULTIPLEXOR, + static_cast(session->get_message_data_length() & 0xFF), + static_cast((session->get_message_data_length() >> 8) & 0xFF), + session->packetCount, + 0xFF, + static_cast(session->sessionMessage.get_identifier().get_parameter_group_number() & 0xFF), + static_cast((session->sessionMessage.get_identifier().get_parameter_group_number() >> 8) & 0xFF), + static_cast((session->sessionMessage.get_identifier().get_parameter_group_number() >> 16) & 0xFF) }; + retVal = CANNetworkManager::CANNetwork.send_can_message(static_cast(CANLibParameterGroupNumber::TransportProtocolCommand), + dataBuffer, + CAN_DATA_LENGTH, + std::static_pointer_cast(session->sessionMessage.get_source_control_function()), + nullptr, + CANIdentifier::CANPriority::PriorityDefault6); + } + return retVal; + } + + bool TransportProtocolManager::send_clear_to_send(TransportProtocolSession *session) const + { + bool retVal = false; + + if (nullptr != session) + { + std::uint8_t packetsRemaining = (session->packetCount - session->processedPacketsThisSession); + std::uint8_t packetsThisSegment; + + if (session->clearToSendPacketMax < packetsRemaining) + { + packetsThisSegment = session->clearToSendPacketMax; + } + else + { + packetsThisSegment = packetsRemaining; + } + + const std::uint8_t dataBuffer[CAN_DATA_LENGTH] = { CLEAR_TO_SEND_MULTIPLEXOR, + packetsThisSegment, + static_cast(session->processedPacketsThisSession + 1), + 0xFF, + 0xFF, + static_cast(session->sessionMessage.get_identifier().get_parameter_group_number() & 0xFF), + static_cast((session->sessionMessage.get_identifier().get_parameter_group_number() >> 8) & 0xFF), + static_cast((session->sessionMessage.get_identifier().get_parameter_group_number() >> 16) & 0xFF) }; + retVal = CANNetworkManager::CANNetwork.send_can_message(static_cast(CANLibParameterGroupNumber::TransportProtocolCommand), + dataBuffer, + CAN_DATA_LENGTH, + std::static_pointer_cast(session->sessionMessage.get_destination_control_function()), + session->sessionMessage.get_source_control_function(), + CANIdentifier::CANPriority::PriorityDefault6); + } + return retVal; + } + + bool TransportProtocolManager::send_request_to_send(TransportProtocolSession *session) const + { + bool retVal = false; + + if (nullptr != session) + { + const std::uint8_t dataBuffer[CAN_DATA_LENGTH] = { REQUEST_TO_SEND_MULTIPLEXOR, + static_cast(session->get_message_data_length() & 0xFF), + static_cast((session->get_message_data_length() >> 8) & 0xFF), + session->packetCount, + 0xFF, + static_cast(session->sessionMessage.get_identifier().get_parameter_group_number() & 0xFF), + static_cast((session->sessionMessage.get_identifier().get_parameter_group_number() >> 8) & 0xFF), + static_cast((session->sessionMessage.get_identifier().get_parameter_group_number() >> 16) & 0xFF) }; + retVal = CANNetworkManager::CANNetwork.send_can_message(static_cast(CANLibParameterGroupNumber::TransportProtocolCommand), + dataBuffer, + CAN_DATA_LENGTH, + std::static_pointer_cast(session->sessionMessage.get_source_control_function()), + session->sessionMessage.get_destination_control_function(), + CANIdentifier::CANPriority::PriorityDefault6); + } + return retVal; + } + + bool TransportProtocolManager::send_end_of_session_acknowledgement(TransportProtocolSession *session) const + { + bool retVal = false; + + if (nullptr != session) + { + std::uint8_t dataBuffer[CAN_DATA_LENGTH] = { 0 }; + std::uint32_t messageLength = session->get_message_data_length(); + std::uint32_t pgn = session->sessionMessage.get_identifier().get_parameter_group_number(); + + dataBuffer[0] = END_OF_MESSAGE_ACKNOWLEDGE_MULTIPLEXOR; + dataBuffer[1] = static_cast(messageLength); + dataBuffer[2] = static_cast(messageLength >> 8); + dataBuffer[3] = (static_cast((messageLength - 1) / 7) + 1); + dataBuffer[4] = 0xFF; + dataBuffer[5] = static_cast(pgn); + dataBuffer[6] = static_cast(pgn >> 8); + dataBuffer[7] = static_cast(pgn >> 16); + + // This message only needs to be sent if we're the recipient. Sanity check the destination is us + if (ControlFunction::Type::Internal == session->sessionMessage.get_destination_control_function()->get_type()) + { + retVal = CANNetworkManager::CANNetwork.send_can_message(static_cast(CANLibParameterGroupNumber::TransportProtocolCommand), + dataBuffer, + CAN_DATA_LENGTH, + std::static_pointer_cast(session->sessionMessage.get_destination_control_function()), + session->sessionMessage.get_source_control_function(), + CANIdentifier::CANPriority::PriorityDefault6); + } + } + else + { + CANStackLogger::CAN_stack_log(CANStackLogger::LoggingLevel::Warning, "[TP]: Attempted to send EOM to null session"); + } + return retVal; + } + + void TransportProtocolManager::set_state(TransportProtocolSession *session, StateMachineState value) + { + if (nullptr != session) + { + session->timestamp_ms = SystemTiming::get_timestamp_ms(); + session->state = value; + } + } + + bool TransportProtocolManager::get_session(TransportProtocolSession *&session, std::shared_ptr source, std::shared_ptr destination) + { + session = nullptr; + + for (auto i : activeSessions) + { + if ((i->sessionMessage.get_source_control_function() == source) && + (i->sessionMessage.get_destination_control_function() == destination)) + { + session = i; + break; + } + } + return (nullptr != session); + } + + bool TransportProtocolManager::get_session(TransportProtocolSession *&session, std::shared_ptr source, std::shared_ptr destination, std::uint32_t parameterGroupNumber) + { + bool retVal = false; + session = nullptr; + + if ((get_session(session, source, destination)) && + (session->sessionMessage.get_identifier().get_parameter_group_number() == parameterGroupNumber)) + { + retVal = true; + } + return retVal; + } + + void TransportProtocolManager::update_state_machine(TransportProtocolSession *session) + { + if (nullptr != session) + { + switch (session->state) + { + case StateMachineState::None: + { + } + break; + + case StateMachineState::ClearToSend: + { + if (send_clear_to_send(session)) + { + set_state(session, StateMachineState::RxDataSession); + } + } + break; + + case StateMachineState::WaitForClearToSend: + case StateMachineState::WaitForEndOfMessageAcknowledge: + { + if (SystemTiming::time_expired_ms(session->timestamp_ms, T2_T3_TIMEOUT_MS)) + { + CANStackLogger::CAN_stack_log(CANStackLogger::LoggingLevel::Error, "[TP]: Timeout"); + abort_session(session, ConnectionAbortReason::Timeout); + close_session(session, false); + } + } + break; + + case StateMachineState::RequestToSend: + { + if (send_request_to_send(session)) + { + set_state(session, StateMachineState::WaitForClearToSend); + } + } + break; + + case StateMachineState::BroadcastAnnounce: + { + if (send_broadcast_announce_message(session)) + { + set_state(session, StateMachineState::TxDataSession); + } + } + break; + + case StateMachineState::TxDataSession: + { + bool sessionStillValid = true; + + if ((nullptr != session->sessionMessage.get_destination_control_function()) || (SystemTiming::time_expired_ms(session->timestamp_ms, CANNetworkManager::CANNetwork.get_configuration().get_minimum_time_between_transport_protocol_bam_frames()))) + { + std::uint8_t dataBuffer[CAN_DATA_LENGTH]; + std::uint32_t framesSentThisUpdate = 0; + + // Try and send packets + for (std::uint8_t i = session->lastPacketNumber; i < session->packetCount; i++) + { + dataBuffer[0] = (session->processedPacketsThisSession + 1); + + if (nullptr != session->frameChunkCallback) + { + // Use the callback to get this frame's data + std::uint8_t callbackBuffer[7] = { + 0xFF, + 0xFF, + 0xFF, + 0xFF, + 0xFF, + 0xFF, + 0xFF + }; + std::uint16_t numberBytesLeft = (session->get_message_data_length() - (PROTOCOL_BYTES_PER_FRAME * session->processedPacketsThisSession)); + + if (numberBytesLeft > PROTOCOL_BYTES_PER_FRAME) + { + numberBytesLeft = PROTOCOL_BYTES_PER_FRAME; + } + + bool callbackSuccessful = session->frameChunkCallback(dataBuffer[0], (PROTOCOL_BYTES_PER_FRAME * session->processedPacketsThisSession), numberBytesLeft, callbackBuffer, session->parent); + + if (callbackSuccessful) + { + for (std::uint8_t j = 0; j < PROTOCOL_BYTES_PER_FRAME; j++) + { + dataBuffer[1 + j] = callbackBuffer[j]; + } + } + else + { + abort_session(session, ConnectionAbortReason::AnyOtherError); + close_session(session, false); + sessionStillValid = false; + break; + } + } + else + { + // Use the data buffer to get the data for this frame + for (std::uint8_t j = 0; j < PROTOCOL_BYTES_PER_FRAME; j++) + { + std::uint32_t index = (j + (PROTOCOL_BYTES_PER_FRAME * session->processedPacketsThisSession)); + if (index < session->get_message_data_length()) + { + dataBuffer[1 + j] = session->sessionMessage.get_data()[j + (PROTOCOL_BYTES_PER_FRAME * session->processedPacketsThisSession)]; + } + else + { + dataBuffer[1 + j] = 0xFF; + } + } + } + + if (CANNetworkManager::CANNetwork.send_can_message(static_cast(CANLibParameterGroupNumber::TransportProtocolData), + dataBuffer, + CAN_DATA_LENGTH, + std::static_pointer_cast(session->sessionMessage.get_source_control_function()), + session->sessionMessage.get_destination_control_function(), + CANIdentifier::CANPriority::PriorityLowest7)) + { + framesSentThisUpdate++; + session->lastPacketNumber++; + session->processedPacketsThisSession++; + session->timestamp_ms = SystemTiming::get_timestamp_ms(); + + if (nullptr == session->sessionMessage.get_destination_control_function()) + { + // Need to wait for the frame delay time before continuing BAM session + break; + } + else if (framesSentThisUpdate >= CANNetworkManager::CANNetwork.get_configuration().get_max_number_of_network_manager_protocol_frames_per_update()) + { + break; // Throttle the session + } + } + else + { + // Process more next time protocol is updated + break; + } + } + } + + if (sessionStillValid) + { + if ((session->lastPacketNumber == (session->packetCount)) && + (session->get_message_data_length() <= (PROTOCOL_BYTES_PER_FRAME * session->processedPacketsThisSession))) + { + if (nullptr == session->sessionMessage.get_destination_control_function()) + { + // BAM is complete + close_session(session, true); + } + else + { + set_state(session, StateMachineState::WaitForEndOfMessageAcknowledge); + session->timestamp_ms = SystemTiming::get_timestamp_ms(); + } + } + else if (session->lastPacketNumber == session->packetCount) + { + set_state(session, StateMachineState::WaitForClearToSend); + session->timestamp_ms = SystemTiming::get_timestamp_ms(); + } + } + } + break; + + case StateMachineState::RxDataSession: + { + if (nullptr == session->sessionMessage.get_destination_control_function()) + { + // BAM Timeout check + if (SystemTiming::time_expired_ms(session->timestamp_ms, T1_TIMEOUT_MS)) + { + CANStackLogger::CAN_stack_log(CANStackLogger::LoggingLevel::Error, "[TP]: BAM Rx Timeout"); + close_session(session, false); + } + } + else + { + // CM TP Timeout check + if (SystemTiming::time_expired_ms(session->timestamp_ms, MESSAGE_TR_TIMEOUT_MS)) + { + CANStackLogger::CAN_stack_log(CANStackLogger::LoggingLevel::Error, "[TP]: CM Rx Timeout"); + abort_session(session, ConnectionAbortReason::Timeout); + close_session(session, false); + } + } + } + break; + } + } + } + +} diff --git a/src/can_transport_protocol.hpp b/src/can_transport_protocol.hpp new file mode 100644 index 0000000..fd6c929 --- /dev/null +++ b/src/can_transport_protocol.hpp @@ -0,0 +1,237 @@ +//================================================================================================ +/// @file can_transport_protocol.hpp +/// +/// @brief A protocol that handles the ISO11783/J1939 transport protocol. +/// It handles both the broadcast version (BAM) and and the connection mode version. +/// @author Adrian Del Grosso +/// +/// @copyright 2022 Adrian Del Grosso +//================================================================================================ + +#ifndef CAN_TRANSPORT_PROTOCOL_HPP +#define CAN_TRANSPORT_PROTOCOL_HPP + +#include "can_badge.hpp" +#include "can_control_function.hpp" +#include "can_protocol.hpp" + +namespace isobus +{ + //================================================================================================ + /// @class TransportProtocolManager + /// + /// @brief A class that handles the ISO11783/J1939 transport protocol. + /// @details This class handles transmission and reception of CAN messages up to 1785 bytes. + /// Both broadcast and connection mode are supported. Simply call send_can_message on the + /// network manager with an appropriate data length, and the protocol will be automatically + /// selected to be used. As a note, use of BAM is discouraged, as it has profound + /// packet timing implications for your application, and is limited to only 1 active session at a time. + /// That session could be busy if you are using DM1 or any other BAM protocol, causing intermittent + /// transmit failures from this class. This is not a bug, rather a limitation of the protocol + /// definition. + //================================================================================================ + class TransportProtocolManager : public CANLibProtocol + { + public: + /// @brief The states that a TP session could be in. Used for the internal state machine. + enum class StateMachineState + { + None, ///< Protocol session is not in progress + ClearToSend, ///< We are sending clear to send message + RxDataSession, ///< Rx data session is in progress + RequestToSend, ///< We are sending the request to send message + WaitForClearToSend, ///< We are waiting for a clear to send message + BroadcastAnnounce, ///< We are sending the broadcast announce message (BAM) + TxDataSession, ///< A Tx data session is in progress + WaitForEndOfMessageAcknowledge ///< We are waiting for an end of message acknowledgement + }; + + //================================================================================================ + /// @class TransportProtocolSession + /// + /// @brief A storage object to keep track of session information internally + //================================================================================================ + class TransportProtocolSession + { + public: + /// @brief Enumerates the possible session directions, Rx or Tx + enum class Direction + { + Transmit, ///< We are transmitting a message + Receive ///< We are receiving a message + }; + + /// @brief A useful way to compare sesson objects to each other for equality + bool operator==(const TransportProtocolSession &obj); + + /// @brief Get the total number of bytes that will be sent or received in this session + /// @return The length of the message in number of bytes + std::uint32_t get_message_data_length() const; + + private: + friend class TransportProtocolManager; ///< Allows the TP manager full access + + /// @brief The constructor for a TP session + /// @param[in] sessionDirection Tx or Rx + /// @param[in] canPortIndex The CAN channel index for the session + TransportProtocolSession(Direction sessionDirection, std::uint8_t canPortIndex); + + /// @brief The destructor for a TP session + ~TransportProtocolSession() = default; + + StateMachineState state = StateMachineState::None; ///< The state machine state for this session + CANMessage sessionMessage; ///< A CAN message is used in the session to represent and store data like PGN + TransmitCompleteCallback sessionCompleteCallback = nullptr; ///< A callback that is to be called when the session is completed + DataChunkCallback frameChunkCallback = nullptr; ///< A callback that might be used to get chunks of data to send + std::uint32_t frameChunkCallbackMessageLength = 0; ///< The length of the message that is being sent in chunks + void *parent = nullptr; ///< A generic context variable that helps identify what object callbacks are destined for. Can be nullptr + std::uint32_t timestamp_ms = 0; ///< A timestamp used to track session timeouts + std::uint16_t lastPacketNumber = 0; ///< The last processed sequence number for this set of packets + std::uint8_t packetCount = 0; ///< The total number of packets to receive or send in this session + std::uint8_t processedPacketsThisSession = 0; ///< The total processed packet count for the whole session so far + std::uint8_t clearToSendPacketMax = 0; ///< The max packets that can be sent per CTS as indicated by the RTS message + const Direction sessionDirection; ///< Represents Tx or Rx session + }; + + /// @brief A list of all defined abort reasons in ISO11783 + enum class ConnectionAbortReason : std::uint8_t + { + Reserved = 0, ///< Reserved, not to be used, but should be tolerated + AlreadyInCMSession = 1, ///< We are already in a connection mode session and can't support another + SystemResourcesNeeded = 2, ///< Session must be aborted because the system needs resources + Timeout = 3, ///< General timeout + ClearToSendReceivedWhileTransferInProgress = 4, ///< A CTS was received while already processing the last CTS + MaximumRetransmitRequestLimitReached = 5, ///< Maximum retries for the data has been reached + UnexpectedDataTransferPacketReceived = 6, ///< A data packet was received outside the proper state + BadSequenceNumber = 7, ///< Incorrect sequence number was received and cannot be recovered + DuplicateSequenceNumber = 8, ///< Re-received a sequence number we've already processed + TotalMessageSizeTooBig = 9, ///< TP Can't support a message this large (>1785 bytes) + AnyOtherError = 250 ///< Any other error not enumerated above, 0xFE + }; + + static constexpr std::uint32_t REQUEST_TO_SEND_MULTIPLEXOR = 0x10; ///< TP.CM_RTS Multiplexor + static constexpr std::uint32_t CLEAR_TO_SEND_MULTIPLEXOR = 0x11; ///< TP.CM_CTS Multiplexor + static constexpr std::uint32_t END_OF_MESSAGE_ACKNOWLEDGE_MULTIPLEXOR = 0x13; ///< TP.CM_EOM_ACK Multiplexor + static constexpr std::uint32_t BROADCAST_ANNOUNCE_MESSAGE_MULTIPLEXOR = 0x20; ///< TP.BAM Multiplexor + static constexpr std::uint32_t CONNECTION_ABORT_MULTIPLEXOR = 0xFF; ///< Abort multiplexor + static constexpr std::uint32_t MAX_PROTOCOL_DATA_LENGTH = 1785; ///< The max number of bytes that this protocol can transfer + static constexpr std::uint32_t T1_TIMEOUT_MS = 750; ///< The t1 timeout as defined by the standard + static constexpr std::uint32_t T2_T3_TIMEOUT_MS = 1250; ///< The t2/t3 timeouts as defined by the standard + static constexpr std::uint32_t T4_TIMEOUT_MS = 1050; ///< The t4 timeout as defined by the standard + static constexpr std::uint8_t SEQUENCE_NUMBER_DATA_INDEX = 0; ///< The index of the sequence number in a frame + static constexpr std::uint8_t MESSAGE_TR_TIMEOUT_MS = 200; ///< The Tr Timeout as defined by the standard + static constexpr std::uint8_t PROTOCOL_BYTES_PER_FRAME = 7; ///< The number of payload bytes per frame minus overhead of sequence number + + /// @brief The constructor for the TransportProtocolManager + TransportProtocolManager() = default; + + /// @brief The destructor for the TransportProtocolManager + ~TransportProtocolManager() final; + + /// @brief The protocol's initializer function + void initialize(CANLibBadge) override; + + /// @brief A generic way for a protocol to process a received message + /// @param[in] message A received CAN message + void process_message(const CANMessage &message) override; + + /// @brief A generic way for a protocol to process a received message + /// @param[in] message A received CAN message + /// @param[in] parent Provides the context to the actual TP manager object + static void process_message(const CANMessage &message, void *parent); + + /// @brief The network manager calls this to see if the protocol can accept a long CAN message for processing + /// @param[in] parameterGroupNumber The PGN of the message + /// @param[in] data The data to be sent + /// @param[in] messageLength The length of the data to be sent + /// @param[in] source The source control function + /// @param[in] destination The destination control function + /// @param[in] transmitCompleteCallback A callback for when the protocol completes its work + /// @param[in] parentPointer A generic context object for the tx complete and chunk callbacks + /// @param[in] frameChunkCallback A callback to get some data to send + /// @returns true if the message was accepted by the protocol for processing + bool protocol_transmit_message(std::uint32_t parameterGroupNumber, + const std::uint8_t *data, + std::uint32_t messageLength, + std::shared_ptr source, + std::shared_ptr destination, + TransmitCompleteCallback transmitCompleteCallback, + void *parentPointer, + DataChunkCallback frameChunkCallback) override; + + /// @brief Updates the protocol cyclically + void update(CANLibBadge) override; + + private: + /// @brief Aborts the session with the specified abort reason. Sends a CAN message. + /// @param[in] session The session to abort + /// @param[in] reason The reason we're aborting the session + /// @returns true if the abort was send OK, false if not sent + bool abort_session(TransportProtocolSession *session, ConnectionAbortReason reason); + + /// @brief Aborts Tp with no corresponding session with the specified abort reason. Sends a CAN message. + /// @param[in] parameterGroupNumber The PGN of the TP "session" we're aborting + /// @param[in] reason The reason we're aborting the session + /// @param[in] source The source control function from which we'll send the abort + /// @param[in] destination The destination control function to which we'll send the abort + /// @returns true if the abort was send OK, false if not sent + bool abort_session(std::uint32_t parameterGroupNumber, ConnectionAbortReason reason, std::shared_ptr source, std::shared_ptr destination); + + /// @brief Gracefully closes a session to prepare for a new session + /// @param[in] session The session to close + /// @param[in] successfull Denotes if the session was successful + void close_session(TransportProtocolSession *session, bool successfull); + + /// @brief Processes end of session callbacks + /// @param[in] session The session we've just completed + /// @param[in] success Denotes if the session was successful + void process_session_complete_callback(TransportProtocolSession *session, bool success); + + /// @brief Sends the "broadcast announce" message + /// @param[in] session The session for which we're sending the BAM + /// @returns true if the BAM was sent, false if sending was not successful + bool send_broadcast_announce_message(TransportProtocolSession *session) const; + + /// @brief Sends the "clear to send" message + /// @param[in] session The session for which we're sending the CTS + /// @returns true if the CTS was sent, false if sending was not successful + bool send_clear_to_send(TransportProtocolSession *session) const; + + /// @brief Sends the "request to send" message as part of initiating a transmit + /// @param[in] session The session for which we're sending the RTS + /// @returns true if the RTS was sent, false if sending was not successful + bool send_request_to_send(TransportProtocolSession *session) const; + + /// @brief Sends the "end of message acknowledgement" message for the provided session + /// @param[in] session The session for which we're sending the EOM ACK + /// @returns true if the EOM was sent, false if sending was not successful + bool send_end_of_session_acknowledgement(TransportProtocolSession *session) const; + + /// @brief Sets the state machine state of the TP session + /// @param[in] session The session to update + /// @param[in] value The state to update the session to + void set_state(TransportProtocolSession *session, StateMachineState value); + + /// @brief Gets a TP session from the passed in source and destination combination + /// @param[in] source The source control function for the session + /// @param[in] destination The destination control function for the session + /// @param[out] session The found session, or nullptr if no session matched the supplied parameters + bool get_session(TransportProtocolSession *&session, std::shared_ptr source, std::shared_ptr destination); + + /// @brief Gets a TP session from the passed in source and destination and PGN combination + /// @param[in] source The source control function for the session + /// @param[in] destination The destination control function for the session + /// @param[in] parameterGroupNumber The PGN of the session + /// @param[out] session The found session, or nullptr if no session matched the supplied parameters + bool get_session(TransportProtocolSession *&session, std::shared_ptr source, std::shared_ptr destination, std::uint32_t parameterGroupNumber); + + /// @brief Updates the state machine of a Tp session + /// @param[in] session The session to update + void update_state_machine(TransportProtocolSession *session); + + std::vector activeSessions; ///< A list of all active TP sessions + }; + +} // namespace isobus + +#endif // CAN_TRANSPORT_PROTOCOL_HPP diff --git a/src/circular_buffer.hpp b/src/circular_buffer.hpp new file mode 100644 index 0000000..a29faac --- /dev/null +++ b/src/circular_buffer.hpp @@ -0,0 +1,781 @@ +/* + MIT License + + Copyright (c) 2018 Antonio Alexander Brewer (tonton81) - https://github.com/tonton81 + + Contributors: + Tim - https://github.com/Defragster + Mike - https://github.com/mjs513 + + Designed and tested for PJRC Teensy 3.2, 3.5, and 3.6 boards. + May or may not work on other microcontrollers, support for them will not be provided. + Use at your own risk. + + Forum link : https://forum.pjrc.com/threads/50395-Circular_Buffer + + Permission is hereby granted, free of charge, to any person obtaining a copy + of this software and associated documentation files (the "Software"), to deal + in the Software without restriction, including without limitation the rights + to use, copy, modify, merge, publish, distribute, sublicense, and / or sell + copies of the Software, and to permit persons to whom the Software is + furnished to do so, subject to the following conditions: + + The above copyright notice and this permission notice shall be included in all + copies or substantial portions of the Software. + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + SOFTWARE. +*/ + +#ifndef CIRCULAR_BUFFER_H +#define CIRCULAR_BUFFER_H +#include + +template +class Circular_Buffer +{ +public: + void push_back(T value) + { + return write(value); + } + void push_front(T value); + T pop_front() + { + return read(); + } + T pop_back(); + void write(T value); + void push_back(const T *buffer, uint16_t length) + { + write(buffer, length); + } + void write(const T *buffer, uint16_t length); + void push_front(const T *buffer, uint16_t length); + T peek(uint16_t pos = 0); + T peekBytes(T *buffer, uint16_t length); + T read(); + T pop_front(T *buffer, uint16_t length) + { + return readBytes(buffer, length); + } + T peek_front(T *buffer, uint16_t length, uint32_t entry = 0); + T read(T *buffer, uint16_t length) + { + return readBytes(buffer, length); + } + T readBytes(T *buffer, uint16_t length); + void flush() + { + clear(); + } + void clear() + { + head = tail = _available = 0; + } + void print(const char *p); + void println(const char *p); + uint16_t size() + { + return _available; + } + uint16_t available() + { + return _available; + } + uint16_t capacity() + { + return _size; + } + uint16_t length_back() + { + return (((int)_cabuf[((head + size() - 1) & (_size - 1))][0] << 8 * sizeof(T)) | (int)_cabuf[((head + size() - 1) & (_size - 1))][1]); + } + uint16_t length_front() + { + return (((int)_cabuf[((head) & (_size - 1))][0] << 8 * sizeof(T)) | (int)_cabuf[((head) & (_size - 1))][1]); + } + T list(); + T variance(); + T deviation(); + T average(); + bool remove(uint16_t pos); + T median(bool override = 0); + void sort_ascending(); + void sort_descending(); + T sum(); + T min(); + T max(); + T mean() + { + return average(); + } + uint16_t max_size() + { + return multi; + } + T pop_back(T *buffer, uint16_t length); + T *peek_front() + { + return front(); + } + T *peek_back() + { + return back(); + } + T *front() + { + return _cabuf[((head) & (_size - 1))] + 2; + } + T *back() + { + return _cabuf[((tail - 1) & (_size - 1))] + 2; + } + bool replace(T *buffer, uint16_t length, int pos1, int pos2, int pos3, int pos4 = -1, int pos5 = -1); + bool isEqual(const T *buffer); + bool find(T *buffer, uint16_t length, int pos1, int pos2, int pos3, int pos4 = -1, int pos5 = -1); + bool findRemove(T *buffer, uint16_t length, int pos1, int pos2, int pos3, int pos4 = -1, int pos5 = -1); + +protected: +private: + volatile uint16_t head = 0; + volatile uint16_t tail = 0; + volatile uint16_t _available = 0; + + T _cbuf[_size]; + T _cabuf[_size][multi + 2]; +}; + +template +bool Circular_Buffer::remove(uint16_t pos) +{ + if (multi) + { + if (pos >= _size) + return 0; + + int32_t find_area = -1; + + for (uint16_t i = 0; i < _size; i++) + { + if (((head + i) & (_size - 1)) == pos) + { + find_area = i; + break; + } + } + if (find_area == -1) + return 0; + + while (((head + find_area) & (_size - 1)) != ((head) & (_size - 1))) + { + memmove(_cabuf[((head + find_area) & (_size - 1))], _cabuf[((head + find_area - 1) & (_size - 1))], (2 + multi) * sizeof(T)); + find_area--; + } + head = ((head + 1) & (2 * _size - 1)); + _available--; + return 1; + } + return 0; +} + +template +bool Circular_Buffer::findRemove(T *buffer, uint16_t length, int pos1, int pos2, int pos3, int pos4, int pos5) +{ + uint8_t input_count = 3; + int32_t found = -1; + if (pos4 != -1) + input_count = 4; + if (pos5 != -1) + input_count = 5; + for (uint16_t j = 0; j < _available; j++) + { + switch (input_count) + { + case 3: + { + if (_cabuf[((head + j) & (_size - 1))][pos1 + 2] == buffer[pos1] && _cabuf[((head + j) & (_size - 1))][pos2 + 2] == buffer[pos2] && + _cabuf[((head + j) & (_size - 1))][pos3 + 2] == buffer[pos3]) + { + found = j; + break; + } + } + case 4: + { + if (_cabuf[((head + j) & (_size - 1))][pos1 + 2] == buffer[pos1] && _cabuf[((head + j) & (_size - 1))][pos2 + 2] == buffer[pos2] && + _cabuf[((head + j) & (_size - 1))][pos3 + 2] == buffer[pos3] && _cabuf[((head + j) & (_size - 1))][pos4 + 2] == buffer[pos4]) + { + found = j; + break; + } + } + case 5: + { + if (_cabuf[((head + j) & (_size - 1))][pos1 + 2] == buffer[pos1] && _cabuf[((head + j) & (_size - 1))][pos2 + 2] == buffer[pos2] && + _cabuf[((head + j) & (_size - 1))][pos3 + 2] == buffer[pos3] && _cabuf[((head + j) & (_size - 1))][pos4 + 2] == buffer[pos4] && + _cabuf[((head + j) & (_size - 1))][pos5 + 2] == buffer[pos5]) + { + found = j; + break; + } + } + } + if (found >= 0) + { + remove((head + found) & (_size - 1)); + return 1; + } + } + return 0; +} + +template +bool Circular_Buffer::find(T *buffer, uint16_t length, int pos1, int pos2, int pos3, int pos4, int pos5) +{ + uint8_t input_count = 3; + bool found = 0; + if (pos4 != -1) + input_count = 4; + if (pos5 != -1) + input_count = 5; + for (uint16_t j = 0; j < _available; j++) + { + switch (input_count) + { + case 3: + { + if (_cabuf[((head + j) & (_size - 1))][pos1 + 2] == buffer[pos1] && _cabuf[((head + j) & (_size - 1))][pos2 + 2] == buffer[pos2] && + _cabuf[((head + j) & (_size - 1))][pos3 + 2] == buffer[pos3]) + { + found = 1; + break; + } + } + case 4: + { + if (_cabuf[((head + j) & (_size - 1))][pos1 + 2] == buffer[pos1] && _cabuf[((head + j) & (_size - 1))][pos2 + 2] == buffer[pos2] && + _cabuf[((head + j) & (_size - 1))][pos3 + 2] == buffer[pos3] && _cabuf[((head + j) & (_size - 1))][pos4 + 2] == buffer[pos4]) + { + found = 1; + break; + } + } + case 5: + { + if (_cabuf[((head + j) & (_size - 1))][pos1 + 2] == buffer[pos1] && _cabuf[((head + j) & (_size - 1))][pos2 + 2] == buffer[pos2] && + _cabuf[((head + j) & (_size - 1))][pos3 + 2] == buffer[pos3] && _cabuf[((head + j) & (_size - 1))][pos4 + 2] == buffer[pos4] && + _cabuf[((head + j) & (_size - 1))][pos5 + 2] == buffer[pos5]) + { + found = 1; + break; + } + } + } + if (found) + { + memmove(buffer, _cabuf[((head + j) & (_size - 1))] + 2, length * sizeof(T)); + break; + } + } + return found; +} + +template +bool Circular_Buffer::replace(T *buffer, uint16_t length, int pos1, int pos2, int pos3, int pos4, int pos5) +{ + uint8_t input_count = 3; + bool found = 0; + if (pos4 != -1) + input_count = 4; + if (pos5 != -1) + input_count = 5; + for (uint16_t j = 0; j < _available; j++) + { + switch (input_count) + { + case 3: + { + if (_cabuf[((head + j) & (_size - 1))][pos1 + 2] == buffer[pos1] && _cabuf[((head + j) & (_size - 1))][pos2 + 2] == buffer[pos2] && + _cabuf[((head + j) & (_size - 1))][pos3 + 2] == buffer[pos3]) + { + found = 1; + break; + } + } + case 4: + { + if (_cabuf[((head + j) & (_size - 1))][pos1 + 2] == buffer[pos1] && _cabuf[((head + j) & (_size - 1))][pos2 + 2] == buffer[pos2] && + _cabuf[((head + j) & (_size - 1))][pos3 + 2] == buffer[pos3] && _cabuf[((head + j) & (_size - 1))][pos4 + 2] == buffer[pos4]) + { + found = 1; + break; + } + } + case 5: + { + if (_cabuf[((head + j) & (_size - 1))][pos1 + 2] == buffer[pos1] && _cabuf[((head + j) & (_size - 1))][pos2 + 2] == buffer[pos2] && + _cabuf[((head + j) & (_size - 1))][pos3 + 2] == buffer[pos3] && _cabuf[((head + j) & (_size - 1))][pos4 + 2] == buffer[pos4] && + _cabuf[((head + j) & (_size - 1))][pos5 + 2] == buffer[pos5]) + { + found = 1; + break; + } + } + } + if (found) + { + _cabuf[((head + j) & (_size - 1))][0] = length & 0xFF00; + _cabuf[((head + j) & (_size - 1))][1] = length & 0xFF; + memmove(_cabuf[((head + j) & (_size - 1))] + 2, buffer, length * sizeof(T)); + break; + } + } + return found; +} + +template +bool Circular_Buffer::isEqual(const T *buffer) +{ + if (multi) + { + bool success = 1; + for (uint16_t j = 0; j < _available; j++) + { + success = 1; + for (uint16_t k = 0; k < (_cabuf[((head + j) & (_size - 1))][0] | _cabuf[((head + j) & (_size - 1))][1]); k++) + { + if (_cabuf[((head + j) & (_size - 1))][k + 2] != buffer[k]) + { + success = 0; + break; + } + } + if (success) + return 1; + } + } + return 0; +} + +template +void Circular_Buffer::print(const char *p) +{ + if (multi) + return; + write((T *)p, strlen(p)); +} + +template +void Circular_Buffer::println(const char *p) +{ + if (multi) + return; + write((T *)p, strlen(p)); + write('\n'); +} + +template +void Circular_Buffer::push_front(const T *buffer, uint16_t length) +{ + if (multi) + { + if (tail == (head ^ _size)) + tail = ((tail - 1) & (2 * _size - 1)); + head = ((head - 1) & (2 * _size - 1)); + _cabuf[(head & (_size - 1))][0] = length & 0xFF00; + _cabuf[(head & (_size - 1))][1] = length & 0xFF; + memmove(_cabuf[((head) & (_size - 1))] + 2, buffer, length * sizeof(T)); + if (_available < _size) + _available++; + return; + } + for (uint16_t i = length - 1; i > 0; i--) + push_front(buffer[i]); + push_front(buffer[0]); +} + +template +T Circular_Buffer::pop_back() +{ + if (_available) + { + if (_available) + _available--; + tail = ((tail - 1) & (2 * _size - 1)); + return _cbuf[((tail - 1) & (_size - 1))]; + } + return -1; +} + +template +void Circular_Buffer::push_front(T value) +{ + if (multi) + return; + head = ((head - 1) & (2 * _size - 1)); + _cbuf[((head) & (_size - 1))] = value; + if (_available < _size) + _available++; +} + +template +void Circular_Buffer::write(const T *buffer, uint16_t length) +{ + if (multi) + { + _cabuf[((tail) & (_size - 1))][0] = length >> 8; + _cabuf[((tail) & (_size - 1))][1] = length & 0xFF; + memmove(_cabuf[((tail) & (_size - 1))] + 2, buffer, length * sizeof(T)); + if (tail == ((head ^ _size))) + head = ((head + 1) & (2 * _size - 1)); + tail = ((tail + 1) & (2 * _size - 1)); + if (_available < _size) + _available++; + return; + } + if ((_available += length) >= _size) + _available = _size; + if (length < (_size - tail)) + { + memmove(_cbuf + tail, buffer, length * sizeof(T)); + tail = ((tail + length) & (2 * _size - 1)); + } + else + for (uint16_t i = 0; i < length; i++) + write(buffer[i]); +} + +template +void Circular_Buffer::write(T value) +{ + if (multi) + return; + if (_available < _size) + _available++; + _cbuf[((tail) & (_size - 1))] = value; + if (tail == ((head ^ _size))) + head = ((head + 1) & (2 * _size - 1)); + tail = ((tail + 1) & (2 * _size - 1)); +} + +template +T Circular_Buffer::list() +{ + if (multi) + { + if (!size()) + { + Serial.println("There are no queues available..."); + return 0; + } + + Serial.print("\nCircular Array Buffer Queue Size: "); + Serial.print(size()); + Serial.print(" / "); + Serial.println(_size); + + Serial.print("\nFirst Entry: "); + for (uint16_t i = 2; i <= (((int)_cabuf[((head) & (_size - 1))][0] << 8 * sizeof(T)) | (int)_cabuf[((head) & (_size - 1))][1]) + 1; i++) + { + if ((int)(_cabuf[((head + i) & (_size - 1))][i]) != (T)(_cabuf[((head + i) & (_size - 1))][i])) + { // possible float? + Serial.print(_cabuf[((head) & (_size - 1))][i], 7); + } + else + Serial.print(_cabuf[((head) & (_size - 1))][i]); + Serial.print(" "); + } + Serial.print("("); + Serial.print((((int)_cabuf[((head) & (_size - 1))][0] << 8 * sizeof(T)) | (int)_cabuf[((head) & (_size - 1))][1])); + Serial.println(" entries.)"); + + Serial.print("Last Entry: "); + for (uint16_t i = 2; i <= (((int)_cabuf[((head + size() - 1) & (_size - 1))][0] << 8 * sizeof(T)) | (int)_cabuf[((head + size() - 1) & (_size - 1))][1]) + 1; i++) + { + if ((int)(_cabuf[((head + size() - 1) & (_size - 1))][i]) != (T)(_cabuf[((head + size() - 1) & (_size - 1))][i])) + { // possible float? + Serial.print(_cabuf[((head + size() - 1) & (_size - 1))][i], 7); + } + else + Serial.print(_cabuf[((head + size() - 1) & (_size - 1))][i]); + Serial.print(" "); + } + Serial.print("("); + Serial.print((((int)_cabuf[((head + size() - 1) & (_size - 1))][0] << 8 * sizeof(T)) | (int)_cabuf[((head + size() - 1) & (_size - 1))][1])); + Serial.println(" entries.)"); + + Serial.print("\n[Indice] [Entries]\n\n"); + for (uint16_t i = 0; i < size(); i++) + { + Serial.print(" "); + Serial.print(((head + i) & (_size - 1))); + Serial.print("\t\t"); + for (uint16_t j = 2; j <= (((int)_cabuf[((head + i) & (_size - 1))][0] << 8 * sizeof(T)) | (int)_cabuf[((head + i) & (_size - 1))][1]) + 1; j++) + { + if ((int)(_cabuf[((head + i) & (_size - 1))][j]) != (T)(_cabuf[((head + i) & (_size - 1))][j])) + { // possible float? + Serial.print(_cabuf[((head + i) & (_size - 1))][j], 7); + Serial.print("\t"); + } + else + { + Serial.print(_cabuf[((head + i) & (_size - 1))][j]); + Serial.print("\t"); + } + } + Serial.print("("); + Serial.print((((int)_cabuf[((head + i) & (_size - 1))][0] << 8 * sizeof(T)) | (int)_cabuf[((head + i) & (_size - 1))][1])); + Serial.println(" entries.)"); + } + return _available; + } + + if (!size()) + { + Serial.println("There are no queues available..."); + return 0; + } + Serial.print("\nCircular Ring Buffer Queue Size: "); + Serial.print(size()); + Serial.print(" / "); + Serial.println(_size); + + Serial.print("\nIndice: \t"); + + for (uint16_t i = 0; i < _available; i++) + { + Serial.print("["); + Serial.print((head + i) & (_size - 1)); + Serial.print("] \t"); + } + + Serial.print("\nEntries:\t"); + + for (uint16_t i = 0; i < _available; i++) + { + if ((int)_cbuf[((head + i) & (_size - 1))] != (T)_cbuf[((head + i) & (_size - 1))]) + { // possible float? + Serial.print(_cbuf[((head + i) & (_size - 1))], 7); + Serial.print("\t"); + } + else + { + Serial.print(_cbuf[((head + i) & (_size - 1))]); + Serial.print("\t\t"); + } + } + Serial.println('\n'); + + return _available; +} + +template +T Circular_Buffer::read() +{ + if (multi) + { + head = ((head + 1) & (2 * _size - 1)); + if (_available) + _available--; + return 0; + } + if (_available) + _available--; + T value = _cbuf[((head) & (_size - 1))]; + head = ((head + 1) & (2 * _size - 1)); + return value; +} + +template +T Circular_Buffer::sum() +{ + if (multi || !_available) + return 0; + T value = 0; + for (uint16_t i = 0; i < _available; i++) + value += _cbuf[((head + i) & (_size - 1))]; + return value; +} + +template +T Circular_Buffer::average() +{ + if (multi || !_available) + return 0; + return sum() / _available; +} + +template +T Circular_Buffer::variance() +{ + if (multi || !_available) + return 0; + T _mean = average(); + T value = 0; + for (uint16_t i = 0; i < _available; i++) + { + value += ((_cbuf[((head + i) & (_size - 1))] - _mean) * (_cbuf[((head + i) & (_size - 1))] - _mean)); + } + value /= _available; + return value; +} + +template +T Circular_Buffer::deviation() +{ + if (multi || !_available) + return 0; + return sqrt(variance()); +} + +template +T Circular_Buffer::peek(uint16_t pos) +{ + if (multi) + return 0; + if (pos > _size) + return 0; + return _cbuf[((head + pos) & (_size - 1))]; +} + +template +void Circular_Buffer::sort_ascending() +{ + if (multi || !_available) + return; + T buffer[_available]; + for (uint16_t i = 0; i < _available; i++) + buffer[i] = _cbuf[((head + i) & (_size - 1))]; + std::sort(&buffer[0], &buffer[_available]); // sort ascending + for (uint16_t i = 0; i < _available; i++) + _cbuf[((head + i) & (_size - 1))] = buffer[i]; +} + +template +void Circular_Buffer::sort_descending() +{ + if (multi || !_available) + return; + sort_ascending(); + T buffer[_available]; + for (uint16_t i = 0; i < _available; i++) + buffer[i] = _cbuf[((head + i) & (_size - 1))]; + std::reverse(&buffer[0], &buffer[_available]); // sort descending + for (uint16_t i = 0; i < _available; i++) + _cbuf[((head + i) & (_size - 1))] = buffer[i]; +} + +template +T Circular_Buffer::median(bool override) +{ + if (multi || !_available) + return 0; + if (override) + sort_ascending(); + else + { + T buffer[_available]; + for (uint16_t i = 0; i < _available; i++) + buffer[i] = _cbuf[((head + i) & (_size - 1))]; + std::sort(&buffer[0], &buffer[_available]); // sort ascending + } + if (!(_available % 2)) + { + return (_cbuf[((head + ((_available / 2) - 1)) & (_size - 1))] + _cbuf[((head + (_available / 2)) & (_size - 1))]) / 2; + } + else + return _cbuf[((head + ((_available / 2))) & (_size - 1))]; + return 0; +} + +template +T Circular_Buffer::max() +{ + if (multi || !_available) + return 0; + T buffer[_available]; + for (uint16_t i = 0; i < _available; i++) + buffer[i] = _cbuf[((head + i) & (_size - 1))]; + std::sort(&buffer[0], &buffer[_available]); // sort ascending + return buffer[_available - 1]; +} +template +T Circular_Buffer::min() +{ + if (multi || !_available) + return 0; + T buffer[_available]; + for (uint16_t i = 0; i < _available; i++) + buffer[i] = _cbuf[((head + i) & (_size - 1))]; + std::sort(&buffer[0], &buffer[_available]); // sort ascending + return buffer[0]; +} + +template +T Circular_Buffer::peekBytes(T *buffer, uint16_t length) +{ + if (multi) + return 0; + uint16_t _count; + (_available < length) ? _count = _available : _count = length; + if (_count < (_size - head)) + memmove(buffer, _cbuf, _count * sizeof(T)); + else + for (uint16_t i = 0; i < _count; i++) + buffer[i] = peek(i); + return _count; +} + +template +T Circular_Buffer::peek_front(T *buffer, uint16_t length, uint32_t entry) +{ + if (multi) + { + memmove(&buffer[0], &_cabuf[((head + entry) & (_size - 1))][2], length * sizeof(T)); // update CA buffer + return 0; + } +} + +template +T Circular_Buffer::readBytes(T *buffer, uint16_t length) +{ + if (multi) + { + memmove(&buffer[0], &_cabuf[((head) & (_size - 1))][2], length * sizeof(T)); // update CA buffer + read(); + return 0; + } + uint16_t _count; + (_available < length) ? _count = _available : _count = length; // memmove if aligned + if (_count < (_size - head)) + { + _available -= length; + memmove(buffer, _cbuf, _count * sizeof(T)); + head = ((head + _count) & (2 * _size - 1)); + } + else + for (uint16_t i = 0; i < _count; i++) + buffer[i] = read(); // if buffer rollover + return _count; +} + +template +T Circular_Buffer::pop_back(T *buffer, uint16_t length) +{ + if (multi) + { + memmove(&buffer[0], &_cabuf[((tail - 1) & (_size - 1))][2], length * sizeof(T)); + tail = (tail - 1) & (2 * _size - 1); + if (_available) + _available--; + return 0; + } +} + +#endif // Circular_Buffer_H diff --git a/src/event_dispatcher.hpp b/src/event_dispatcher.hpp new file mode 100644 index 0000000..2fdfb7c --- /dev/null +++ b/src/event_dispatcher.hpp @@ -0,0 +1,124 @@ +//================================================================================================ +/// @file event_dispatcher.hpp +/// +/// @brief An object to represent a dispatcher that can invoke callbacks in a thread-safe manner. +/// @author Daan Steenbergen +/// +/// @copyright 2023 Adrian Del Grosso +//================================================================================================ +#ifndef EVENT_DISPATCHER_HPP +#define EVENT_DISPATCHER_HPP + +#include +#include +#include +#include +#include + +namespace isobus +{ + //================================================================================================ + /// @class EventDispatcher + /// + /// @brief A dispatcher that notifies listeners when an event is invoked. + //================================================================================================ + template + class EventDispatcher + { + public: + /// @brief Register a callback to be invoked when the event is invoked. + /// @param callback The callback to register. + /// @return A shared pointer to the callback. + std::shared_ptr> add_listener(const std::function &callback) + { + std::lock_guard lock(callbacksMutex); + auto shared = std::make_shared>(callback); + callbacks.push_back(shared); + return shared; + } + + /// @brief Register a callback to be invoked when the event is invoked. + /// @param callback The callback to register. + /// @param context The context object to pass through to the callback. + /// @return A shared pointer to the contextless callback. + template + std::shared_ptr> add_listener(const std::function)> &callback, std::weak_ptr context) + { + std::function callbackWrapper = [callback, context](const E &...args) { + if (auto contextPtr = context.lock()) + { + callback(args..., contextPtr); + } + }; + return add_listener(callbackWrapper); + } + + /// @brief Register an unsafe callback to be invoked when the event is invoked. + /// @param callback The callback to register. + /// @param context The context object to pass through to the callback. + /// @return A shared pointer to the contextless callback. + template + std::shared_ptr> add_unsafe_listener(const std::function)> &callback, std::weak_ptr context) + { + std::function callbackWrapper = [callback, context](const E &...args) { + callback(args..., context); + }; + return add_listener(callbackWrapper); + } + + /// @brief Get the number of listeners registered to this event. + /// @return The number of listeners + std::size_t get_listener_count() + { + std::lock_guard lock(callbacksMutex); + return callbacks.size(); + } + + /// @brief Remove expired listeners from the dispatcher + void remove_expired_listeners() + { + auto removeResult = std::remove_if(callbacks.begin(), callbacks.end(), [](std::weak_ptr> &callback) { + return callback.expired(); + }); + callbacks.erase(removeResult, callbacks.end()); + } + + /// @brief Call and event with context that is moved using move semantics to notify all listeners. + /// @param args The event context to notify listeners with. + /// @return True if the event was successfully invoked, false otherwise. + void invoke(E &&...args) + { + std::lock_guard lock(callbacksMutex); + remove_expired_listeners(); + + std::for_each(callbacks.begin(), callbacks.end(), [&args...](std::weak_ptr> &callback) { + if (auto callbackPtr = callback.lock()) + { + (*callbackPtr)(std::forward(args)...); + } + }); + } + + /// @brief Call an event with existing context to notify all listeners. + /// @param args The event context to notify listeners with. + /// @return True if the event was successfully invoked, false otherwise. + void call(const E &...args) + { + std::lock_guard lock(callbacksMutex); + remove_expired_listeners(); + + std::for_each(callbacks.begin(), callbacks.end(), [&args...](std::weak_ptr> &callback) { + if (auto callbackPtr = callback.lock()) + { + (*callbackPtr)(args...); + } + }); + } + + private: + std::vector>> callbacks; ///< The callbacks to invoke + std::mutex callbacksMutex; ///< The mutex to protect the callbacks + }; +} // namespace isobus + +#endif // EVENT_DISPATCHER_HPP diff --git a/src/flex_can_t4_plugin.cpp b/src/flex_can_t4_plugin.cpp new file mode 100644 index 0000000..26803a0 --- /dev/null +++ b/src/flex_can_t4_plugin.cpp @@ -0,0 +1,125 @@ +//================================================================================================ +/// @file flex_can_t4_plugin.cpp +/// +/// @brief An interface for using Teensy4/4.1 CAN hardware +/// @author Adrian Del Grosso +/// +/// @copyright 2023 Adrian Del Grosso +//================================================================================================ + +#include "flex_can_t4_plugin.hpp" +#include "FlexCAN_T4.hpp" +#include "can_stack_logger.hpp" + +namespace isobus +{ +#if defined(__IMXRT1062__) + FlexCAN_T4 FlexCANT4Plugin::can0; + FlexCAN_T4 FlexCANT4Plugin::can1; + FlexCAN_T4 FlexCANT4Plugin::can2; +#elif defined(__MK20DX256__) || defined(__MK64FX512__) || defined(__MK66FX1M0__) + FlexCAN_T4 FlexCANT4Plugin::can0; + FlexCAN_T4 FlexCANT4Plugin::can1; +#endif + + FlexCANT4Plugin::FlexCANT4Plugin(std::uint8_t channel) : + selectedChannel(channel) + { + } + + bool FlexCANT4Plugin::get_is_valid() const + { + return isOpen; + } + + void FlexCANT4Plugin::close() + { + // Flex CAN doesn't have a way to stop it... + isOpen = false; + } + + void FlexCANT4Plugin::open() + { + if (0 == selectedChannel) + { + can0.begin(); + can0.setBaudRate(250000); + isOpen = true; + } + else if (1 == selectedChannel) + { + can1.begin(); + can1.setBaudRate(250000); + isOpen = true; + } +#if defined(__IMXRT1062__) + else if (2 == selectedChannel) + { + can2.begin(); + can2.setBaudRate(250000); + isOpen = true; + } +#endif + else + { + isobus::CANStackLogger::critical("[FlexCAN]: Invalid Channel Selected"); + } + } + + bool FlexCANT4Plugin::read_frame(isobus::CANMessageFrame &canFrame) + { + CAN_message_t message; + bool retVal = false; + + if (0 == selectedChannel) + { + retVal = can0.read(message); + canFrame.channel = 0; + } + else if (1 == selectedChannel) + { + retVal = can1.read(message); + canFrame.channel = 1; + } +#if defined(__IMXRT1062__) + else if (2 == selectedChannel) + { + retVal = can2.read(message); + canFrame.channel = 2; + } +#endif + + memcpy(canFrame.data, message.buf, 8); + canFrame.identifier = message.id; + canFrame.dataLength = message.len; + canFrame.isExtendedFrame = message.flags.extended; + return retVal; + } + + bool FlexCANT4Plugin::write_frame(const isobus::CANMessageFrame &canFrame) + { + CAN_message_t message; + bool retVal = false; + + message.id = canFrame.identifier; + message.len = canFrame.dataLength; + message.flags.extended = true; + memcpy(message.buf, canFrame.data, canFrame.dataLength); + + if (0 == selectedChannel) + { + retVal = can0.write(message); + } + else if (1 == selectedChannel) + { + retVal = can1.write(message); + } +#if defined(__IMXRT1062__) + else if (2 == selectedChannel) + { + retVal = can2.write(message); + } +#endif + return retVal; + } +} diff --git a/src/flex_can_t4_plugin.hpp b/src/flex_can_t4_plugin.hpp new file mode 100644 index 0000000..ecd5c6e --- /dev/null +++ b/src/flex_can_t4_plugin.hpp @@ -0,0 +1,63 @@ +//================================================================================================ +/// @file flex_can_t4_plugin.hpp +/// +/// @brief An interface for using FlexCAN_T4 on a Teensy4/4.1 device. +/// @author Adrian Del Grosso +/// +/// @copyright 2023 Adrian Del Grosso +//================================================================================================ +#ifndef FLEX_CAN_T4_PLUGIN_HPP +#define FLEX_CAN_T4_PLUGIN_HPP + +#include "FlexCAN_T4.hpp" +#include "can_hardware_plugin.hpp" +#include "can_hardware_abstraction.hpp" +#include "can_message_frame.hpp" + +namespace isobus +{ + /// @brief An interface for using FlexCAN_T4 on a Teensy4/4.1 device + class FlexCANT4Plugin : public CANHardwarePlugin + { + public: + /// @brief Constructor for the FlexCANT4Plugin + /// @param[in] channel The channel to use on the device + explicit FlexCANT4Plugin(std::uint8_t channel); + + /// @brief The destructor for FlexCANT4Plugin + virtual ~FlexCANT4Plugin() = default; + + /// @brief Returns if the connection with the hardware is valid + /// @returns `true` if connected, `false` if not connected + bool get_is_valid() const override; + + /// @brief Closes the connection to the hardware + void close() override; + + /// @brief Connects to the hardware you specified in the constructor's channel argument + void open() override; + + /// @brief Returns a frame from the hardware (synchronous), or `false` if no frame can be read. + /// @param[in, out] canFrame The CAN frame that was read + /// @returns `true` if a CAN frame was read, otherwise `false` + bool read_frame(isobus::CANMessageFrame &canFrame) override; + + /// @brief Writes a frame to the bus (synchronous) + /// @param[in] canFrame The frame to write to the bus + /// @returns `true` if the frame was written, otherwise `false` + bool write_frame(const isobus::CANMessageFrame &canFrame) override; + + private: +#if defined(__IMXRT1062__) + static FlexCAN_T4 can0; + static FlexCAN_T4 can1; + static FlexCAN_T4 can2; +#elif defined(__MK20DX256__) || defined(__MK64FX512__) || defined(__MK66FX1M0__) + static FlexCAN_T4 can0; + static FlexCAN_T4 can1; +#endif + std::uint8_t selectedChannel; + bool isOpen = false; + }; +} +#endif // FLEX_CAN_T4_PLUGIN_HPP diff --git a/src/imxrt_flexcan.hpp b/src/imxrt_flexcan.hpp new file mode 100644 index 0000000..4e94c33 --- /dev/null +++ b/src/imxrt_flexcan.hpp @@ -0,0 +1,1205 @@ +/* + * File: kinetis_flexcan.h + * Purpose: Register and bit definitions + */ + +#ifndef __KINETIS_FLEXCAN_H +#define __KINETIS_FLEXCAN_H + +#include +#define FLEXCAN1_BASE (0x401D0000) +#define FLEXCAN2_BASE (0x401D4000) + +#define FLEXCANb_MCR(b) (*(vuint32_t *)(b)) +#define FLEXCANb_CTRL1(b) (*(vuint32_t *)(b + 4)) +#define FLEXCANb_TIMER(b) (*(vuint32_t *)(b + 8)) +#define FLEXCANb_RXMGMASK(b) (*(vuint32_t *)(b + 0x10)) +#define FLEXCANb_RX14MASK(b) (*(vuint32_t *)(b + 0x14)) +#define FLEXCANb_RX15MASK(b) (*(vuint32_t *)(b + 0x18)) +#define FLEXCANb_ECR(b) (*(vuint32_t *)(b + 0x1C)) +#define FLEXCANb_ESR1(b) (*(vuint32_t *)(b + 0x20)) +#define FLEXCANb_IMASK2(b) (*(vuint32_t *)(b + 0x24)) +#define FLEXCANb_IMASK1(b) (*(vuint32_t *)(b + 0x28)) +#define FLEXCANb_IFLAG2(b) (*(vuint32_t *)(b + 0x2C)) +#define FLEXCANb_IFLAG1(b) (*(vuint32_t *)(b + 0x30)) +#define FLEXCANb_CTRL2(b) (*(vuint32_t *)(b + 0x34)) +#define FLEXCANb_ESR2(b) (*(vuint32_t *)(b + 0x38)) +#define FLEXCANb_CRCR(b) (*(vuint32_t *)(b + 0x44)) +#define FLEXCANb_RXFGMASK(b) (*(vuint32_t *)(b + 0x48)) +#define FLEXCANb_RXFIR(b) (*(vuint32_t *)(b + 0x4C)) +#define FLEXCANb_RXIMR(b, n) (*(vuint32_t *)(b + 0x880 + (n * 4))) +#define FLEXCANb_GFWR(b, n) (*(vuint32_t *)(b + 0x1E0)) + +#define FLEXCANb_MBn_CS(b, n) (*(vuint32_t *)(b + 0x80 + n * 0x10)) +#define FLEXCANb_MBn_ID(b, n) (*(vuint32_t *)(b + 0x84 + n * 0x10)) +#define FLEXCANb_MBn_WORD0(b, n) (*(vuint32_t *)(b + 0x88 + n * 0x10)) +#define FLEXCANb_MBn_WORD1(b, n) (*(vuint32_t *)(b + 0x8C + n * 0x10)) +#define FLEXCANb_IDFLT_TAB(b, n) (*(vuint32_t *)(b + 0xE0 + (n * 4))) +#define FLEXCANb_MAXMB_SIZE(b) ((FLEXCANb_MCR(b) & 0x7F) + 1) + +// CANFD (confirmed valid) +#define FLEXCANb_FDCTRL(b) (*(vuint32_t *)(b + 0xC00)) +#define FLEXCANb_FDCBT(b) (*(vuint32_t *)(b + 0xC04)) +#define FLEXCANb_CBT(b) (*(vuint32_t *)(b + 0x50)) +#define FLEXCANb_ERFCR(b) (*(vuint32_t *)(b + 0xC0C)) +#define FLEXCANb_DBG1(b) (*(vuint32_t *)(b + 0x58)) +#define FLEXCANb_DBG2(b) (*(vuint32_t *)(b + 0x5C)) + +#define FLEXCAN_MCR_IDAM(x) (((x)&0x00000003) << 8) +#define FLEXCAN_MCR_IDAM_MASK (0x00000300) +#define FLEXCAN_MCR_IDAM_BIT_NO (8) +#define FLEXCAN_MCR_AEN (0x00001000) +#define FLEXCAN_MCR_LPRIO_EN (0x00002000) +#define FLEXCAN_MCR_IRMQ (0x00010000) +#define FLEXCAN_MCR_SRX_DIS (0x00020000) +#define FLEXCAN_MCR_DOZE (0x00040000) +#define FLEXCAN_MCR_WAK_SRC (0x00080000) +#define FLEXCAN_MCR_LPM_ACK (0x00100000) +#define FLEXCAN_MCR_WRN_EN (0x00200000) +#define FLEXCAN_MCR_SLF_WAK (0x00400000) +#define FLEXCAN_MCR_SUPV (0x00800000) +#define FLEXCAN_MCR_FRZ_ACK (0x01000000) +#define FLEXCAN_MCR_SOFT_RST (0x02000000) +#define FLEXCAN_MCR_WAK_MSK (0x04000000) +#define FLEXCAN_MCR_NOT_RDY (0x08000000) +#define FLEXCAN_MCR_HALT (0x10000000) +#define FLEXCAN_MCR_FEN (0x20000000) +#define FLEXCAN_MCR_FRZ (0x40000000) +#define FLEXCAN_MCR_MDIS (0x80000000) +#define FLEXCAN_MCR_DMA (0x00008000) + +#define FLEXCAN_CTRL_PROPSEG(x) (((x)&0x00000007L) << 0) +#define FLEXCAN_CTRL_LOM (0x00000008) +#define FLEXCAN_CTRL_LBUF (0x00000010) +#define FLEXCAN_CTRL_TSYNC (0x00000020) +#define FLEXCAN_CTRL_BOFF_REC (0x00000040) +#define FLEXCAN_CTRL_SMP (0x00000080) +#define FLEXCAN_CTRL_RWRN_MSK (0x00000400) +#define FLEXCAN_CTRL_TWRN_MSK (0x00000800) +#define FLEXCAN_CTRL_LPB (0x00001000UL) +#define FLEXCAN_CTRL_CLK_SRC (0x00002000) +#define FLEXCAN_CTRL_ERR_MSK (0x00004000) +#define FLEXCAN_CTRL_BOFF_MSK (0x00008000) +#define FLEXCAN_CTRL_PSEG2(x) (((x)&0x00000007L) << 16) +#define FLEXCAN_CTRL_PSEG1(x) (((x)&0x00000007L) << 19) +#define FLEXCAN_CTRL_RJW(x) (((x)&0x00000003L) << 22) +#define FLEXCAN_CTRL_PRESDIV(x) (((x)&0x000000FFL) << 24) + +#define FLEXCAN_CTRL2_ISOCANFDEN (BIT12) +#define FLEXCAN_CTRL2_IMEUEN (BIT31) +#define FLEXCAN_CTRL2_RFFN (0x0F000000UL) +#define FLEXCAN_CTRL2_RFFN_BIT_NO (24) +#define FLEXCAN_CTRL2_TASD (0x00F80000UL) +#define FLEXCAN_CTRL2_TASD_BIT_NO (19) +#define FLEXCAN_CTRL2_MRP (BIT18) +#define FLEXCAN_CTRL2_RRS (BIT17) +#define FLEXCAN_CTRL2_EACEN (BIT16) +#define FLEXCAN_CTRL2_MUMASK (BIT1) +#define FLEXCAN_CTRL2_FUMASK (BIT0) +#define FLEXCAN_CTRL2_LOSTRLMSK (BIT2) +#define FLEXCAN_CTRL2_LOSTRMMSK (BIT1) +#define FLEXCAN_CTRL2_IMEUMASK (BIT0) +#define FLEXCAN_set_rffn(ctrl2, rffn) ctrl2 = ((ctrl2) & ~FLEXCAN_CTRL2_RFFN) | ((rffn & 0xF) << FLEXCAN_CTRL2_RFFN_BIT_NO) +/* Common bit definition */ +#define BIT0 (1UL) +#define BIT1 (BIT0 << 1) +#define BIT2 (BIT0 << 2) +#define BIT3 (BIT0 << 3) +#define BIT4 (BIT0 << 4) +#define BIT5 (BIT0 << 5) +#define BIT6 (BIT0 << 6) +#define BIT7 (BIT0 << 7) +#define BIT8 (BIT0 << 8) +#define BIT9 (BIT0 << 9) +#define BIT10 (BIT0 << 10) +#define BIT11 (BIT0 << 11) +#define BIT12 (0x00001000UL) +#define BIT13 (0x00002000UL) +#define BIT14 (0x00004000UL) +#define BIT15 (0x00008000UL) +#define BIT16 (0x00010000UL) +#define BIT17 (0x00020000UL) +#define BIT18 (0x00040000UL) +#define BIT19 (0x00080000UL) +#define BIT20 (0x00100000UL) +#define BIT21 (0x00200000UL) +#define BIT22 (0x00400000UL) +#define BIT23 (0x00800000UL) +#define BIT24 (0x01000000UL) +#define BIT25 (0x02000000UL) +#define BIT26 (0x04000000UL) +#define BIT27 (0x08000000UL) +#define BIT28 (0x10000000UL) +#define BIT29 (0x20000000UL) +#define BIT30 (0x40000000UL) +#define BIT31 (0x80000000UL) + +/* FlexCAN module I/O Base Addresss */ + +typedef volatile uint32_t vuint32_t; + +/********************************************************************* +* +* FlexCAN0 (FLEXCAN0) +* +*********************************************************************/ + +/* Register read/write macros */ +#define FLEXCAN0_MCR (*(vuint32_t *)(FLEXCAN0_BASE)) +#define FLEXCAN0_CTRL1 (*(vuint32_t *)(FLEXCAN0_BASE + 4)) +#define FLEXCAN0_TIMER (*(vuint32_t *)(FLEXCAN0_BASE + 8)) +#define FLEXCAN0_TCR (*(vuint32_t *)(FLEXCAN0_BASE + 0x0C)) +#define FLEXCAN0_RXMGMASK (*(vuint32_t *)(FLEXCAN0_BASE + 0x10)) +#define FLEXCAN0_RX14MASK (*(vuint32_t *)(FLEXCAN0_BASE + 0x14)) +#define FLEXCAN0_RX15MASK (*(vuint32_t *)(FLEXCAN0_BASE + 0x18)) +#define FLEXCAN0_ECR (*(vuint32_t *)(FLEXCAN0_BASE + 0x1C)) +#define FLEXCAN0_ESR1 (*(vuint32_t *)(FLEXCAN0_BASE + 0x20)) +#define FLEXCAN0_IMASK2 (*(vuint32_t *)(FLEXCAN0_BASE + 0x24)) +#define FLEXCAN0_IMASK1 (*(vuint32_t *)(FLEXCAN0_BASE + 0x28)) +#define FLEXCAN0_IFLAG2 (*(vuint32_t *)(FLEXCAN0_BASE + 0x2C)) +#define FLEXCAN0_IFLAG1 (*(vuint32_t *)(FLEXCAN0_BASE + 0x30)) +#define FLEXCAN0_CTRL2 (*(vuint32_t *)(FLEXCAN0_BASE + 0x34)) +#define FLEXCAN0_ESR2 (*(vuint32_t *)(FLEXCAN0_BASE + 0x38)) +#define FLEXCAN0_FUREQ (*(vuint32_t *)(FLEXCAN0_BASE + 0x3C)) +#define FLEXCAN0_FUACK (*(vuint32_t *)(FLEXCAN0_BASE + 0x40)) +#define FLEXCAN0_CRCR (*(vuint32_t *)(FLEXCAN0_BASE + 0x44)) +#define FLEXCAN0_RXFGMASK (*(vuint32_t *)(FLEXCAN0_BASE + 0x48)) +#define FLEXCAN0_RXFIR (*(vuint32_t *)(FLEXCAN0_BASE + 0x4C)) +#define FLEXCAN0_DBG1 (*(vuint32_t *)(FLEXCAN0_BASE + 0x58)) +#define FLEXCAN0_DBG2 (*(vuint32_t *)(FLEXCAN0_BASE + 0x5C)) + +#define FLEXCAN0_IMEUR FLEXCAN0_FUREQ +#define FLEXCAN0_LRFR FLEXCAN0_FUACK + +/* Message Buffers */ +#define FLEXCAN0_MB0_CS (*(vuint32_t *)(FLEXCAN0_BASE + 0x80)) +#define FLEXCAN0_MB0_ID (*(vuint32_t *)(FLEXCAN0_BASE + 0x84)) +#define FLEXCAN0_MB0_WORD0 (*(vuint32_t *)(FLEXCAN0_BASE + 0x88)) +#define FLEXCAN0_MB0_WORD1 (*(vuint32_t *)(FLEXCAN0_BASE + 0x8C)) + +#define FLEXCAN0_MBn_CS(n) (*(vuint32_t *)(FLEXCAN0_BASE + 0x80 + n * 0x10)) +#define FLEXCAN0_MBn_ID(n) (*(vuint32_t *)(FLEXCAN0_BASE + 0x84 + n * 0x10)) +#define FLEXCAN0_MBn_WORD0(n) (*(vuint32_t *)(FLEXCAN0_BASE + 0x88 + n * 0x10)) +#define FLEXCAN0_MBn_WORD1(n) (*(vuint32_t *)(FLEXCAN0_BASE + 0x8C + n * 0x10)) + +/* Rx Individual Mask Registers */ +#define FLEXCAN0_RXIMR0 (*(vuint32_t *)(FLEXCAN0_BASE + 0x880)) +#define FLEXCAN0_RXIMRn(n) (*(vuint32_t *)(FLEXCAN0_BASE + 0x880 + n * 4)) + +/* Rx FIFO ID Filter Table Element 0 to 127 */ +#define FLEXCAN0_IDFLT_TAB0 (*(vuint32_t *)(FLEXCAN0_BASE + 0xE0)) +#define FLEXCAN0_IDFLT_TAB(n) (*(vuint32_t *)(FLEXCAN0_BASE + 0xE0 + (n * 4))) +//#define FLEXCAN0_IDFLT_TAB(n) (*(vuint32_t*)(FLEXCAN0_BASE+0xE0+(n<<2))) + +/* Memory Error Control Register */ +#define FLEXCAN0_MECR *(vuint32_t*)(FLEXCAN0_BASE+0x3B70)) + +/* Error Injection Address Register */ +#define FLEXCAN0_ERRIAR *(vuint32_t*)(FLEXCAN0_BASE+0x3B74)) + +/* Error Injection Data Pattern Register */ +#define FLEXCAN0_ERRIDPR *(vuint32_t*)(FLEXCAN0_BASE+0x3B78)) + +/* Error Injection Parity Pattern Register */ +#define FLEXCAN0_ERRIPPR *(vuint32_t*)(FLEXCAN0_BASE+0x3B7C)) + +/* Error Report Address Register */ +#define FLEXCAN0_RERRAR *(vuint32_t*)(FLEXCAN0_BASE+0x3B80)) + +/* Error Report Data Register */ +#define FLEXCAN0_RERRDR *(vuint32_t*)(FLEXCAN0_BASE+0x3B84)) + +/* Error Report Syndrome Register */ +#define FLEXCAN0_RERRSYNR *(vuint32_t*)(FLEXCAN0_BASE+0x3B88)) + +/* Error Status Register */ +#define FLEXCAN0_ERRSR *(vuint32_t*)(FLEXCAN0_BASE+0x3B8C)) + +/********************************************************************* +* +* FlexCAN1 (FLEXCAN1) +* +*********************************************************************/ +/* Register read/write macros */ +/* +#define FLEXCAN1_MCR (*(vuint32_t*)(FLEXCAN1_BASE)) +#define FLEXCAN1_CTRL1 (*(vuint32_t*)(FLEXCAN1_BASE+4)) +#define FLEXCAN1_TIMER (*(vuint32_t*)(FLEXCAN1_BASE+8)) +#define FLEXCAN1_TCR (*(vuint32_t*)(FLEXCAN1_BASE+0x0C)) +#define FLEXCAN1_RXMGMASK (*(vuint32_t*)(FLEXCAN1_BASE+0x10)) +#define FLEXCAN1_RX14MASK (*(vuint32_t*)(FLEXCAN1_BASE+0x14)) +#define FLEXCAN1_RX15MASK (*(vuint32_t*)(FLEXCAN1_BASE+0x18)) +#define FLEXCAN1_ECR (*(vuint32_t*)(FLEXCAN1_BASE+0x1C)) +#define FLEXCAN1_ESR1 (*(vuint32_t*)(FLEXCAN1_BASE+0x20)) +#define FLEXCAN1_IMASK2 (*(vuint32_t*)(FLEXCAN1_BASE+0x24)) +#define FLEXCAN1_IMASK1 (*(vuint32_t*)(FLEXCAN1_BASE+0x28)) +#define FLEXCAN1_IFLAG2 (*(vuint32_t*)(FLEXCAN1_BASE+0x2C)) +#define FLEXCAN1_IFLAG1 (*(vuint32_t*)(FLEXCAN1_BASE+0x30)) +#define FLEXCAN1_CTRL2 (*(vuint32_t*)(FLEXCAN1_BASE+0x34)) +#define FLEXCAN1_ESR2 (*(vuint32_t*)(FLEXCAN1_BASE+0x38)) +#define FLEXCAN1_FUREQ (*(vuint32_t*)(FLEXCAN1_BASE+0x3C)) +#define FLEXCAN1_FUACK (*(vuint32_t*)(FLEXCAN1_BASE+0x40)) +#define FLEXCAN1_CRCR (*(vuint32_t*)(FLEXCAN1_BASE+0x44)) +#define FLEXCAN1_RXFGMASK (*(vuint32_t*)(FLEXCAN1_BASE+0x48)) +#define FLEXCAN1_RXFIR (*(vuint32_t*)(FLEXCAN1_BASE+0x4C)) +#define FLEXCAN1_DBG1 (*(vuint32_t*)(FLEXCAN1_BASE+0x58)) +#define FLEXCAN1_DBG2 (*(vuint32_t*)(FLEXCAN1_BASE+0x5C)) + +#define FLEXCAN1_IMEUR FLEXCAN1_FUREQ +#define FLEXCAN1_LRFR FLEXCAN1_FUACK +*/ +/* Message Buffers */ +#define FLEXCAN1_MB0_CS (*(vuint32_t *)(FLEXCAN1_BASE + 0x80)) +#define FLEXCAN1_MB0_ID (*(vuint32_t *)(FLEXCAN1_BASE + 0x84)) +#define FLEXCAN1_MB0_WORD0 (*(vuint32_t *)(FLEXCAN1_BASE + 0x88)) +#define FLEXCAN1_MB0_WORD1 (*(vuint32_t *)(FLEXCAN1_BASE + 0x8C)) + +#define FLEXCAN1_MBn_CS(n) (*(vuint32_t *)(FLEXCAN1_BASE + 0x80 + n * 0x10)) +#define FLEXCAN1_MBn_ID(n) (*(vuint32_t *)(FLEXCAN1_BASE + 0x84 + n * 0x10)) +#define FLEXCAN1_MBn_WORD0(n) (*(vuint32_t *)(FLEXCAN1_BASE + 0x88 + n * 0x10)) +#define FLEXCAN1_MBn_WORD1(n) (*(vuint32_t *)(FLEXCAN1_BASE + 0x8C + n * 0x10)) + +/* Rx Individual Mask Registers */ +/* +#define FLEXCAN1_RXIMR0 (*(vuint32_t*)(FLEXCAN1_BASE+0x880)) +#define FLEXCAN1_RXIMRn(n) (*(vuint32_t*)(FLEXCAN1_BASE+0x880+n*4)) +*/ + +/* Rx FIFO ID Filter Table Element 0 to 127 */ +#define FLEXCAN1_IDFLT_TAB0 (*(vuint32_t *)(FLEXCAN1_BASE + 0xE0)) +#define FLEXCAN1_IDFLT_TAB(n) (*(vuint32_t *)(FLEXCAN1_BASE + 0xE0 + (n << 2))) + +/* Memory Error Control Register */ +#define FLEXCAN1_MECR *(vuint32_t*)(FLEXCAN1_BASE+0x7B70)) + +/* Error Injection Address Register */ +#define FLEXCAN1_ERRIAR *(vuint32_t*)(FLEXCAN1_BASE+0x3B74)) + +/* Error Injection Data Pattern Register */ +#define FLEXCAN1_ERRIDPR *(vuint32_t*)(FLEXCAN1_BASE+0x3B78)) + +/* Error Injection Parity Pattern Register */ +#define FLEXCAN1_ERRIPPR *(vuint32_t*)(FLEXCAN1_BASE+0x3B7C)) + +/* Error Report Address Register */ +#define FLEXCAN1_RERRAR *(vuint32_t*)(FLEXCAN1_BASE+0x3B80)) + +/* Error Report Data Register */ +#define FLEXCAN1_RERRDR *(vuint32_t*)(FLEXCAN1_BASE+0x3B84)) + +/* Error Report Syndrome Register */ +#define FLEXCAN1_RERRSYNR *(vuint32_t*)(FLEXCAN1_BASE+0x3B88)) + +/* Error Status Register */ +#define FLEXCAN1_ERRSR *(vuint32_t*)(FLEXCAN1_BASE+0x3B8C)) + +/* Bit definitions and macros for FLEXCAN_MCR */ +/* +#define FLEXCAN_MCR_MAXMB(x) (((x)&0x0000007F)<<0) +#define FLEXCAN_MCR_MAXMB_MASK (0x0000007F) +*/ +/* Bit definitions and macros for FLEXCAN_CTRL */ +/* +#define FLEXCAN_CTRL_PROPSEG(x) (((x)&0x00000007L)<<0) +#define FLEXCAN_CTRL_LOM (0x00000008) +#define FLEXCAN_CTRL_LBUF (0x00000010) +#define FLEXCAN_CTRL_TSYNC (0x00000020) +#define FLEXCAN_CTRL_BOFF_REC (0x00000040) +#define FLEXCAN_CTRL_SMP (0x00000080) +#define FLEXCAN_CTRL_RWRN_MSK (0x00000400) +#define FLEXCAN_CTRL_TWRN_MSK (0x00000800) +#define FLEXCAN_CTRL_LPB (0x00001000UL) +#define FLEXCAN_CTRL_CLK_SRC (0x00002000) +#define FLEXCAN_CTRL_ERR_MSK (0x00004000) +#define FLEXCAN_CTRL_BOFF_MSK (0x00008000) +#define FLEXCAN_CTRL_PSEG2(x) (((x)&0x00000007L)<<16) +#define FLEXCAN_CTRL_PSEG1(x) (((x)&0x00000007L)<<19) +#define FLEXCAN_CTRL_RJW(x) (((x)&0x00000003L)<<22) +#define FLEXCAN_CTRL_PRESDIV(x) (((x)&0x000000FFL)<<24) +*/ +/* Bit definitions and macros for FLEXCAN_CTRL2 */ +/* +*/ + +/* Bit definitions and macros for FLEXCAN_TIMER */ +#define FLEXCAN_TIMER_TIMER(x) (((x)&0x0000FFFF) << 0) + +/* Bit definitions and macros for FLEXCAN_TCR */ +#define FLEXCAN_TCR_DSCACK (0x00000100) +#define FLEXCAN_TCR_BIT_CLS (0x00000200) +#define FLEXCAN_TCR_TRD (0x00000400) + +/* Bit definitions and macros for FLEXCAN_RXGMASK */ +/* +#define FLEXCAN_RXGMASK_MI0 (0x00000001) +#define FLEXCAN_RXGMASK_MI1 (0x00000002) +#define FLEXCAN_RXGMASK_MI2 (0x00000004) +#define FLEXCAN_RXGMASK_MI3 (0x00000008) +#define FLEXCAN_RXGMASK_MI4 (0x00000010) +#define FLEXCAN_RXGMASK_MI5 (0x00000020) +#define FLEXCAN_RXGMASK_MI6 (0x00000040) +#define FLEXCAN_RXGMASK_MI7 (0x00000080) +#define FLEXCAN_RXGMASK_MI8 (0x00000100) +#define FLEXCAN_RXGMASK_MI9 (0x00000200) +#define FLEXCAN_RXGMASK_MI10 (0x00000400) +#define FLEXCAN_RXGMASK_MI11 (0x00000800) +#define FLEXCAN_RXGMASK_MI12 (0x00001000) +#define FLEXCAN_RXGMASK_MI13 (0x00002000) +#define FLEXCAN_RXGMASK_MI14 (0x00004000) +#define FLEXCAN_RXGMASK_MI15 (0x00008000) +#define FLEXCAN_RXGMASK_MI16 (0x00010000) +#define FLEXCAN_RXGMASK_MI17 (0x00020000) +#define FLEXCAN_RXGMASK_MI18 (0x00040000) +#define FLEXCAN_RXGMASK_MI19 (0x00080000) +#define FLEXCAN_RXGMASK_MI20 (0x00100000) +#define FLEXCAN_RXGMASK_MI21 (0x00200000) +#define FLEXCAN_RXGMASK_MI22 (0x00400000) +#define FLEXCAN_RXGMASK_MI23 (0x00800000) +#define FLEXCAN_RXGMASK_MI24 (0x01000000) +#define FLEXCAN_RXGMASK_MI25 (0x02000000) +#define FLEXCAN_RXGMASK_MI26 (0x04000000) +#define FLEXCAN_RXGMASK_MI27 (0x08000000) +#define FLEXCAN_RXGMASK_MI28 (0x10000000) +#define FLEXCAN_RXGMASK_MI29 (0x20000000) +#define FLEXCAN_RXGMASK_MI30 (0x40000000) +#define FLEXCAN_RXGMASK_MI31 (0x80000000) +*/ +/* Bit definitions and macros for FLEXCAN_RX14MASK */ +/* +#define FLEXCAN_RX14MASK_MI0 (0x00000001) +#define FLEXCAN_RX14MASK_MI1 (0x00000002) +#define FLEXCAN_RX14MASK_MI2 (0x00000004) +#define FLEXCAN_RX14MASK_MI3 (0x00000008) +#define FLEXCAN_RX14MASK_MI4 (0x00000010) +#define FLEXCAN_RX14MASK_MI5 (0x00000020) +#define FLEXCAN_RX14MASK_MI6 (0x00000040) +#define FLEXCAN_RX14MASK_MI7 (0x00000080) +#define FLEXCAN_RX14MASK_MI8 (0x00000100) +#define FLEXCAN_RX14MASK_MI9 (0x00000200) +#define FLEXCAN_RX14MASK_MI10 (0x00000400) +#define FLEXCAN_RX14MASK_MI11 (0x00000800) +#define FLEXCAN_RX14MASK_MI12 (0x00001000) +#define FLEXCAN_RX14MASK_MI13 (0x00002000) +#define FLEXCAN_RX14MASK_MI14 (0x00004000) +#define FLEXCAN_RX14MASK_MI15 (0x00008000) +#define FLEXCAN_RX14MASK_MI16 (0x00010000) +#define FLEXCAN_RX14MASK_MI17 (0x00020000) +#define FLEXCAN_RX14MASK_MI18 (0x00040000) +#define FLEXCAN_RX14MASK_MI19 (0x00080000) +#define FLEXCAN_RX14MASK_MI20 (0x00100000) +#define FLEXCAN_RX14MASK_MI21 (0x00200000) +#define FLEXCAN_RX14MASK_MI22 (0x00400000) +#define FLEXCAN_RX14MASK_MI23 (0x00800000) +#define FLEXCAN_RX14MASK_MI24 (0x01000000) +#define FLEXCAN_RX14MASK_MI25 (0x02000000) +#define FLEXCAN_RX14MASK_MI26 (0x04000000) +#define FLEXCAN_RX14MASK_MI27 (0x08000000) +#define FLEXCAN_RX14MASK_MI28 (0x10000000) +#define FLEXCAN_RX14MASK_MI29 (0x20000000) +#define FLEXCAN_RX14MASK_MI30 (0x40000000) +#define FLEXCAN_RX14MASK_MI31 (0x80000000) +*/ +/* Bit definitions and macros for FLEXCAN_RX15MASK */ +#define FLEXCAN_RX15MASK_MI0 (0x00000001) +#define FLEXCAN_RX15MASK_MI1 (0x00000002) +#define FLEXCAN_RX15MASK_MI2 (0x00000004) +#define FLEXCAN_RX15MASK_MI3 (0x00000008) +#define FLEXCAN_RX15MASK_MI4 (0x00000010) +#define FLEXCAN_RX15MASK_MI5 (0x00000020) +#define FLEXCAN_RX15MASK_MI6 (0x00000040) +#define FLEXCAN_RX15MASK_MI7 (0x00000080) +#define FLEXCAN_RX15MASK_MI8 (0x00000100) +#define FLEXCAN_RX15MASK_MI9 (0x00000200) +#define FLEXCAN_RX15MASK_MI10 (0x00000400) +#define FLEXCAN_RX15MASK_MI11 (0x00000800) +#define FLEXCAN_RX15MASK_MI12 (0x00001000) +#define FLEXCAN_RX15MASK_MI13 (0x00002000) +#define FLEXCAN_RX15MASK_MI14 (0x00004000) +#define FLEXCAN_RX15MASK_MI15 (0x00008000) +#define FLEXCAN_RX15MASK_MI16 (0x00010000) +#define FLEXCAN_RX15MASK_MI17 (0x00020000) +#define FLEXCAN_RX15MASK_MI18 (0x00040000) +#define FLEXCAN_RX15MASK_MI19 (0x00080000) +#define FLEXCAN_RX15MASK_MI20 (0x00100000) +#define FLEXCAN_RX15MASK_MI21 (0x00200000) +#define FLEXCAN_RX15MASK_MI22 (0x00400000) +#define FLEXCAN_RX15MASK_MI23 (0x00800000) +#define FLEXCAN_RX15MASK_MI24 (0x01000000) +#define FLEXCAN_RX15MASK_MI25 (0x02000000) +#define FLEXCAN_RX15MASK_MI26 (0x04000000) +#define FLEXCAN_RX15MASK_MI27 (0x08000000) +#define FLEXCAN_RX15MASK_MI28 (0x10000000) +#define FLEXCAN_RX15MASK_MI29 (0x20000000) +#define FLEXCAN_RX15MASK_MI30 (0x40000000) +#define FLEXCAN_RX15MASK_MI31 (0x80000000) + +/* Bit definitions and macros for FLEXCAN_ECR */ +#define FLEXCAN_ECR_TX_ERR_COUNTER(x) (((x)&0x000000FF) << 0) +#define FLEXCAN_ECR_RX_ERR_COUNTER(x) (((x)&0x000000FF) << 8) + +/* Bit definitions and macros for FLEXCAN_ESR1 */ +#define FLEXCAN_ESR_WAK_INT (0x00000001) +#define FLEXCAN_ESR_ERR_INT (0x00000002) +#define FLEXCAN_ESR_BOFF_INT (0x00000004) +#define FLEXCAN_ESR_RX (0x00000008) +#define FLEXCAN_ESR_FLT_CONF(x) (((x)&0x00000003) << 4) +#define FLEXCAN_ESR_FLT_CONF_MASK (0x00000030) +#define FLEXCAN_ESR_TX (0x00000040) +#define FLEXCAN_ESR_IDLE (0x00000080) +#define FLEXCAN_ESR_RX_WRN (0x00000100) +#define FLEXCAN_ESR_TX_WRN (0x00000200) +#define FLEXCAN_ESR_STF_ERR (0x00000400) +#define FLEXCAN_ESR_FRM_ERR (0x00000800) +#define FLEXCAN_ESR_CRC_ERR (0x00001000) +#define FLEXCAN_ESR_ACK_ERR (0x00002000) +#define FLEXCAN_ESR_BIT0_ERR (0x00004000) +#define FLEXCAN_ESR_BIT1_ERR (0x00008000) +#define FLEXCAN_ESR_RWRN_INT (0x00010000) +#define FLEXCAN_ESR_TWRN_INT (0x00020000) +#define FLEXCAN_ESR_get_fault_code(esr) (((esr)&FLEXCAN_ESR_FLT_CONF_MASK) >> 4) +#define CAN_ERROR_ACTIVE 0 +#define CAN_ERROR_PASSIVE 1 +#define CAN_ERROR_BUS_OFF 2 + +/* Bit definition for FLEXCAN_ESR2 */ +#define FLEXCAN_ESR2_IMB (0x00002000) +#define FLEXCAN_ESR2_VPS (0x00004000) +#define FLEXCAN_ESR2_LTM (0x007F0000UL) +#define FLEXCAN_ESR2_LTM_BIT_NO (16) +#define FLEXCAN_ESR2_LOSTRLF (0x00000004) +#define FLEXCAN_ESR2_LOSTRMF (0x00000002) +#define FLEXCAN_ESR2_IMEUF (0x00000001) +#define FLEXCAN_get_LTM(esr2_value) (((esr2_value) & (FLEXCAN_ESR2_LTM)) >> (FLEXCAN_ESR2_LTM_BIT_NO)) + +/* Bit definitions and macros for FLEXCAN_IMASK1 */ +#define FLEXCAN_IMASK1_BUF0M (0x00000001) +#define FLEXCAN_IMASK1_BUF1M (0x00000002) +#define FLEXCAN_IMASK1_BUF2M (0x00000004) +#define FLEXCAN_IMASK1_BUF3M (0x00000008) +#define FLEXCAN_IMASK1_BUF4M (0x00000010) +#define FLEXCAN_IMASK1_BUF5M (0x00000020) +#define FLEXCAN_IMASK1_BUF6M (0x00000040) +#define FLEXCAN_IMASK1_BUF7M (0x00000080) +#define FLEXCAN_IMASK1_BUF8M (0x00000100) +#define FLEXCAN_IMASK1_BUF9M (0x00000200) +#define FLEXCAN_IMASK1_BUF10M (0x00000400) +#define FLEXCAN_IMASK1_BUF11M (0x00000800) +#define FLEXCAN_IMASK1_BUF12M (0x00001000) +#define FLEXCAN_IMASK1_BUF13M (0x00002000) +#define FLEXCAN_IMASK1_BUF14M (0x00004000) +#define FLEXCAN_IMASK1_BUF15M (0x00008000) +#define FLEXCAN_IMASK1_BUF16M (0x00010000) +#define FLEXCAN_IMASK1_BUF17M (0x00020000) +#define FLEXCAN_IMASK1_BUF18M (0x00040000) +#define FLEXCAN_IMASK1_BUF19M (0x00080000) +#define FLEXCAN_IMASK1_BUF20M (0x00100000) +#define FLEXCAN_IMASK1_BUF21M (0x00200000) +#define FLEXCAN_IMASK1_BUF22M (0x00400000) +#define FLEXCAN_IMASK1_BUF23M (0x00800000) +#define FLEXCAN_IMASK1_BUF24M (0x01000000) +#define FLEXCAN_IMASK1_BUF25M (0x02000000) +#define FLEXCAN_IMASK1_BUF26M (0x04000000) +#define FLEXCAN_IMASK1_BUF27M (0x08000000) +#define FLEXCAN_IMASK1_BUF28M (0x10000000) +#define FLEXCAN_IMASK1_BUF29M (0x20000000) +#define FLEXCAN_IMASK1_BUF30M (0x40000000) +#define FLEXCAN_IMASK1_BUF31M (0x80000000) + +/* Bit definitions and macros for FLEXCAN_IFLAG1 */ +#define FLEXCAN_IFLAG1_BUF0I (0x00000001) +#define FLEXCAN_IFLAG1_BUF1I (0x00000002) +#define FLEXCAN_IFLAG1_BUF2I (0x00000004) +#define FLEXCAN_IFLAG1_BUF3I (0x00000008) +#define FLEXCAN_IFLAG1_BUF4I (0x00000010) +#define FLEXCAN_IFLAG1_BUF5I (0x00000020) +#define FLEXCAN_IFLAG1_BUF6I (0x00000040) +#define FLEXCAN_IFLAG1_BUF7I (0x00000080) +#define FLEXCAN_IFLAG1_BUF8I (0x00000100) +#define FLEXCAN_IFLAG1_BUF9I (0x00000200) +#define FLEXCAN_IFLAG1_BUF10I (0x00000400) +#define FLEXCAN_IFLAG1_BUF11I (0x00000800) +#define FLEXCAN_IFLAG1_BUF12I (0x00001000) +#define FLEXCAN_IFLAG1_BUF13I (0x00002000) +#define FLEXCAN_IFLAG1_BUF14I (0x00004000) +#define FLEXCAN_IFLAG1_BUF15I (0x00008000) +#define FLEXCAN_IFLAG1_BUF16I (0x00010000) +#define FLEXCAN_IFLAG1_BUF17I (0x00020000) +#define FLEXCAN_IFLAG1_BUF18I (0x00040000) +#define FLEXCAN_IFLAG1_BUF19I (0x00080000) +#define FLEXCAN_IFLAG1_BUF20I (0x00100000) +#define FLEXCAN_IFLAG1_BUF21I (0x00200000) +#define FLEXCAN_IFLAG1_BUF22I (0x00400000) +#define FLEXCAN_IFLAG1_BUF23I (0x00800000) +#define FLEXCAN_IFLAG1_BUF24I (0x01000000) +#define FLEXCAN_IFLAG1_BUF25I (0x02000000) +#define FLEXCAN_IFLAG1_BUF26I (0x04000000) +#define FLEXCAN_IFLAG1_BUF27I (0x08000000) +#define FLEXCAN_IFLAG1_BUF28I (0x10000000) +#define FLEXCAN_IFLAG1_BUF29I (0x20000000) +#define FLEXCAN_IFLAG1_BUF30I (0x40000000) +#define FLEXCAN_IFLAG1_BUF31I (0x80000000) + +/* Bit definitions and macros for FLEXCAN_MB_CS */ +#define FLEXCAN_MB_CS_TIMESTAMP(x) (((x)&0x0000FFFF) << 0) +#define FLEXCAN_MB_CS_TIMESTAMP_MASK (0x0000FFFFUL) +#define FLEXCAN_MB_CS_LENGTH(x) (((x)&0x0000000F) << 16) +#define FLEXCAN_MB_CS_RTR (0x00100000) +#define FLEXCAN_MB_CS_IDE (0x00200000) +#define FLEXCAN_MB_CS_SRR (0x00400000) +#define FLEXCAN_MB_CS_CODE(x) (((x)&0x0000000F) << 24) +#define FLEXCAN_MB_CS_CODE_MASK (0x0F000000UL) +#define FLEXCAN_MB_CS_DLC_MASK (0x000F0000UL) +#define FLEXCAN_MB_CODE_RX_INACTIVE (0) +#define FLEXCAN_MB_CODE_RX_EMPTY (4) +#define FLEXCAN_MB_CODE_RX_FULL (2) +#define FLEXCAN_MB_CODE_RX_OVERRUN (6) +#define FLEXCAN_MB_CODE_RX_BUSY (1) + +#define FLEXCAN_MB_CS_IDE_BIT_NO (21) +#define FLEXCAN_MB_CS_RTR_BIT_NO (20) +#define FLEXCAN_MB_CS_DLC_BIT_NO (16) + +#define FLEXCAN_MB_CODE_TX_INACTIVE (8) +#define FLEXCAN_MB_CODE_TX_ABORT (9) +#define FLEXCAN_MB_CODE_TX_ONCE (0x0C) +#define FLEXCAN_MB_CODE_TX_RESPONSE (0x0A) +#define FLEXCAN_MB_CODE_TX_RESPONSE_TEMPO (0x0E) +#define FLEXCAN_get_code(cs) (((cs)&FLEXCAN_MB_CS_CODE_MASK) >> 24) +#define FLEXCAN_get_length(cs) (((cs)&FLEXCAN_MB_CS_DLC_MASK) >> 16) +#define FLEXCAN_get_timestamp(cs) (((cs)&FLEXCAN_MB_CS_TIMESTAMP_MASK) >> 0) + +/* Bit definitions and macros for FLEXCAN_MB_ID */ +#define FLEXCAN_MB_ID_STD_MASK (0x1FFC0000UL) +#define FLEXCAN_MB_ID_EXT_MASK (0x1FFFFFFFUL) +#define FLEXCAN_MB_ID_IDEXT(x) (((x)&0x0003FFFF) << 0) +#define FLEXCAN_MB_ID_IDSTD(x) (((x)&0x000007FF) << 18) +#define FLEXCAN_MB_ID_PRIO(x) (((x)&0x00000007) << 29) +#define FLEXCAN_MB_ID_PRIO_BIT_NO (29) +#define FLEXCAN_MB_ID_STD_BIT_NO (18) +#define FLEXCAN_MB_ID_EXT_BIT_NO (0) + +/* Bit definitions and macros for FLEXCAN_MB_WORD0 */ +#define FLEXCAN_MB_WORD0_DATA3(x) (((x)&0x000000FF) << 0) +#define FLEXCAN_MB_WORD0_DATA2(x) (((x)&0x000000FF) << 8) +#define FLEXCAN_MB_WORD0_DATA1(x) (((x)&0x000000FF) << 16) +#define FLEXCAN_MB_WORD0_DATA0(x) (((x)&0x000000FF) << 24) + +/* Bit definitions and macros for FLEXCAN_MB_WORD1 */ +#define FLEXCAN_MB_WORD1_DATA7(x) (((x)&0x000000FF) << 0) +#define FLEXCAN_MB_WORD1_DATA6(x) (((x)&0x000000FF) << 8) +#define FLEXCAN_MB_WORD1_DATA5(x) (((x)&0x000000FF) << 16) +#define FLEXCAN_MB_WORD1_DATA4(x) (((x)&0x000000FF) << 24) + +/* Bit definitions and macros for FLEXCAN_RXIMR0 */ +#define FLEXCAN_RXIMR0_MI0 (0x00000001) +#define FLEXCAN_RXIMR0_MI1 (0x00000002) +#define FLEXCAN_RXIMR0_MI2 (0x00000004) +#define FLEXCAN_RXIMR0_MI3 (0x00000008) +#define FLEXCAN_RXIMR0_MI4 (0x00000010) +#define FLEXCAN_RXIMR0_MI5 (0x00000020) +#define FLEXCAN_RXIMR0_MI6 (0x00000040) +#define FLEXCAN_RXIMR0_MI7 (0x00000080) +#define FLEXCAN_RXIMR0_MI8 (0x00000100) +#define FLEXCAN_RXIMR0_MI9 (0x00000200) +#define FLEXCAN_RXIMR0_MI10 (0x00000400) +#define FLEXCAN_RXIMR0_MI11 (0x00000800) +#define FLEXCAN_RXIMR0_MI12 (0x00001000) +#define FLEXCAN_RXIMR0_MI13 (0x00002000) +#define FLEXCAN_RXIMR0_MI14 (0x00004000) +#define FLEXCAN_RXIMR0_MI15 (0x00008000) +#define FLEXCAN_RXIMR0_MI16 (0x00010000) +#define FLEXCAN_RXIMR0_MI17 (0x00020000) +#define FLEXCAN_RXIMR0_MI18 (0x00040000) +#define FLEXCAN_RXIMR0_MI19 (0x00080000) +#define FLEXCAN_RXIMR0_MI20 (0x00100000) +#define FLEXCAN_RXIMR0_MI21 (0x00200000) +#define FLEXCAN_RXIMR0_MI22 (0x00400000) +#define FLEXCAN_RXIMR0_MI23 (0x00800000) +#define FLEXCAN_RXIMR0_MI24 (0x01000000) +#define FLEXCAN_RXIMR0_MI25 (0x02000000) +#define FLEXCAN_RXIMR0_MI26 (0x04000000) +#define FLEXCAN_RXIMR0_MI27 (0x08000000) +#define FLEXCAN_RXIMR0_MI28 (0x10000000) +#define FLEXCAN_RXIMR0_MI29 (0x20000000) +#define FLEXCAN_RXIMR0_MI30 (0x40000000) +#define FLEXCAN_RXIMR0_MI31 (0x80000000) + +/* Bit definitions and macros for FLEXCAN_RXIMR1 */ +#define FLEXCAN_RXIMR1_MI0 (0x00000001) +#define FLEXCAN_RXIMR1_MI1 (0x00000002) +#define FLEXCAN_RXIMR1_MI2 (0x00000004) +#define FLEXCAN_RXIMR1_MI3 (0x00000008) +#define FLEXCAN_RXIMR1_MI4 (0x00000010) +#define FLEXCAN_RXIMR1_MI5 (0x00000020) +#define FLEXCAN_RXIMR1_MI6 (0x00000040) +#define FLEXCAN_RXIMR1_MI7 (0x00000080) +#define FLEXCAN_RXIMR1_MI8 (0x00000100) +#define FLEXCAN_RXIMR1_MI9 (0x00000200) +#define FLEXCAN_RXIMR1_MI10 (0x00000400) +#define FLEXCAN_RXIMR1_MI11 (0x00000800) +#define FLEXCAN_RXIMR1_MI12 (0x00001000) +#define FLEXCAN_RXIMR1_MI13 (0x00002000) +#define FLEXCAN_RXIMR1_MI14 (0x00004000) +#define FLEXCAN_RXIMR1_MI15 (0x00008000) +#define FLEXCAN_RXIMR1_MI16 (0x00010000) +#define FLEXCAN_RXIMR1_MI17 (0x00020000) +#define FLEXCAN_RXIMR1_MI18 (0x00040000) +#define FLEXCAN_RXIMR1_MI19 (0x00080000) +#define FLEXCAN_RXIMR1_MI20 (0x00100000) +#define FLEXCAN_RXIMR1_MI21 (0x00200000) +#define FLEXCAN_RXIMR1_MI22 (0x00400000) +#define FLEXCAN_RXIMR1_MI23 (0x00800000) +#define FLEXCAN_RXIMR1_MI24 (0x01000000) +#define FLEXCAN_RXIMR1_MI25 (0x02000000) +#define FLEXCAN_RXIMR1_MI26 (0x04000000) +#define FLEXCAN_RXIMR1_MI27 (0x08000000) +#define FLEXCAN_RXIMR1_MI28 (0x10000000) +#define FLEXCAN_RXIMR1_MI29 (0x20000000) +#define FLEXCAN_RXIMR1_MI30 (0x40000000) +#define FLEXCAN_RXIMR1_MI31 (0x80000000) + +/* Bit definitions and macros for FLEXCAN_RXIMR2 */ +#define FLEXCAN_RXIMR2_MI0 (0x00000001) +#define FLEXCAN_RXIMR2_MI1 (0x00000002) +#define FLEXCAN_RXIMR2_MI2 (0x00000004) +#define FLEXCAN_RXIMR2_MI3 (0x00000008) +#define FLEXCAN_RXIMR2_MI4 (0x00000010) +#define FLEXCAN_RXIMR2_MI5 (0x00000020) +#define FLEXCAN_RXIMR2_MI6 (0x00000040) +#define FLEXCAN_RXIMR2_MI7 (0x00000080) +#define FLEXCAN_RXIMR2_MI8 (0x00000100) +#define FLEXCAN_RXIMR2_MI9 (0x00000200) +#define FLEXCAN_RXIMR2_MI10 (0x00000400) +#define FLEXCAN_RXIMR2_MI11 (0x00000800) +#define FLEXCAN_RXIMR2_MI12 (0x00001000) +#define FLEXCAN_RXIMR2_MI13 (0x00002000) +#define FLEXCAN_RXIMR2_MI14 (0x00004000) +#define FLEXCAN_RXIMR2_MI15 (0x00008000) +#define FLEXCAN_RXIMR2_MI16 (0x00010000) +#define FLEXCAN_RXIMR2_MI17 (0x00020000) +#define FLEXCAN_RXIMR2_MI18 (0x00040000) +#define FLEXCAN_RXIMR2_MI19 (0x00080000) +#define FLEXCAN_RXIMR2_MI20 (0x00100000) +#define FLEXCAN_RXIMR2_MI21 (0x00200000) +#define FLEXCAN_RXIMR2_MI22 (0x00400000) +#define FLEXCAN_RXIMR2_MI23 (0x00800000) +#define FLEXCAN_RXIMR2_MI24 (0x01000000) +#define FLEXCAN_RXIMR2_MI25 (0x02000000) +#define FLEXCAN_RXIMR2_MI26 (0x04000000) +#define FLEXCAN_RXIMR2_MI27 (0x08000000) +#define FLEXCAN_RXIMR2_MI28 (0x10000000) +#define FLEXCAN_RXIMR2_MI29 (0x20000000) +#define FLEXCAN_RXIMR2_MI30 (0x40000000) +#define FLEXCAN_RXIMR2_MI31 (0x80000000) + +/* Bit definitions and macros for FLEXCAN_RXIMR3 */ +#define FLEXCAN_RXIMR3_MI0 (0x00000001) +#define FLEXCAN_RXIMR3_MI1 (0x00000002) +#define FLEXCAN_RXIMR3_MI2 (0x00000004) +#define FLEXCAN_RXIMR3_MI3 (0x00000008) +#define FLEXCAN_RXIMR3_MI4 (0x00000010) +#define FLEXCAN_RXIMR3_MI5 (0x00000020) +#define FLEXCAN_RXIMR3_MI6 (0x00000040) +#define FLEXCAN_RXIMR3_MI7 (0x00000080) +#define FLEXCAN_RXIMR3_MI8 (0x00000100) +#define FLEXCAN_RXIMR3_MI9 (0x00000200) +#define FLEXCAN_RXIMR3_MI10 (0x00000400) +#define FLEXCAN_RXIMR3_MI11 (0x00000800) +#define FLEXCAN_RXIMR3_MI12 (0x00001000) +#define FLEXCAN_RXIMR3_MI13 (0x00002000) +#define FLEXCAN_RXIMR3_MI14 (0x00004000) +#define FLEXCAN_RXIMR3_MI15 (0x00008000) +#define FLEXCAN_RXIMR3_MI16 (0x00010000) +#define FLEXCAN_RXIMR3_MI17 (0x00020000) +#define FLEXCAN_RXIMR3_MI18 (0x00040000) +#define FLEXCAN_RXIMR3_MI19 (0x00080000) +#define FLEXCAN_RXIMR3_MI20 (0x00100000) +#define FLEXCAN_RXIMR3_MI21 (0x00200000) +#define FLEXCAN_RXIMR3_MI22 (0x00400000) +#define FLEXCAN_RXIMR3_MI23 (0x00800000) +#define FLEXCAN_RXIMR3_MI24 (0x01000000) +#define FLEXCAN_RXIMR3_MI25 (0x02000000) +#define FLEXCAN_RXIMR3_MI26 (0x04000000) +#define FLEXCAN_RXIMR3_MI27 (0x08000000) +#define FLEXCAN_RXIMR3_MI28 (0x10000000) +#define FLEXCAN_RXIMR3_MI29 (0x20000000) +#define FLEXCAN_RXIMR3_MI30 (0x40000000) +#define FLEXCAN_RXIMR3_MI31 (0x80000000) + +/* Bit definitions and macros for FLEXCAN_RXIMR4 */ +#define FLEXCAN_RXIMR4_MI0 (0x00000001) +#define FLEXCAN_RXIMR4_MI1 (0x00000002) +#define FLEXCAN_RXIMR4_MI2 (0x00000004) +#define FLEXCAN_RXIMR4_MI3 (0x00000008) +#define FLEXCAN_RXIMR4_MI4 (0x00000010) +#define FLEXCAN_RXIMR4_MI5 (0x00000020) +#define FLEXCAN_RXIMR4_MI6 (0x00000040) +#define FLEXCAN_RXIMR4_MI7 (0x00000080) +#define FLEXCAN_RXIMR4_MI8 (0x00000100) +#define FLEXCAN_RXIMR4_MI9 (0x00000200) +#define FLEXCAN_RXIMR4_MI10 (0x00000400) +#define FLEXCAN_RXIMR4_MI11 (0x00000800) +#define FLEXCAN_RXIMR4_MI12 (0x00001000) +#define FLEXCAN_RXIMR4_MI13 (0x00002000) +#define FLEXCAN_RXIMR4_MI14 (0x00004000) +#define FLEXCAN_RXIMR4_MI15 (0x00008000) +#define FLEXCAN_RXIMR4_MI16 (0x00010000) +#define FLEXCAN_RXIMR4_MI17 (0x00020000) +#define FLEXCAN_RXIMR4_MI18 (0x00040000) +#define FLEXCAN_RXIMR4_MI19 (0x00080000) +#define FLEXCAN_RXIMR4_MI20 (0x00100000) +#define FLEXCAN_RXIMR4_MI21 (0x00200000) +#define FLEXCAN_RXIMR4_MI22 (0x00400000) +#define FLEXCAN_RXIMR4_MI23 (0x00800000) +#define FLEXCAN_RXIMR4_MI24 (0x01000000) +#define FLEXCAN_RXIMR4_MI25 (0x02000000) +#define FLEXCAN_RXIMR4_MI26 (0x04000000) +#define FLEXCAN_RXIMR4_MI27 (0x08000000) +#define FLEXCAN_RXIMR4_MI28 (0x10000000) +#define FLEXCAN_RXIMR4_MI29 (0x20000000) +#define FLEXCAN_RXIMR4_MI30 (0x40000000) +#define FLEXCAN_RXIMR4_MI31 (0x80000000) + +/* Bit definitions and macros for FLEXCAN_RXIMR5 */ +#define FLEXCAN_RXIMR5_MI0 (0x00000001) +#define FLEXCAN_RXIMR5_MI1 (0x00000002) +#define FLEXCAN_RXIMR5_MI2 (0x00000004) +#define FLEXCAN_RXIMR5_MI3 (0x00000008) +#define FLEXCAN_RXIMR5_MI4 (0x00000010) +#define FLEXCAN_RXIMR5_MI5 (0x00000020) +#define FLEXCAN_RXIMR5_MI6 (0x00000040) +#define FLEXCAN_RXIMR5_MI7 (0x00000080) +#define FLEXCAN_RXIMR5_MI8 (0x00000100) +#define FLEXCAN_RXIMR5_MI9 (0x00000200) +#define FLEXCAN_RXIMR5_MI10 (0x00000400) +#define FLEXCAN_RXIMR5_MI11 (0x00000800) +#define FLEXCAN_RXIMR5_MI12 (0x00001000) +#define FLEXCAN_RXIMR5_MI13 (0x00002000) +#define FLEXCAN_RXIMR5_MI14 (0x00004000) +#define FLEXCAN_RXIMR5_MI15 (0x00008000) +#define FLEXCAN_RXIMR5_MI16 (0x00010000) +#define FLEXCAN_RXIMR5_MI17 (0x00020000) +#define FLEXCAN_RXIMR5_MI18 (0x00040000) +#define FLEXCAN_RXIMR5_MI19 (0x00080000) +#define FLEXCAN_RXIMR5_MI20 (0x00100000) +#define FLEXCAN_RXIMR5_MI21 (0x00200000) +#define FLEXCAN_RXIMR5_MI22 (0x00400000) +#define FLEXCAN_RXIMR5_MI23 (0x00800000) +#define FLEXCAN_RXIMR5_MI24 (0x01000000) +#define FLEXCAN_RXIMR5_MI25 (0x02000000) +#define FLEXCAN_RXIMR5_MI26 (0x04000000) +#define FLEXCAN_RXIMR5_MI27 (0x08000000) +#define FLEXCAN_RXIMR5_MI28 (0x10000000) +#define FLEXCAN_RXIMR5_MI29 (0x20000000) +#define FLEXCAN_RXIMR5_MI30 (0x40000000) +#define FLEXCAN_RXIMR5_MI31 (0x80000000) + +/* Bit definitions and macros for FLEXCAN_RXIMR6 */ +#define FLEXCAN_RXIMR6_MI0 (0x00000001) +#define FLEXCAN_RXIMR6_MI1 (0x00000002) +#define FLEXCAN_RXIMR6_MI2 (0x00000004) +#define FLEXCAN_RXIMR6_MI3 (0x00000008) +#define FLEXCAN_RXIMR6_MI4 (0x00000010) +#define FLEXCAN_RXIMR6_MI5 (0x00000020) +#define FLEXCAN_RXIMR6_MI6 (0x00000040) +#define FLEXCAN_RXIMR6_MI7 (0x00000080) +#define FLEXCAN_RXIMR6_MI8 (0x00000100) +#define FLEXCAN_RXIMR6_MI9 (0x00000200) +#define FLEXCAN_RXIMR6_MI10 (0x00000400) +#define FLEXCAN_RXIMR6_MI11 (0x00000800) +#define FLEXCAN_RXIMR6_MI12 (0x00001000) +#define FLEXCAN_RXIMR6_MI13 (0x00002000) +#define FLEXCAN_RXIMR6_MI14 (0x00004000) +#define FLEXCAN_RXIMR6_MI15 (0x00008000) +#define FLEXCAN_RXIMR6_MI16 (0x00010000) +#define FLEXCAN_RXIMR6_MI17 (0x00020000) +#define FLEXCAN_RXIMR6_MI18 (0x00040000) +#define FLEXCAN_RXIMR6_MI19 (0x00080000) +#define FLEXCAN_RXIMR6_MI20 (0x00100000) +#define FLEXCAN_RXIMR6_MI21 (0x00200000) +#define FLEXCAN_RXIMR6_MI22 (0x00400000) +#define FLEXCAN_RXIMR6_MI23 (0x00800000) +#define FLEXCAN_RXIMR6_MI24 (0x01000000) +#define FLEXCAN_RXIMR6_MI25 (0x02000000) +#define FLEXCAN_RXIMR6_MI26 (0x04000000) +#define FLEXCAN_RXIMR6_MI27 (0x08000000) +#define FLEXCAN_RXIMR6_MI28 (0x10000000) +#define FLEXCAN_RXIMR6_MI29 (0x20000000) +#define FLEXCAN_RXIMR6_MI30 (0x40000000) +#define FLEXCAN_RXIMR6_MI31 (0x80000000) + +/* Bit definitions and macros for FLEXCAN_RXIMR7 */ +#define FLEXCAN_RXIMR7_MI0 (0x00000001) +#define FLEXCAN_RXIMR7_MI1 (0x00000002) +#define FLEXCAN_RXIMR7_MI2 (0x00000004) +#define FLEXCAN_RXIMR7_MI3 (0x00000008) +#define FLEXCAN_RXIMR7_MI4 (0x00000010) +#define FLEXCAN_RXIMR7_MI5 (0x00000020) +#define FLEXCAN_RXIMR7_MI6 (0x00000040) +#define FLEXCAN_RXIMR7_MI7 (0x00000080) +#define FLEXCAN_RXIMR7_MI8 (0x00000100) +#define FLEXCAN_RXIMR7_MI9 (0x00000200) +#define FLEXCAN_RXIMR7_MI10 (0x00000400) +#define FLEXCAN_RXIMR7_MI11 (0x00000800) +#define FLEXCAN_RXIMR7_MI12 (0x00001000) +#define FLEXCAN_RXIMR7_MI13 (0x00002000) +#define FLEXCAN_RXIMR7_MI14 (0x00004000) +#define FLEXCAN_RXIMR7_MI15 (0x00008000) +#define FLEXCAN_RXIMR7_MI16 (0x00010000) +#define FLEXCAN_RXIMR7_MI17 (0x00020000) +#define FLEXCAN_RXIMR7_MI18 (0x00040000) +#define FLEXCAN_RXIMR7_MI19 (0x00080000) +#define FLEXCAN_RXIMR7_MI20 (0x00100000) +#define FLEXCAN_RXIMR7_MI21 (0x00200000) +#define FLEXCAN_RXIMR7_MI22 (0x00400000) +#define FLEXCAN_RXIMR7_MI23 (0x00800000) +#define FLEXCAN_RXIMR7_MI24 (0x01000000) +#define FLEXCAN_RXIMR7_MI25 (0x02000000) +#define FLEXCAN_RXIMR7_MI26 (0x04000000) +#define FLEXCAN_RXIMR7_MI27 (0x08000000) +#define FLEXCAN_RXIMR7_MI28 (0x10000000) +#define FLEXCAN_RXIMR7_MI29 (0x20000000) +#define FLEXCAN_RXIMR7_MI30 (0x40000000) +#define FLEXCAN_RXIMR7_MI31 (0x80000000) + +/* Bit definitions and macros for FLEXCAN_RXIMR8 */ +#define FLEXCAN_RXIMR8_MI0 (0x00000001) +#define FLEXCAN_RXIMR8_MI1 (0x00000002) +#define FLEXCAN_RXIMR8_MI2 (0x00000004) +#define FLEXCAN_RXIMR8_MI3 (0x00000008) +#define FLEXCAN_RXIMR8_MI4 (0x00000010) +#define FLEXCAN_RXIMR8_MI5 (0x00000020) +#define FLEXCAN_RXIMR8_MI6 (0x00000040) +#define FLEXCAN_RXIMR8_MI7 (0x00000080) +#define FLEXCAN_RXIMR8_MI8 (0x00000100) +#define FLEXCAN_RXIMR8_MI9 (0x00000200) +#define FLEXCAN_RXIMR8_MI10 (0x00000400) +#define FLEXCAN_RXIMR8_MI11 (0x00000800) +#define FLEXCAN_RXIMR8_MI12 (0x00001000) +#define FLEXCAN_RXIMR8_MI13 (0x00002000) +#define FLEXCAN_RXIMR8_MI14 (0x00004000) +#define FLEXCAN_RXIMR8_MI15 (0x00008000) +#define FLEXCAN_RXIMR8_MI16 (0x00010000) +#define FLEXCAN_RXIMR8_MI17 (0x00020000) +#define FLEXCAN_RXIMR8_MI18 (0x00040000) +#define FLEXCAN_RXIMR8_MI19 (0x00080000) +#define FLEXCAN_RXIMR8_MI20 (0x00100000) +#define FLEXCAN_RXIMR8_MI21 (0x00200000) +#define FLEXCAN_RXIMR8_MI22 (0x00400000) +#define FLEXCAN_RXIMR8_MI23 (0x00800000) +#define FLEXCAN_RXIMR8_MI24 (0x01000000) +#define FLEXCAN_RXIMR8_MI25 (0x02000000) +#define FLEXCAN_RXIMR8_MI26 (0x04000000) +#define FLEXCAN_RXIMR8_MI27 (0x08000000) +#define FLEXCAN_RXIMR8_MI28 (0x10000000) +#define FLEXCAN_RXIMR8_MI29 (0x20000000) +#define FLEXCAN_RXIMR8_MI30 (0x40000000) +#define FLEXCAN_RXIMR8_MI31 (0x80000000) + +/* Bit definitions and macros for FLEXCAN_RXIMR9 */ +#define FLEXCAN_RXIMR9_MI0 (0x00000001) +#define FLEXCAN_RXIMR9_MI1 (0x00000002) +#define FLEXCAN_RXIMR9_MI2 (0x00000004) +#define FLEXCAN_RXIMR9_MI3 (0x00000008) +#define FLEXCAN_RXIMR9_MI4 (0x00000010) +#define FLEXCAN_RXIMR9_MI5 (0x00000020) +#define FLEXCAN_RXIMR9_MI6 (0x00000040) +#define FLEXCAN_RXIMR9_MI7 (0x00000080) +#define FLEXCAN_RXIMR9_MI8 (0x00000100) +#define FLEXCAN_RXIMR9_MI9 (0x00000200) +#define FLEXCAN_RXIMR9_MI10 (0x00000400) +#define FLEXCAN_RXIMR9_MI11 (0x00000800) +#define FLEXCAN_RXIMR9_MI12 (0x00001000) +#define FLEXCAN_RXIMR9_MI13 (0x00002000) +#define FLEXCAN_RXIMR9_MI14 (0x00004000) +#define FLEXCAN_RXIMR9_MI15 (0x00008000) +#define FLEXCAN_RXIMR9_MI16 (0x00010000) +#define FLEXCAN_RXIMR9_MI17 (0x00020000) +#define FLEXCAN_RXIMR9_MI18 (0x00040000) +#define FLEXCAN_RXIMR9_MI19 (0x00080000) +#define FLEXCAN_RXIMR9_MI20 (0x00100000) +#define FLEXCAN_RXIMR9_MI21 (0x00200000) +#define FLEXCAN_RXIMR9_MI22 (0x00400000) +#define FLEXCAN_RXIMR9_MI23 (0x00800000) +#define FLEXCAN_RXIMR9_MI24 (0x01000000) +#define FLEXCAN_RXIMR9_MI25 (0x02000000) +#define FLEXCAN_RXIMR9_MI26 (0x04000000) +#define FLEXCAN_RXIMR9_MI27 (0x08000000) +#define FLEXCAN_RXIMR9_MI28 (0x10000000) +#define FLEXCAN_RXIMR9_MI29 (0x20000000) +#define FLEXCAN_RXIMR9_MI30 (0x40000000) +#define FLEXCAN_RXIMR9_MI31 (0x80000000) + +/* Bit definitions and macros for FLEXCAN_RXIMR10 */ +#define FLEXCAN_RXIMR10_MI0 (0x00000001) +#define FLEXCAN_RXIMR10_MI1 (0x00000002) +#define FLEXCAN_RXIMR10_MI2 (0x00000004) +#define FLEXCAN_RXIMR10_MI3 (0x00000008) +#define FLEXCAN_RXIMR10_MI4 (0x00000010) +#define FLEXCAN_RXIMR10_MI5 (0x00000020) +#define FLEXCAN_RXIMR10_MI6 (0x00000040) +#define FLEXCAN_RXIMR10_MI7 (0x00000080) +#define FLEXCAN_RXIMR10_MI8 (0x00000100) +#define FLEXCAN_RXIMR10_MI9 (0x00000200) +#define FLEXCAN_RXIMR10_MI10 (0x00000400) +#define FLEXCAN_RXIMR10_MI11 (0x00000800) +#define FLEXCAN_RXIMR10_MI12 (0x00001000) +#define FLEXCAN_RXIMR10_MI13 (0x00002000) +#define FLEXCAN_RXIMR10_MI14 (0x00004000) +#define FLEXCAN_RXIMR10_MI15 (0x00008000) +#define FLEXCAN_RXIMR10_MI16 (0x00010000) +#define FLEXCAN_RXIMR10_MI17 (0x00020000) +#define FLEXCAN_RXIMR10_MI18 (0x00040000) +#define FLEXCAN_RXIMR10_MI19 (0x00080000) +#define FLEXCAN_RXIMR10_MI20 (0x00100000) +#define FLEXCAN_RXIMR10_MI21 (0x00200000) +#define FLEXCAN_RXIMR10_MI22 (0x00400000) +#define FLEXCAN_RXIMR10_MI23 (0x00800000) +#define FLEXCAN_RXIMR10_MI24 (0x01000000) +#define FLEXCAN_RXIMR10_MI25 (0x02000000) +#define FLEXCAN_RXIMR10_MI26 (0x04000000) +#define FLEXCAN_RXIMR10_MI27 (0x08000000) +#define FLEXCAN_RXIMR10_MI28 (0x10000000) +#define FLEXCAN_RXIMR10_MI29 (0x20000000) +#define FLEXCAN_RXIMR10_MI30 (0x40000000) +#define FLEXCAN_RXIMR10_MI31 (0x80000000) + +/* Bit definitions and macros for FLEXCAN_RXIMR11 */ +#define FLEXCAN_RXIMR11_MI0 (0x00000001) +#define FLEXCAN_RXIMR11_MI1 (0x00000002) +#define FLEXCAN_RXIMR11_MI2 (0x00000004) +#define FLEXCAN_RXIMR11_MI3 (0x00000008) +#define FLEXCAN_RXIMR11_MI4 (0x00000010) +#define FLEXCAN_RXIMR11_MI5 (0x00000020) +#define FLEXCAN_RXIMR11_MI6 (0x00000040) +#define FLEXCAN_RXIMR11_MI7 (0x00000080) +#define FLEXCAN_RXIMR11_MI8 (0x00000100) +#define FLEXCAN_RXIMR11_MI9 (0x00000200) +#define FLEXCAN_RXIMR11_MI10 (0x00000400) +#define FLEXCAN_RXIMR11_MI11 (0x00000800) +#define FLEXCAN_RXIMR11_MI12 (0x00001000) +#define FLEXCAN_RXIMR11_MI13 (0x00002000) +#define FLEXCAN_RXIMR11_MI14 (0x00004000) +#define FLEXCAN_RXIMR11_MI15 (0x00008000) +#define FLEXCAN_RXIMR11_MI16 (0x00010000) +#define FLEXCAN_RXIMR11_MI17 (0x00020000) +#define FLEXCAN_RXIMR11_MI18 (0x00040000) +#define FLEXCAN_RXIMR11_MI19 (0x00080000) +#define FLEXCAN_RXIMR11_MI20 (0x00100000) +#define FLEXCAN_RXIMR11_MI21 (0x00200000) +#define FLEXCAN_RXIMR11_MI22 (0x00400000) +#define FLEXCAN_RXIMR11_MI23 (0x00800000) +#define FLEXCAN_RXIMR11_MI24 (0x01000000) +#define FLEXCAN_RXIMR11_MI25 (0x02000000) +#define FLEXCAN_RXIMR11_MI26 (0x04000000) +#define FLEXCAN_RXIMR11_MI27 (0x08000000) +#define FLEXCAN_RXIMR11_MI28 (0x10000000) +#define FLEXCAN_RXIMR11_MI29 (0x20000000) +#define FLEXCAN_RXIMR11_MI30 (0x40000000) +#define FLEXCAN_RXIMR11_MI31 (0x80000000) + +/* Bit definitions and macros for FLEXCAN_RXIMR12 */ +#define FLEXCAN_RXIMR12_MI0 (0x00000001) +#define FLEXCAN_RXIMR12_MI1 (0x00000002) +#define FLEXCAN_RXIMR12_MI2 (0x00000004) +#define FLEXCAN_RXIMR12_MI3 (0x00000008) +#define FLEXCAN_RXIMR12_MI4 (0x00000010) +#define FLEXCAN_RXIMR12_MI5 (0x00000020) +#define FLEXCAN_RXIMR12_MI6 (0x00000040) +#define FLEXCAN_RXIMR12_MI7 (0x00000080) +#define FLEXCAN_RXIMR12_MI8 (0x00000100) +#define FLEXCAN_RXIMR12_MI9 (0x00000200) +#define FLEXCAN_RXIMR12_MI10 (0x00000400) +#define FLEXCAN_RXIMR12_MI11 (0x00000800) +#define FLEXCAN_RXIMR12_MI12 (0x00001000) +#define FLEXCAN_RXIMR12_MI13 (0x00002000) +#define FLEXCAN_RXIMR12_MI14 (0x00004000) +#define FLEXCAN_RXIMR12_MI15 (0x00008000) +#define FLEXCAN_RXIMR12_MI16 (0x00010000) +#define FLEXCAN_RXIMR12_MI17 (0x00020000) +#define FLEXCAN_RXIMR12_MI18 (0x00040000) +#define FLEXCAN_RXIMR12_MI19 (0x00080000) +#define FLEXCAN_RXIMR12_MI20 (0x00100000) +#define FLEXCAN_RXIMR12_MI21 (0x00200000) +#define FLEXCAN_RXIMR12_MI22 (0x00400000) +#define FLEXCAN_RXIMR12_MI23 (0x00800000) +#define FLEXCAN_RXIMR12_MI24 (0x01000000) +#define FLEXCAN_RXIMR12_MI25 (0x02000000) +#define FLEXCAN_RXIMR12_MI26 (0x04000000) +#define FLEXCAN_RXIMR12_MI27 (0x08000000) +#define FLEXCAN_RXIMR12_MI28 (0x10000000) +#define FLEXCAN_RXIMR12_MI29 (0x20000000) +#define FLEXCAN_RXIMR12_MI30 (0x40000000) +#define FLEXCAN_RXIMR12_MI31 (0x80000000) + +/* Bit definitions and macros for FLEXCAN_RXIMR13 */ +#define FLEXCAN_RXIMR13_MI0 (0x00000001) +#define FLEXCAN_RXIMR13_MI1 (0x00000002) +#define FLEXCAN_RXIMR13_MI2 (0x00000004) +#define FLEXCAN_RXIMR13_MI3 (0x00000008) +#define FLEXCAN_RXIMR13_MI4 (0x00000010) +#define FLEXCAN_RXIMR13_MI5 (0x00000020) +#define FLEXCAN_RXIMR13_MI6 (0x00000040) +#define FLEXCAN_RXIMR13_MI7 (0x00000080) +#define FLEXCAN_RXIMR13_MI8 (0x00000100) +#define FLEXCAN_RXIMR13_MI9 (0x00000200) +#define FLEXCAN_RXIMR13_MI10 (0x00000400) +#define FLEXCAN_RXIMR13_MI11 (0x00000800) +#define FLEXCAN_RXIMR13_MI12 (0x00001000) +#define FLEXCAN_RXIMR13_MI13 (0x00002000) +#define FLEXCAN_RXIMR13_MI14 (0x00004000) +#define FLEXCAN_RXIMR13_MI15 (0x00008000) +#define FLEXCAN_RXIMR13_MI16 (0x00010000) +#define FLEXCAN_RXIMR13_MI17 (0x00020000) +#define FLEXCAN_RXIMR13_MI18 (0x00040000) +#define FLEXCAN_RXIMR13_MI19 (0x00080000) +#define FLEXCAN_RXIMR13_MI20 (0x00100000) +#define FLEXCAN_RXIMR13_MI21 (0x00200000) +#define FLEXCAN_RXIMR13_MI22 (0x00400000) +#define FLEXCAN_RXIMR13_MI23 (0x00800000) +#define FLEXCAN_RXIMR13_MI24 (0x01000000) +#define FLEXCAN_RXIMR13_MI25 (0x02000000) +#define FLEXCAN_RXIMR13_MI26 (0x04000000) +#define FLEXCAN_RXIMR13_MI27 (0x08000000) +#define FLEXCAN_RXIMR13_MI28 (0x10000000) +#define FLEXCAN_RXIMR13_MI29 (0x20000000) +#define FLEXCAN_RXIMR13_MI30 (0x40000000) +#define FLEXCAN_RXIMR13_MI31 (0x80000000) + +/* Bit definitions and macros for FLEXCAN_RXIMR14 */ +#define FLEXCAN_RXIMR14_MI0 (0x00000001) +#define FLEXCAN_RXIMR14_MI1 (0x00000002) +#define FLEXCAN_RXIMR14_MI2 (0x00000004) +#define FLEXCAN_RXIMR14_MI3 (0x00000008) +#define FLEXCAN_RXIMR14_MI4 (0x00000010) +#define FLEXCAN_RXIMR14_MI5 (0x00000020) +#define FLEXCAN_RXIMR14_MI6 (0x00000040) +#define FLEXCAN_RXIMR14_MI7 (0x00000080) +#define FLEXCAN_RXIMR14_MI8 (0x00000100) +#define FLEXCAN_RXIMR14_MI9 (0x00000200) +#define FLEXCAN_RXIMR14_MI10 (0x00000400) +#define FLEXCAN_RXIMR14_MI11 (0x00000800) +#define FLEXCAN_RXIMR14_MI12 (0x00001000) +#define FLEXCAN_RXIMR14_MI13 (0x00002000) +#define FLEXCAN_RXIMR14_MI14 (0x00004000) +#define FLEXCAN_RXIMR14_MI15 (0x00008000) +#define FLEXCAN_RXIMR14_MI16 (0x00010000) +#define FLEXCAN_RXIMR14_MI17 (0x00020000) +#define FLEXCAN_RXIMR14_MI18 (0x00040000) +#define FLEXCAN_RXIMR14_MI19 (0x00080000) +#define FLEXCAN_RXIMR14_MI20 (0x00100000) +#define FLEXCAN_RXIMR14_MI21 (0x00200000) +#define FLEXCAN_RXIMR14_MI22 (0x00400000) +#define FLEXCAN_RXIMR14_MI23 (0x00800000) +#define FLEXCAN_RXIMR14_MI24 (0x01000000) +#define FLEXCAN_RXIMR14_MI25 (0x02000000) +#define FLEXCAN_RXIMR14_MI26 (0x04000000) +#define FLEXCAN_RXIMR14_MI27 (0x08000000) +#define FLEXCAN_RXIMR14_MI28 (0x10000000) +#define FLEXCAN_RXIMR14_MI29 (0x20000000) +#define FLEXCAN_RXIMR14_MI30 (0x40000000) +#define FLEXCAN_RXIMR14_MI31 (0x80000000) + +/* Bit definitions and macros for FLEXCAN_RXIMR15 */ +#define FLEXCAN_RXIMR15_MI0 (0x00000001) +#define FLEXCAN_RXIMR15_MI1 (0x00000002) +#define FLEXCAN_RXIMR15_MI2 (0x00000004) +#define FLEXCAN_RXIMR15_MI3 (0x00000008) +#define FLEXCAN_RXIMR15_MI4 (0x00000010) +#define FLEXCAN_RXIMR15_MI5 (0x00000020) +#define FLEXCAN_RXIMR15_MI6 (0x00000040) +#define FLEXCAN_RXIMR15_MI7 (0x00000080) +#define FLEXCAN_RXIMR15_MI8 (0x00000100) +#define FLEXCAN_RXIMR15_MI9 (0x00000200) +#define FLEXCAN_RXIMR15_MI10 (0x00000400) +#define FLEXCAN_RXIMR15_MI11 (0x00000800) +#define FLEXCAN_RXIMR15_MI12 (0x00001000) +#define FLEXCAN_RXIMR15_MI13 (0x00002000) +#define FLEXCAN_RXIMR15_MI14 (0x00004000) +#define FLEXCAN_RXIMR15_MI15 (0x00008000) +#define FLEXCAN_RXIMR15_MI16 (0x00010000) +#define FLEXCAN_RXIMR15_MI17 (0x00020000) +#define FLEXCAN_RXIMR15_MI18 (0x00040000) +#define FLEXCAN_RXIMR15_MI19 (0x00080000) +#define FLEXCAN_RXIMR15_MI20 (0x00100000) +#define FLEXCAN_RXIMR15_MI21 (0x00200000) +#define FLEXCAN_RXIMR15_MI22 (0x00400000) +#define FLEXCAN_RXIMR15_MI23 (0x00800000) +#define FLEXCAN_RXIMR15_MI24 (0x01000000) +#define FLEXCAN_RXIMR15_MI25 (0x02000000) +#define FLEXCAN_RXIMR15_MI26 (0x04000000) +#define FLEXCAN_RXIMR15_MI27 (0x08000000) +#define FLEXCAN_RXIMR15_MI28 (0x10000000) +#define FLEXCAN_RXIMR15_MI29 (0x20000000) +#define FLEXCAN_RXIMR15_MI30 (0x40000000) +#define FLEXCAN_RXIMR15_MI31 (0x80000000) + +/* Bit definitions for CRC register */ +#define FLEXCAN_CRCR_MBCRC_BIT_NO (16) +#define FLEXCAN_CRCR_MBCRC_MASK (0x007F0000) +#define FLEXCAN_CRCR_CRC_BIT_NO (0) +#define FLEXCAN_CRCR_CRC_MASK (0x00007FFF) + +/* Bit definition for Individual Matching Elements Update Register (IMEUR) */ +#define FLEXCAN_IMEUR_IMEUP_MASK (0x0000007F) +#define FLEXCAN_IMEUR_IMEUP_BIT_NO (0) +#define FLEXCAN_IMEUR_IMEUREQ_MASK (0x00000100) +#define FLEXCAN_IMEUR_IMEUACK_MASK (0x00000200) +#define FLEXCAN_Set_IMEUP(imeur, imeup) imeur = (imeur & ~(FLEXCAN_IMEUR_IMEUP_MASK)) | (imeup & FLEXCAN_IMEUR_IMEUP_MASK) +#define FLEXCAN_Get_IMEUP(imeur) (imeur & FLEXCAN_IMEUR_IMEUP_MASK) + +/* Bit definition for Lost Rx Frames Register (LRFR) + */ +#define FLEXCAN_LRFR_LOSTRLP_MASK (0x007F0000) +#define FLEXCAN_LRFR_LFIFOMTC_MASK (0x00008000) +#define FLEXCAN_LRFR_LOSTRMP_MASK (0x000001FF) +#define FLEXCAN_LRFR_LOSTRLP_BIT_NO (16) +#define FLEXCAN_LRFR_LFIFOMTC_BIT_NO (15) +#define FLEXCAN_LRFR_LOSTRMP_BIT_NO (0) +#define FLEXCAN_Get_LostMBLocked(lrfr) ((lrfr & FLEXCAN_LRFR_LOSTRLP_MASK) >> (FLEXCAN_LRFR_LOSTRLP_BIT_NO)) +#define FLEXCAN_Get_LostMBUpdated(lrfr) ((lrfr & FLEXCAN_LRFR_LOSTRMP_MASK)) + +/* Bit definition for Memory Error Control Register */ +#define FLEXCAN_MECR_NCEFAFRZ_MASK (0x00000080) +#define FLEXCAN_MECR_RERRDIS_MASK (0x00000100) +#define FLEXCAN_MECR_EXTERRIE_MAKS (0x00002000) +#define FLEXCAN_MECR_FAERRIE_MAKS (0x00004000) +#define FLEXCAN_MECR_HAERRIE_MAKS (0x00008000) +#define FLEXCAN_MECR_CEI_MSK_MAKS (0x00010000) +#define FLEXCAN_MECR_FANCEI_MSK_MAKS (0x00040000) +#define FLEXCAN_MECR_HANCEI_MSK_MAKS (0x00080000) +#define FLEXCAN_MECR_ECRWRDIS_MSK_MAKS (0x80000000) + +/* Bit definition for Error Report Address Register (RERRAR) */ +#define FLEXCAN_RERRAR_NCE_MASK (0x01000000) +#define FLEXCAN_RERRAR_SAID_MASK (0x00070000) +#define FLEXCAN_ERRADDR_MASK (0x00003FFF) + +/* Bit definition for Error Report Syndrome Register (RERRSYNR) */ +#define FLEXCAN_RERRSYNR_BE3_MASK (0x80000000) +#define FLEXCAN_RERRSYNR_SYND3_MASK (0x1F000000) +#define FLEXCAN_RERRSYNR_SYND3_BIT_NO (24) +#define FLEXCAN_RERRSYNR_BE2_MASK (0x00800000) +#define FLEXCAN_RERRSYNR_SYND2_MASK (0x001F0000) +#define FLEXCAN_RERRSYNR_SYND2_BIT_NO (16) +#define FLEXCAN_RERRSYNR_BE1_MASK (0x00008000) +#define FLEXCAN_RERRSYNR_SYND1_MASK (0x00001F00) +#define FLEXCAN_RERRSYNR_SYND1_BIT_NO (8) +#define FLEXCAN_RERRSYNR_BE0_MASK (0x00000080) +#define FLEXCAN_RERRSYNR_SYND0_MASK (0x0000001F) +#define FLEXCAN_RERRSYNR_SYND0_BIT_NO (0) + +#define FLEXCAN_RERRSYNR_check_BEn_Bit(errsynr, n) (errsynr & FLEXCAN_RERRSYNR_BE##n##_MASK) +#define FLEXCAN_RERRSYNR_get_SYNDn(errsynr, n) (errsynr & FLEXCAN_RERRSYNR_SYND##n##_MASK) +#define FLEXCAN_RERRSYNR_check_SYNDn_Bit(errsynr, n) ((errsynr & FLEXCAN_RERRSYNR_SYND##n##_MASK) >> FLEXCAN_RERRSYNR_SYND##n##_BIT_NO) + +/* Bit definition for Error Status Register (ERRSR) */ +#define FLEXCAN_ERRSR_CEIOF_MASK (0x00000001) +#define FLEXCAN_ERRSR_FANCEIOF_MASK (0x00000004) +#define FLEXCAN_ERRSR_HANCEIOF_MASK (0x00000008) +#define FLEXCAN_ERRSR_CEIF_MASK (0x00010000) +#define FLEXCAN_ERRSR_FANCEIF_MASK (0x00040000) +#define FLEXCAN_ERRSR_HANCEIF_MASK (0x00080000) + +/********************************************************************/ +#endif // __KINETIS_FLEXCAN_H diff --git a/src/isobus.hpp b/src/isobus.hpp new file mode 100644 index 0000000..00817d2 --- /dev/null +++ b/src/isobus.hpp @@ -0,0 +1,61 @@ +/******************************************************************************* +** @file isobus.hpp +** @author Automatic Code Generation +** @date September 09, 2023 at 11:11:41 +** @brief Includes all important files in the AgIsoStack library. +** +** Copyright 2023 The AgIsoStack++ Developers +*******************************************************************************/ + +#ifndef ISOBUS_HPP +#define ISOBUS_HPP + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#endif // ISOBUS_HPP diff --git a/src/isobus_device_descriptor_object_pool.cpp b/src/isobus_device_descriptor_object_pool.cpp new file mode 100644 index 0000000..39f8ec2 --- /dev/null +++ b/src/isobus_device_descriptor_object_pool.cpp @@ -0,0 +1,1054 @@ +//================================================================================================ +/// @file isobus_device_descriptor_object_pool.cpp +/// +/// @brief Implements an interface for creating a Task Controller DDOP. +/// @author Adrian Del Grosso +/// +/// @copyright 2023 Adrian Del Grosso +//================================================================================================ +#include "isobus_device_descriptor_object_pool.hpp" + +#include "can_stack_logger.hpp" +#include "platform_endianness.hpp" +#include "to_string.hpp" + +#include +#include +#include + +namespace isobus +{ + DeviceDescriptorObjectPool::DeviceDescriptorObjectPool(std::uint8_t taskControllerServerVersion) : + taskControllerCompatibilityLevel(taskControllerServerVersion) + { + assert(taskControllerCompatibilityLevel <= MAX_TC_VERSION_SUPPORTED); + } + + bool DeviceDescriptorObjectPool::add_device(std::string deviceDesignator, + std::string deviceSoftwareVersion, + std::string deviceSerialNumber, + std::string deviceStructureLabel, + std::array deviceLocalizationLabel, + std::vector deviceExtendedStructureLabel, + std::uint64_t clientIsoNAME) + { + bool retVal = true; + + for (auto ¤tObject : objectList) + { + if (task_controller_object::ObjectTypes::Device == currentObject->get_object_type()) + { + retVal = false; + break; + } + } + + if (retVal) + { + if ((taskControllerCompatibilityLevel < MAX_TC_VERSION_SUPPORTED) && + (deviceDesignator.size() > task_controller_object::Object::MAX_DESIGNATOR_LEGACY_LENGTH)) + { + CANStackLogger::warn("[DDOP]: Device designator " + + deviceDesignator + + " is greater than the max byte length of 32. Value will be truncated."); + deviceDesignator.resize(task_controller_object::Object::MAX_DESIGNATOR_LEGACY_LENGTH); + } + else if ((taskControllerCompatibilityLevel == MAX_TC_VERSION_SUPPORTED) && + (deviceDesignator.size() > task_controller_object::Object::MAX_DESIGNATOR_LENGTH)) + { + CANStackLogger::warn("[DDOP]: Device designator " + + deviceDesignator + + " is greater than the max byte length of 128. Value will be truncated."); + deviceDesignator.resize(task_controller_object::Object::MAX_DESIGNATOR_LENGTH); + } + else if ((taskControllerCompatibilityLevel == MAX_TC_VERSION_SUPPORTED) && + (deviceDesignator.size() > task_controller_object::Object::MAX_DESIGNATOR_LEGACY_LENGTH)) + { + CANStackLogger::info("[DDOP]: Device designator " + + deviceDesignator + + " byte length is greater than the max character count of 32. " + + "This is only acceptable if you have 32 or fewer UTF-8 characters!" + + " Please verify your DDOP configuration meets this requirement."); + } + + if ((taskControllerCompatibilityLevel < MAX_TC_VERSION_SUPPORTED) && + (deviceSerialNumber.size() > task_controller_object::Object::MAX_DESIGNATOR_LEGACY_LENGTH)) + { + CANStackLogger::warn("[DDOP]: Device serial number " + + deviceSerialNumber + + " is greater than the max byte length of 32. Value will be truncated."); + deviceSerialNumber.resize(task_controller_object::Object::MAX_DESIGNATOR_LEGACY_LENGTH); + } + else if ((taskControllerCompatibilityLevel == MAX_TC_VERSION_SUPPORTED) && + (deviceSerialNumber.size() > task_controller_object::Object::MAX_DESIGNATOR_LENGTH)) + { + CANStackLogger::warn("[DDOP]: Device serial number " + + deviceSerialNumber + + " is greater than the max byte length of 128. Value will be truncated."); + deviceSerialNumber.resize(task_controller_object::Object::MAX_DESIGNATOR_LENGTH); + } + else if ((taskControllerCompatibilityLevel == MAX_TC_VERSION_SUPPORTED) && + (deviceSerialNumber.size() > task_controller_object::Object::MAX_DESIGNATOR_LEGACY_LENGTH)) + { + CANStackLogger::info("[DDOP]: Device serial number " + + deviceSerialNumber + + " byte length is greater than the max character count of 32. " + + "This is only acceptable if you have 32 or fewer UTF-8 characters!" + + " Please verify your DDOP configuration meets this requirement."); + } + + if (deviceStructureLabel.size() > task_controller_object::DeviceObject::MAX_STRUCTURE_AND_LOCALIZATION_LABEL_LENGTH) + { + CANStackLogger::warn("[DDOP]: Device structure label " + + deviceStructureLabel + + " is greater than the max length of 7. Value will be truncated."); + deviceStructureLabel.resize(task_controller_object::DeviceObject::MAX_STRUCTURE_AND_LOCALIZATION_LABEL_LENGTH); + } + if (deviceExtendedStructureLabel.size() > task_controller_object::DeviceObject::MAX_EXTENDED_STRUCTURE_LABEL_LENGTH) + { + CANStackLogger::warn("[DDOP]: Device extended structure label is greater than the max length of 32. Value will be truncated."); + deviceExtendedStructureLabel.resize(task_controller_object::DeviceObject::MAX_EXTENDED_STRUCTURE_LABEL_LENGTH); + } + + if (deviceLocalizationLabel[6] != 0xFF) + { + CANStackLogger::warn("[DDOP]: Device localization label byte 7 must be the reserved value 0xFF. This value will be enforced when DDOP binary is generated."); + } + objectList.emplace_back(new task_controller_object::DeviceObject(deviceDesignator, + deviceSoftwareVersion, + deviceSerialNumber, + deviceStructureLabel, + deviceLocalizationLabel, + deviceExtendedStructureLabel, + clientIsoNAME, + (taskControllerCompatibilityLevel >= 4))); + } + else + { + CANStackLogger::error("[DDOP]: Cannot add more than 1 Device object to a DDOP."); + } + return retVal; + } + + bool DeviceDescriptorObjectPool::add_device_element(std::string deviceElementDesignator, + std::uint16_t deviceElementNumber, + std::uint16_t parentObjectID, + task_controller_object::DeviceElementObject::Type deviceElementType, + std::uint16_t uniqueID) + { + bool retVal = check_object_id_unique(uniqueID); + + if (retVal) + { + // Object will be added + // Check for warnings + if ((taskControllerCompatibilityLevel < MAX_TC_VERSION_SUPPORTED) && + (deviceElementDesignator.size() > task_controller_object::Object::MAX_DESIGNATOR_LEGACY_LENGTH)) + { + CANStackLogger::warn("[DDOP]: Device element designator " + + deviceElementDesignator + + " is greater than the max length of 32. Value will be truncated."); + deviceElementDesignator.resize(task_controller_object::Object::MAX_DESIGNATOR_LEGACY_LENGTH); + } + if ((taskControllerCompatibilityLevel == MAX_TC_VERSION_SUPPORTED) && + (deviceElementDesignator.size() > task_controller_object::Object::MAX_DESIGNATOR_LENGTH)) + { + CANStackLogger::warn("[DDOP]: Device element designator " + + deviceElementDesignator + + " is greater than the max length of 128. Value will be truncated."); + deviceElementDesignator.resize(task_controller_object::Object::MAX_DESIGNATOR_LENGTH); + } + + objectList.emplace_back(new task_controller_object::DeviceElementObject(deviceElementDesignator, + deviceElementNumber, + parentObjectID, + deviceElementType, + uniqueID)); + } + else + { + CANStackLogger::error("[DDOP]: Device element ID " + + isobus::to_string(static_cast(uniqueID)) + + " is not unique. Object will not be added to the DDOP."); + } + return retVal; + } + + bool DeviceDescriptorObjectPool::add_device_process_data(std::string processDataDesignator, + std::uint16_t processDataDDI, + std::uint16_t deviceValuePresentationObjectID, + std::uint8_t processDataProperties, + std::uint8_t processDataTriggerMethods, + std::uint16_t uniqueID) + { + bool retVal = check_object_id_unique(uniqueID); + + if (retVal) + { + // Object will be added + // Check for warnings + if ((processDataProperties & 0x02) && (processDataProperties & 0x04)) + { + CANStackLogger::warn("[DDOP]: Process data object " + + isobus::to_string(static_cast(uniqueID)) + + " has mutually exclusive options 'settable' and 'control source' set."); + } + + if ((taskControllerCompatibilityLevel < MAX_TC_VERSION_SUPPORTED) && + (processDataDesignator.size() > task_controller_object::Object::MAX_DESIGNATOR_LEGACY_LENGTH)) + { + CANStackLogger::warn("[DDOP]: Device process data designator " + + processDataDesignator + + " is greater than the max byte length of 32. Value will be truncated."); + processDataDesignator.resize(task_controller_object::Object::MAX_DESIGNATOR_LEGACY_LENGTH); + } + else if ((taskControllerCompatibilityLevel == MAX_TC_VERSION_SUPPORTED) && + (processDataDesignator.size() > task_controller_object::Object::MAX_DESIGNATOR_LENGTH)) + { + CANStackLogger::warn("[DDOP]: Device process data designator " + + processDataDesignator + + " is greater than the max byte length of 128. Value will be truncated."); + processDataDesignator.resize(task_controller_object::Object::MAX_DESIGNATOR_LENGTH); + } + else if ((taskControllerCompatibilityLevel == MAX_TC_VERSION_SUPPORTED) && + (processDataDesignator.size() > task_controller_object::Object::MAX_DESIGNATOR_LEGACY_LENGTH)) + { + CANStackLogger::info("[DDOP]: Device process data designator " + + processDataDesignator + + " byte length is greater than the max character count of 32. " + + "This is only acceptable if you have 32 or fewer UTF-8 characters!" + + " Please verify your DDOP configuration meets this requirement."); + } + + objectList.emplace_back(new task_controller_object::DeviceProcessDataObject(processDataDesignator, + processDataDDI, + deviceValuePresentationObjectID, + processDataProperties, + processDataTriggerMethods, + uniqueID)); + } + else + { + CANStackLogger::error("[DDOP]: Device process data ID " + + isobus::to_string(static_cast(uniqueID)) + + " is not unique. Object will not be added to the DDOP."); + } + return retVal; + } + + bool DeviceDescriptorObjectPool::add_device_property(std::string propertyDesignator, + std::int32_t propertyValue, + std::uint16_t propertyDDI, + std::uint16_t valuePresentationObject, + std::uint16_t uniqueID) + { + bool retVal = check_object_id_unique(uniqueID); + + if (retVal) + { + // Object will be added + // Check for warnings + if ((taskControllerCompatibilityLevel < MAX_TC_VERSION_SUPPORTED) && + (propertyDesignator.size() > task_controller_object::Object::MAX_DESIGNATOR_LEGACY_LENGTH)) + { + CANStackLogger::warn("[DDOP]: Device property designator " + + propertyDesignator + + " is greater than the max byte length of 32. Value will be truncated."); + propertyDesignator.resize(task_controller_object::Object::MAX_DESIGNATOR_LEGACY_LENGTH); + } + else if ((taskControllerCompatibilityLevel == MAX_TC_VERSION_SUPPORTED) && + (propertyDesignator.size() > task_controller_object::Object::MAX_DESIGNATOR_LENGTH)) + { + CANStackLogger::warn("[DDOP]: Device property designator " + + propertyDesignator + + " is greater than the max byte length of 128. Value will be truncated."); + propertyDesignator.resize(task_controller_object::Object::MAX_DESIGNATOR_LENGTH); + } + else if ((taskControllerCompatibilityLevel == MAX_TC_VERSION_SUPPORTED) && + (propertyDesignator.size() > task_controller_object::Object::MAX_DESIGNATOR_LEGACY_LENGTH)) + { + CANStackLogger::info("[DDOP]: Device property designator " + + propertyDesignator + + " byte length is greater than the max character count of 32. " + + "This is only acceptable if you have 32 or fewer UTF-8 characters!" + + " Please verify your DDOP configuration meets this requirement."); + } + + objectList.emplace_back(new task_controller_object::DevicePropertyObject(propertyDesignator, + propertyValue, + propertyDDI, + valuePresentationObject, + uniqueID)); + } + else + { + CANStackLogger::error("[DDOP]: Device property ID " + + isobus::to_string(static_cast(uniqueID)) + + " is not unique. Object will not be added to the DDOP."); + } + return retVal; + } + + bool DeviceDescriptorObjectPool::add_device_value_presentation(std::string unitDesignator, + std::int32_t offsetValue, + float scaleFactor, + std::uint8_t numberDecimals, + std::uint16_t uniqueID) + { + bool retVal = check_object_id_unique(uniqueID); + + if (retVal) + { + // Object will be added + // Check for warnings + if ((taskControllerCompatibilityLevel < MAX_TC_VERSION_SUPPORTED) && + (unitDesignator.size() > task_controller_object::Object::MAX_DESIGNATOR_LEGACY_LENGTH)) + { + CANStackLogger::warn("[DDOP]: Device value presentation unit designator " + + unitDesignator + + " is greater than the max byte length of 32. Value will be truncated."); + unitDesignator.resize(task_controller_object::Object::MAX_DESIGNATOR_LEGACY_LENGTH); + } + else if ((taskControllerCompatibilityLevel == MAX_TC_VERSION_SUPPORTED) && + (unitDesignator.size() > task_controller_object::Object::MAX_DESIGNATOR_LENGTH)) + { + CANStackLogger::warn("[DDOP]: Device value presentation unit designator " + + unitDesignator + + " is greater than the max byte length of 128. Value will be truncated."); + unitDesignator.resize(task_controller_object::Object::MAX_DESIGNATOR_LENGTH); + } + else if ((taskControllerCompatibilityLevel == MAX_TC_VERSION_SUPPORTED) && + (unitDesignator.size() > task_controller_object::Object::MAX_DESIGNATOR_LEGACY_LENGTH)) + { + CANStackLogger::info("[DDOP]: Device value presentation unit designator " + + unitDesignator + + " byte length is greater than the max character count of 32. " + + "This is only acceptable if you have 32 or fewer UTF-8 characters!" + + " Please verify your DDOP configuration meets this requirement."); + } + + objectList.emplace_back(new task_controller_object::DeviceValuePresentationObject(unitDesignator, + offsetValue, + scaleFactor, + numberDecimals, + uniqueID)); + } + else + { + CANStackLogger::error("[DDOP]: Device value presentation object ID " + + isobus::to_string(static_cast(uniqueID)) + + " is not unique. Object will not be added to the DDOP."); + } + return retVal; + } + + bool DeviceDescriptorObjectPool::deserialize_binary_object_pool(std::vector &binaryPool, NAME clientNAME) + { + return deserialize_binary_object_pool(binaryPool.data(), static_cast(binaryPool.size()), clientNAME); + } + + bool DeviceDescriptorObjectPool::deserialize_binary_object_pool(const std::uint8_t *binaryPool, std::uint32_t binaryPoolSizeBytes, NAME clientNAME) + { + bool retVal = true; + + if ((nullptr != binaryPool) && (0 != binaryPoolSizeBytes)) + { + CANStackLogger::debug("[DDOP]: Attempting to deserialize a binary object pool with size %u.", binaryPoolSizeBytes); + clear(); + + // Iterate over the DDOP and convert to objects. + // Using the size to track how much is left to process. + while (0 != binaryPoolSizeBytes) + { + // Verify there's enough data to read the XML namespace + if (binaryPoolSizeBytes > 3) + { + std::string xmlNameSpace; + xmlNameSpace.push_back(static_cast(binaryPool[0])); + xmlNameSpace.push_back(static_cast(binaryPool[1])); + xmlNameSpace.push_back(static_cast(binaryPool[2])); + + if ("DVC" == xmlNameSpace) + { + // Process a Device Object + std::uint8_t numberDesignatorBytes = 0; // Labelled "N" in 11783-10 table A.1 + std::uint8_t numberSoftwareVersionBytes = 0; // Labelled "M" in 11783-10 table A.1 + std::uint8_t numberDeviceSerialNumberBytes = 0; // Labelled "O" in 11783-10 table A.1 + std::uint8_t numberExtendedStructureLabelBytes = 0; // Version 4+ only + + if ((binaryPoolSizeBytes >= 6) && + (binaryPool[5] < 128)) + { + numberDesignatorBytes = binaryPool[5]; + } + else + { + CANStackLogger::error("[DDOP]: Binary device object designator has invalid length."); + retVal = false; + } + + if ((binaryPoolSizeBytes >= static_cast(7 + numberDesignatorBytes)) && + (binaryPool[6 + numberDesignatorBytes] < 128)) + { + numberSoftwareVersionBytes = binaryPool[6 + numberDesignatorBytes]; + } + else + { + CANStackLogger::error("[DDOP]: Binary device object software version has invalid length."); + retVal = false; + } + + if ((binaryPoolSizeBytes >= static_cast(16 + numberDesignatorBytes + numberSoftwareVersionBytes)) && + (binaryPool[15 + numberDesignatorBytes + numberSoftwareVersionBytes] < 128)) + { + numberDeviceSerialNumberBytes = binaryPool[15 + numberDesignatorBytes + numberSoftwareVersionBytes]; + } + else + { + CANStackLogger::error("[DDOP]: Binary device object serial number has invalid length."); + retVal = false; + } + + if (taskControllerCompatibilityLevel >= 4) + { + if ((binaryPoolSizeBytes >= static_cast(31 + numberDeviceSerialNumberBytes + numberDesignatorBytes + numberSoftwareVersionBytes)) && + (binaryPool[30 + numberDeviceSerialNumberBytes + numberDesignatorBytes + numberSoftwareVersionBytes] <= 32)) + { + numberExtendedStructureLabelBytes = binaryPool[30 + numberDeviceSerialNumberBytes + numberDesignatorBytes + numberSoftwareVersionBytes]; + } + else + { + CANStackLogger::error("[DDOP]: Binary device object with version 4 contains invalid extended structure label length."); + retVal = false; + } + } + + std::uint32_t expectedSize = (31 + numberDeviceSerialNumberBytes + numberDesignatorBytes + numberSoftwareVersionBytes + numberExtendedStructureLabelBytes); + + if (taskControllerCompatibilityLevel < 4) + { + expectedSize--; // One byte less due to no extended structure label length + } + + if ((binaryPoolSizeBytes >= expectedSize) && retVal) + { + std::string deviceDesignator; + std::string deviceSoftwareVersion; + std::string deviceSerialNumber; + std::string deviceStructureLabel; + std::array localizationLabel; + std::vector extendedStructureLabel; + std::uint64_t ddopClientNAME = 0; + + for (std::uint16_t i = 0; i < numberDesignatorBytes; i++) + { + deviceDesignator.push_back(binaryPool[6 + i]); + } + + for (std::uint16_t i = 0; i < numberSoftwareVersionBytes; i++) + { + deviceSoftwareVersion.push_back(binaryPool[7 + numberDesignatorBytes + i]); + } + + for (std::uint8_t i = 0; i < 8; i++) + { + std::uint64_t currentNameByte = binaryPool[7 + numberDesignatorBytes + numberSoftwareVersionBytes + i]; + ddopClientNAME |= (currentNameByte << (8 * i)); + } + + if ((0 != clientNAME.get_full_name()) && (ddopClientNAME != clientNAME.get_full_name())) + { + CANStackLogger::error("[DDOP]: Failed adding deserialized device object. DDOP NAME doesn't match client's actual NAME."); + retVal = false; + } + else if (0 == clientNAME.get_full_name()) + { + clientNAME.set_full_name(ddopClientNAME); + } + + for (std::uint16_t i = 0; i < numberDeviceSerialNumberBytes; i++) + { + deviceSerialNumber.push_back(binaryPool[16 + numberDesignatorBytes + numberSoftwareVersionBytes + i]); + } + + for (std::uint16_t i = 0; i < 7; i++) + { + deviceStructureLabel.push_back(binaryPool[16 + numberDeviceSerialNumberBytes + numberDesignatorBytes + numberSoftwareVersionBytes + i]); + } + + for (std::uint16_t i = 0; i < 7; i++) + { + localizationLabel.at(i) = (binaryPool[23 + numberDeviceSerialNumberBytes + numberDesignatorBytes + numberSoftwareVersionBytes + i]); + } + + for (std::uint16_t i = 0; i < numberExtendedStructureLabelBytes; i++) + { + extendedStructureLabel.push_back(binaryPool[31 + numberDeviceSerialNumberBytes + numberDesignatorBytes + numberSoftwareVersionBytes + i]); + } + + if (add_device(deviceDesignator, deviceSoftwareVersion, deviceSerialNumber, deviceStructureLabel, localizationLabel, extendedStructureLabel, clientNAME.get_full_name())) + { + binaryPoolSizeBytes -= expectedSize; + binaryPool += expectedSize; + } + else + { + CANStackLogger::error("[DDOP]: Failed adding deserialized device object. DDOP schema is not valid."); + retVal = false; + } + } + else + { + CANStackLogger::error("[DDOP]: Not enough binary DDOP data left to parse device object. DDOP schema is not valid"); + retVal = false; + } + } + else if ("DET" == xmlNameSpace) + { + // Process a device element object + std::uint8_t numberDesignatorBytes = 0; + std::uint8_t numberOfObjectIDs = 0; + + if (binaryPoolSizeBytes >= 7) + { + numberDesignatorBytes = binaryPool[6]; + } + else + { + CANStackLogger::error("[DDOP]: Binary device element object has invalid length."); + retVal = false; + } + + if (binaryPoolSizeBytes >= static_cast(12 + numberDesignatorBytes)) + { + numberOfObjectIDs = binaryPool[11 + numberDesignatorBytes]; + } + else + { + CANStackLogger::error("[DDOP]: Binary device element object has invalid length to process referenced object IDs."); + retVal = false; + } + + if (binaryPool[5] > static_cast(task_controller_object::DeviceElementObject::Type::NavigationReference)) + { + CANStackLogger::error("[DDOP]: Binary device element object has invalid element type."); + retVal = false; + } + + std::uint32_t expectedSize = (13 + (2 * numberOfObjectIDs) + numberDesignatorBytes); + + if ((binaryPoolSizeBytes >= expectedSize) && retVal) + { + std::string deviceElementDesignator; + std::uint16_t parentObject = static_cast(static_cast(binaryPool[9 + numberDesignatorBytes]) | (static_cast(binaryPool[10 + numberDesignatorBytes]) << 8)); + std::uint16_t uniqueID = static_cast(static_cast(binaryPool[3]) | (static_cast(binaryPool[4]) << 8)); + auto type = static_cast(binaryPool[5]); + + for (std::uint16_t i = 0; i < numberDesignatorBytes; i++) + { + deviceElementDesignator.push_back(binaryPool[7 + i]); + } + + if (add_device_element(deviceElementDesignator, binaryPool[7 + numberDesignatorBytes], parentObject, type, uniqueID)) + { + auto DETObject = std::static_pointer_cast(get_object_by_id(uniqueID)); + + if (nullptr != DETObject) + { + for (std::uint8_t i = 0; i < numberOfObjectIDs; i++) + { + std::uint16_t childID = static_cast(static_cast(binaryPool[13 + (2 * i) + numberDesignatorBytes]) | (static_cast(binaryPool[14 + (2 * i) + numberDesignatorBytes]) << 8)); + DETObject->add_reference_to_child_object(childID); + } + } + + binaryPoolSizeBytes -= expectedSize; + binaryPool += expectedSize; + } + else + { + CANStackLogger::error("[DDOP]: Failed adding deserialized device element object. DDOP schema is not valid."); + retVal = false; + } + } + else + { + CANStackLogger::error("[DDOP]: Not enough binary DDOP data left to parse device element object. DDOP schema is not valid"); + retVal = false; + } + } + else if ("DPD" == xmlNameSpace) + { + // Process a device process data object + std::uint8_t numberDesignatorBytes = 0; + + if ((binaryPoolSizeBytes >= 10) && + (binaryPool[9] < 128)) + { + numberDesignatorBytes = binaryPool[9]; + } + else + { + CANStackLogger::error("[DDOP]: Binary device process data object has invalid length."); + retVal = false; + } + + std::uint32_t expectedSize = (12 + numberDesignatorBytes); + + if ((binaryPoolSizeBytes >= expectedSize) && retVal) + { + std::string processDataDesignator; + std::uint16_t DDI = static_cast(static_cast(binaryPool[5]) | (static_cast(binaryPool[6]) << 8)); + std::uint16_t uniqueID = static_cast(static_cast(binaryPool[3]) | (static_cast(binaryPool[4]) << 8)); + std::uint16_t presentationObjectID = static_cast(static_cast(binaryPool[10 + numberDesignatorBytes]) | (static_cast(binaryPool[11 + numberDesignatorBytes]) << 8)); + + for (std::uint16_t i = 0; i < numberDesignatorBytes; i++) + { + processDataDesignator.push_back(binaryPool[10 + i]); + } + + if (add_device_process_data(processDataDesignator, DDI, presentationObjectID, binaryPool[7], binaryPool[8], uniqueID)) + { + binaryPoolSizeBytes -= expectedSize; + binaryPool += expectedSize; + } + else + { + CANStackLogger::error("[DDOP]: Failed adding deserialized device process data object. DDOP schema is not valid."); + retVal = false; + } + } + else + { + CANStackLogger::error("[DDOP]: Not enough binary DDOP data left to parse device process data object. DDOP schema is not valid"); + retVal = false; + } + } + else if ("DPT" == xmlNameSpace) + { + std::uint8_t numberDesignatorBytes = 0; + + if ((binaryPoolSizeBytes >= 12) && + (binaryPool[11] < 128)) + { + numberDesignatorBytes = binaryPool[11]; + } + else + { + CANStackLogger::error("[DDOP]: Binary device property object has invalid length."); + retVal = false; + } + + std::uint32_t expectedSize = (14 + numberDesignatorBytes); + + if ((binaryPoolSizeBytes >= expectedSize) && retVal) + { + std::string designator; + std::int32_t propertyValue = static_cast(binaryPool[7]) | + (static_cast(binaryPool[8]) << 8) | + (static_cast(binaryPool[9]) << 16) | + (static_cast(binaryPool[10]) << 24); + std::uint16_t DDI = static_cast(static_cast(binaryPool[5]) | (static_cast(binaryPool[6]) << 8)); + std::uint16_t uniqueID = static_cast(static_cast(binaryPool[3]) | (static_cast(binaryPool[4]) << 8)); + std::uint16_t presentationObjectID = static_cast(static_cast(binaryPool[12 + numberDesignatorBytes]) | (static_cast(binaryPool[13 + numberDesignatorBytes]) << 8)); + + for (std::uint16_t i = 0; i < numberDesignatorBytes; i++) + { + designator.push_back(binaryPool[12 + i]); + } + + if (add_device_property(designator, propertyValue, DDI, presentationObjectID, uniqueID)) + { + binaryPoolSizeBytes -= expectedSize; + binaryPool += expectedSize; + } + else + { + CANStackLogger::error("[DDOP]: Failed adding deserialized device property object. DDOP schema is not valid."); + retVal = false; + } + } + else + { + CANStackLogger::error("[DDOP]: Not enough binary DDOP data left to parse device property object. DDOP schema is not valid"); + retVal = false; + } + } + else if ("DVP" == xmlNameSpace) + { + std::uint8_t numberDesignatorBytes = 0; + + if ((binaryPoolSizeBytes >= 15) && + (binaryPool[14] < 128)) + { + numberDesignatorBytes = binaryPool[14]; + } + else + { + CANStackLogger::error("[DDOP]: Binary device value presentation object has invalid length."); + retVal = false; + } + + std::uint32_t expectedSize = (15 + numberDesignatorBytes); + + if ((binaryPoolSizeBytes >= expectedSize) && retVal) + { + std::string designator; + std::int32_t offset = static_cast(binaryPool[5]) | + (static_cast(binaryPool[6]) << 8) | + (static_cast(binaryPool[7]) << 16) | + (static_cast(binaryPool[8]) << 24); + std::array scaleBytes = { + binaryPool[9], + binaryPool[10], + binaryPool[11], + binaryPool[12], + }; + float scale = 0.0f; + std::uint16_t uniqueID = static_cast(static_cast(binaryPool[3]) | (static_cast(binaryPool[4]) << 8)); + + if (is_big_endian()) + { + std::reverse(scaleBytes.begin(), scaleBytes.end()); + } + + memcpy(&scale, scaleBytes.data(), sizeof(float)); + + for (std::uint16_t i = 0; i < numberDesignatorBytes; i++) + { + designator.push_back(binaryPool[15 + i]); + } + + if (add_device_value_presentation(designator, offset, scale, binaryPool[13], uniqueID)) + { + binaryPoolSizeBytes -= expectedSize; + binaryPool += expectedSize; + } + else + { + CANStackLogger::error("[DDOP]: Failed adding deserialized device value presentation object. DDOP schema is not valid."); + retVal = false; + } + } + else + { + CANStackLogger::error("[DDOP]: Not enough binary DDOP data left to parse device value presentation object. DDOP schema is not valid"); + retVal = false; + } + } + else + { + CANStackLogger::error("[DDOP]: Cannot process an unknown XML namespace from binary DDOP. DDOP schema is invalid."); + retVal = false; + } + } + else + { + retVal = false; + } + + if (!retVal) + { + CANStackLogger::error("[DDOP]: Binary DDOP deserialization aborted."); + break; + } + } + } + else + { + retVal = false; + CANStackLogger::error("[DDOP]: Cannot deserialize a DDOP with zero length."); + } + return retVal; + } + + bool DeviceDescriptorObjectPool::generate_binary_object_pool(std::vector &resultantPool) + { + bool retVal = true; + + resultantPool.clear(); + + if (taskControllerCompatibilityLevel > MAX_TC_VERSION_SUPPORTED) + { + CANStackLogger::warn("[DDOP]: A DDOP is being generated for a TC version that is unsupported. This may cause issues."); + } + + if (resolve_parent_ids_to_objects()) + { + retVal = true; + for (auto ¤tObject : objectList) + { + auto objectBinary = currentObject->get_binary_object(); + + if (!objectBinary.empty()) + { + resultantPool.insert(resultantPool.end(), objectBinary.begin(), objectBinary.end()); + } + else + { + CANStackLogger::error("[DDOP]: Failed to create all object binaries. Your DDOP is invalid."); + retVal = false; + break; + } + } + } + else + { + CANStackLogger::error("[DDOP]: Failed to resolve all object IDs in DDOP. Your DDOP contains invalid object references."); + retVal = false; + } + return retVal; + } + + std::shared_ptr DeviceDescriptorObjectPool::get_object_by_id(std::uint16_t objectID) + { + std::shared_ptr retVal; + + for (auto ¤tObject : objectList) + { + if (currentObject->get_object_id() == objectID) + { + retVal = currentObject; + break; + } + } + return retVal; + } + + std::shared_ptr DeviceDescriptorObjectPool::get_object_by_index(std::uint16_t index) + { + std::shared_ptr retVal = nullptr; + + if (index < objectList.size()) + { + retVal = objectList.at(index); + } + return retVal; + } + + bool DeviceDescriptorObjectPool::remove_object_by_id(std::uint16_t objectID) + { + bool retVal = false; + + for (auto object = objectList.begin(); object != objectList.end(); object++) + { + if ((nullptr != *object) && (*object)->get_object_id() == objectID) + { + objectList.erase(object); + retVal = true; + break; + } + } + return retVal; + } + + void DeviceDescriptorObjectPool::set_task_controller_compatibility_level(std::uint8_t tcVersion) + { + assert(tcVersion <= MAX_TC_VERSION_SUPPORTED); // You can't set the version higher than the max + + taskControllerCompatibilityLevel = tcVersion; + + // Manipulate the device object if it exists + auto deviceObject = std::static_pointer_cast(get_object_by_id(0)); + if (nullptr != deviceObject) + { + deviceObject->set_use_extended_structure_label(taskControllerCompatibilityLevel >= 4); + } + } + + std::uint8_t DeviceDescriptorObjectPool::get_task_controller_compatibility_level() const + { + return taskControllerCompatibilityLevel; + } + + std::uint8_t DeviceDescriptorObjectPool::get_max_supported_task_controller_version() + { + return MAX_TC_VERSION_SUPPORTED; + } + + void DeviceDescriptorObjectPool::clear() + { + objectList.clear(); + } + + std::size_t DeviceDescriptorObjectPool::size() const + { + return objectList.size(); + } + + bool DeviceDescriptorObjectPool::resolve_parent_ids_to_objects() + { + bool retVal = true; + + for (auto ¤tObject : objectList) + { + assert(nullptr != currentObject); + switch (currentObject->get_object_type()) + { + case task_controller_object::ObjectTypes::DeviceElement: + { + // Process parent object + auto currentDeviceElement = reinterpret_cast(currentObject.get()); + if (task_controller_object::Object::NULL_OBJECT_ID != currentDeviceElement->get_parent_object()) + { + auto parent = get_object_by_id(currentDeviceElement->get_parent_object()); + if (nullptr != parent.get()) + { + switch (parent->get_object_type()) + { + case task_controller_object::ObjectTypes::DeviceElement: + case task_controller_object::ObjectTypes::Device: + { + // Device and device element are allowed + } + break; + + default: + { + CANStackLogger::error("[DDOP]: Object " + + isobus::to_string(static_cast(currentObject->get_object_id())) + + " has an invalid parent object type. Only device element objects or device objects may be its parent."); + retVal = false; + } + break; + } + } + else + { + CANStackLogger::error("[DDOP]: Object " + + isobus::to_string(static_cast(currentDeviceElement->get_parent_object())) + + " is not found."); + retVal = false; + } + } + else + { + CANStackLogger::error("[DDOP]: Object " + + isobus::to_string(static_cast(currentObject->get_object_id())) + + " is an orphan. It's parent is 0xFFFF!"); + retVal = false; + } + + if (retVal) + { + // Process children now that parent has been validated + for (std::size_t i = 0; i < currentDeviceElement->get_number_child_objects(); i++) + { + auto child = get_object_by_id(currentDeviceElement->get_child_object_id(i)); + if (nullptr == child.get()) + { + CANStackLogger::error("[DDOP]: Object " + + isobus::to_string(static_cast(currentDeviceElement->get_child_object_id(i))) + + " is not found."); + retVal = false; + break; + } + else if ((task_controller_object::ObjectTypes::DeviceProcessData != child->get_object_type()) && + (task_controller_object::ObjectTypes::DeviceProperty != child->get_object_type())) + { + CANStackLogger::error("[DDOP]: Object %u has child %u which is an object type that is not allowed.", + currentDeviceElement->get_child_object_id(i), + child->get_object_id()); + CANStackLogger::error("[DDOP]: A DET object may only have DPD and DPT children."); + retVal = false; + break; + } + } + } + } + break; + + case task_controller_object::ObjectTypes::DeviceProcessData: + { + auto currentProcessData = reinterpret_cast(currentObject.get()); + + if (task_controller_object::Object::NULL_OBJECT_ID != currentProcessData->get_device_value_presentation_object_id()) + { + auto child = get_object_by_id(currentProcessData->get_device_value_presentation_object_id()); + if (nullptr == child.get()) + { + CANStackLogger::error("[DDOP]: Object " + + isobus::to_string(static_cast(currentProcessData->get_device_value_presentation_object_id())) + + " is not found."); + retVal = false; + break; + } + else if (task_controller_object::ObjectTypes::DeviceValuePresentation != child->get_object_type()) + { + CANStackLogger::error("[DDOP]: Object %u has a child %u with an object type that is not allowed." + + currentProcessData->get_device_value_presentation_object_id(), + child->get_object_id()); + CANStackLogger::error("[DDOP]: A DPD object may only have DVP children."); + retVal = false; + break; + } + } + } + break; + + case task_controller_object::ObjectTypes::DeviceProperty: + { + auto currentProperty = reinterpret_cast(currentObject.get()); + + if (task_controller_object::Object::NULL_OBJECT_ID != currentProperty->get_device_value_presentation_object_id()) + { + auto child = get_object_by_id(currentProperty->get_device_value_presentation_object_id()); + if (nullptr == child.get()) + { + CANStackLogger::error("[DDOP]: Object " + + isobus::to_string(static_cast(currentProperty->get_device_value_presentation_object_id())) + + " is not found."); + retVal = false; + break; + } + else if (task_controller_object::ObjectTypes::DeviceValuePresentation != child->get_object_type()) + { + CANStackLogger::error("[DDOP]: Object %u has a child %u with an object type that is not allowed." + + currentProperty->get_device_value_presentation_object_id(), + child->get_object_id()); + CANStackLogger::error("[DDOP]: A DPT object may only have DVP children."); + retVal = false; + break; + } + } + } + break; + + default: + { + // This object has no child/parent to validate + } + break; + } + + if (!retVal) + { + break; + } + } + return retVal; + } + + bool DeviceDescriptorObjectPool::check_object_id_unique(std::uint16_t uniqueID) const + { + bool retVal = true; + + if ((0 != uniqueID) && (task_controller_object::Object::NULL_OBJECT_ID != uniqueID)) + { + for (auto ¤tObject : objectList) + { + if (uniqueID == currentObject->get_object_id()) + { + retVal = false; + break; + } + } + } + else + { + retVal = false; + } + return retVal; + } + +} // namespace isobus diff --git a/src/isobus_device_descriptor_object_pool.hpp b/src/isobus_device_descriptor_object_pool.hpp new file mode 100644 index 0000000..e2257bb --- /dev/null +++ b/src/isobus_device_descriptor_object_pool.hpp @@ -0,0 +1,191 @@ +//================================================================================================ +/// @file isobus_device_descriptor_object_pool.hpp +/// +/// @brief Defines an interface for creating a Task Controller DDOP. +/// @author Adrian Del Grosso +/// +/// @copyright 2023 Adrian Del Grosso +//================================================================================================ + +#ifndef ISOBUS_DEVICE_DESCRIPTOR_OBJECT_POOL_HPP +#define ISOBUS_DEVICE_DESCRIPTOR_OBJECT_POOL_HPP + +#include "can_NAME.hpp" +#include "isobus_task_controller_client_objects.hpp" + +#include + +namespace isobus +{ + /// @brief Defines a device descriptor object pool + /// @details This class can be used to build up a task controller DDOP by adding objects to it + /// in a hierarchy, then calling generate_binary_object_pool to get the object pool in + /// binary form. + /// @note To ensure maximum compatibility with task controllers, it may be best to stick to + /// limits that were defined for TC 3 and older when providing things like labels for + /// device element designators. + class DeviceDescriptorObjectPool + { + public: + /// @brief Default constructor for a DDOP. Sets TC compatibility to version 4. + DeviceDescriptorObjectPool() = default; + + /// @brief This constructor allows customization of the TC compatibility level + /// @param[in] taskControllerServerVersion The version of TC server to support with this DDOP + explicit DeviceDescriptorObjectPool(std::uint8_t taskControllerServerVersion); + + /// @brief Adds a device object to the DDOP + /// @note There can only be one of these per DDOP + /// @attention Pay close attention to which values are UTF-8 and which are byte arrays + /// @param[in] deviceDesignator Descriptive text for the object, UTF-8, 32 chars max + /// @param[in] deviceSoftwareVersion Software version indicating text + /// @param[in] deviceSerialNumber Device and manufacturer-specific serial number of the Device (UTF-8) + /// @param[in] deviceStructureLabel This label allows the device to identify the current version of the device descriptor object pool + /// @param[in] deviceLocalizationLabel Defined by the language command PGN + /// @param[in] deviceExtendedStructureLabel Continuation of the Label given by Device to identify the Device descriptor Structure + /// @param[in] clientIsoNAME NAME of client device as defined in ISO 11783-5 + /// @returns `true` if the object was added to the DDOP, `false` if the object cannot be added (duplicate or some other error) + bool add_device(std::string deviceDesignator, + std::string deviceSoftwareVersion, + std::string deviceSerialNumber, + std::string deviceStructureLabel, + std::array deviceLocalizationLabel, + std::vector deviceExtendedStructureLabel, + std::uint64_t clientIsoNAME); + + /// @brief Adds a device element object to the DDOP + /// @param[in] deviceElementDesignator Descriptive text for the object, UTF-8, 32-128 chars max depending on TC version + /// @param[in] deviceElementNumber The Element number for process data variable addressing + /// @param[in] parentObjectID Object ID of parent DeviceElementObject or DeviceObject in order to establish a hierarchical order of DeviceElements + /// @param[in] deviceElementType The type of element, such as "device" or "bin" + /// @param[in] uniqueID The object ID of the object. Must be unique in the DDOP. + /// @returns `true` if the object was added to the DDOP, `false` if the object cannot be added (duplicate or some other error) + bool add_device_element(std::string deviceElementDesignator, + std::uint16_t deviceElementNumber, + std::uint16_t parentObjectID, + task_controller_object::DeviceElementObject::Type deviceElementType, + std::uint16_t uniqueID); + + /// @brief Adds a device process data object to the DDOP + /// @param[in] processDataDesignator Descriptive text for the object, UTF-8, 32-128 chars max + /// @param[in] processDataDDI Identifier of process data variable (DDI) according to definitions in Annex B and ISO 11783 - 11 + /// @param[in] deviceValuePresentationObjectID Object identifier of a DeviceValuePresentationObject, or the null ID + /// @param[in] processDataProperties A bitset of properties associated to this object. Some combination of `PropertiesBit` + /// @param[in] processDataTriggerMethods A bitset of available trigger methods, built from some combination of `AvailableTriggerMethods` + /// @param[in] uniqueID The object ID of the object. Must be unique in the DDOP. + /// @returns `true` if the object was added to the DDOP, `false` if the object cannot be added (duplicate or some other error) + bool add_device_process_data(std::string processDataDesignator, + std::uint16_t processDataDDI, + std::uint16_t deviceValuePresentationObjectID, + std::uint8_t processDataProperties, + std::uint8_t processDataTriggerMethods, + std::uint16_t uniqueID); + + /// @brief Adds a device property object to the DDOP + /// @param[in] propertyDesignator Descriptive text for the object, UTF-8, 32-128 chars max + /// @param[in] propertyValue The value of the property + /// @param[in] propertyDDI Identifier of property (DDI) according to definitions in Annex B and ISO 11783 - 11. + /// @param[in] valuePresentationObject Object identifier of DeviceValuePresentationObject, or NULL object ID + /// @param[in] uniqueID The object ID of the object. Must be unique in the DDOP. + /// @returns `true` if the object was added to the DDOP, `false` if the object cannot be added (duplicate or some other error) + bool add_device_property(std::string propertyDesignator, + std::int32_t propertyValue, + std::uint16_t propertyDDI, + std::uint16_t valuePresentationObject, + std::uint16_t uniqueID); + + /// @brief Adds a device value presentation object to the DDOP + /// @param[in] unitDesignator Unit designator for this value presentation + /// @param[in] offsetValue Offset to be applied to the value for presentation. + /// @param[in] scaleFactor Scale to be applied to the value for presentation. + /// @param[in] numberDecimals Specifies the number of decimals to display after the decimal point. + /// @param[in] uniqueID The object ID of the object. Must be unique in the DDOP. + /// @returns `true` if the object was added to the DDOP, `false` if the object cannot be added (duplicate or some other error) + bool add_device_value_presentation(std::string unitDesignator, + std::int32_t offsetValue, + float scaleFactor, + std::uint8_t numberDecimals, + std::uint16_t uniqueID); + + /// @brief Attempts to take a binary object pool and convert it back into + /// C++ objects. Useful for a task controller server or to view the content + /// of a DDOP captured in a CAN log, for example. + /// @param binaryPool The binary object pool, as an array of bytes. + /// @param clientNAME The ISO NAME of the source ECU for this DDOP, or NAME(0) to ignore checking against actual ECU NAME + /// @returns True if the object pool was successfully deserialized, otherwise false. + /// NOTE: This only means that the pool was deserialized. It does not mean that the + /// relationship between objects is valid. You may have to do additional + /// checking on the pool before using it. + bool deserialize_binary_object_pool(std::vector &binaryPool, NAME clientNAME = NAME(0)); + + /// @brief Attempts to take a binary object pool and convert it back into + /// C++ objects. Useful for a task controller server or to view the content + /// of a DDOP captured in a CAN log, for example. + /// @param binaryPool The binary object pool, as an array of bytes. + /// @param binaryPoolSizeBytes The size of the DDOP to process in bytes. + /// @param clientNAME The ISO NAME of the source ECU for this DDOP, or NAME(0) to ignore checking against actual ECU NAME + /// @returns True if the object pool was successfully deserialized, otherwise false. + /// NOTE: This only means that the pool was deserialized. It does not mean that the + /// relationship between objects is valid. You may have to do additional + /// checking on the pool before using it. + bool deserialize_binary_object_pool(const std::uint8_t *binaryPool, std::uint32_t binaryPoolSizeBytes, NAME clientNAME = NAME(0)); + + /// Constructs a binary DDOP using the objects that were previously added + /// @param[in,out] resultantPool The binary representation of the DDOP, or an empty vector if this function returns false + /// @returns `true` if the object pool was generated and is valid, otherwise `false`. + bool generate_binary_object_pool(std::vector &resultantPool); + + /// @brief Gets an object from the DDOP that corresponds to a certain object ID + /// @returns Pointer to the object matching the provided ID, or nullptr if no match was found + std::shared_ptr get_object_by_id(std::uint16_t objectID); + + /// @brief Gets an object from the DDOP by index based on object creation + /// @returns Pointer to the object matching the index, or nullptr if no match was found + std::shared_ptr get_object_by_index(std::uint16_t index); + + /// @brief Removes an object from the DDOP using its object ID. + /// @note This will not fix orphaned parent/child relationships. + /// Also, if two or more objects were created with the same ID, only one match will be removed. + /// You should consider the case where 2 objects have the same ID undefined behavior. + /// @param[in] objectID The ID of the object to remove + /// @returns true if the object with the specified ID was removed, otherwise false + bool remove_object_by_id(std::uint16_t objectID); + + /// @brief Sets the TC version to use when generating a binary DDOP. + /// @note If you do not call this, TC version 4 is used by default + /// @param[in] tcVersion The version of TC you are targeting for this DDOP + void set_task_controller_compatibility_level(std::uint8_t tcVersion); + + /// @brief Returns the current TC version used when generating a binary DDOP. + /// @returns the current TC version used when generating a binary DDOP. + std::uint8_t get_task_controller_compatibility_level() const; + + /// @brief Returns The maximum TC version supported by the CAN stack's DDOP generator + /// @returns The maximum TC version supported by the CAN stack's DDOP generator + static std::uint8_t get_max_supported_task_controller_version(); + + /// @brief Clears the DDOP back to an empty state + void clear(); + + /// @brief Returns the number of objects in the DDOP + /// @returns The number of objects in the DDOP + std::size_t size() const; + + private: + /// @brief Checks to see that all parent object IDs correspond to an object in this DDOP + /// @returns `true` if all object IDs were validated, otherwise `false` + bool resolve_parent_ids_to_objects(); + + /// @brief Checks the DDOP to see if an object ID has already been used + /// @param[in] uniqueID The ID to check against in the DDOP for uniqueness + /// @returns true if the object ID parameter is unique in the DDOP, otherwise false + bool check_object_id_unique(std::uint16_t uniqueID) const; + + static constexpr std::uint8_t MAX_TC_VERSION_SUPPORTED = 4; ///< The max TC version a DDOP object can support as of today + + std::vector> objectList; ///< Maintains a list of all added objects + std::uint8_t taskControllerCompatibilityLevel = MAX_TC_VERSION_SUPPORTED; ///< Stores the max TC version + }; +} // namespace isobus + +#endif // ISOBUS_DEVICE_DESCRIPTOR_OBJECT_POOL_HPP diff --git a/src/isobus_diagnostic_protocol.cpp b/src/isobus_diagnostic_protocol.cpp new file mode 100644 index 0000000..53cb1b7 --- /dev/null +++ b/src/isobus_diagnostic_protocol.cpp @@ -0,0 +1,1358 @@ +//================================================================================================ +/// @file isobus_diagnostic_protocol.cpp +/// +/// @brief A protocol that handles the ISO 11783-12 Diagnostic Protocol and some J1939 DMs. +/// @details This protocol manages many of the messages defined in ISO 11783-12 +/// and a subset of the messages defined in SAE J1939-73. +/// The ISO-11783 definition of some of these is based on the J1939 definition with some tweaks. +/// You can select if you want the protocol to behave like J1939 by calling set_j1939_mode. +/// One of the messages this protocol supports is the DM1 message. +/// The DM1 is sent via BAM, which has some implications to your application, +/// as only 1 BAM can be active at a time. This message +/// is sent at 1 Hz. In ISOBUS mode, unlike in J1939, the message is discontinued when no DTCs are active to +/// minimize bus load. Also, ISO-11783 does not utilize or support lamp status. +/// Other messages this protocol supports include: DM2, DM3, DM11, DM13, DM22, software ID, and Product ID. +/// +/// @note DM13 has two primary functions. It may be used as a command, from either a tool or an +/// ECU, directed to a single controller or to all controllers to request the receiving +/// controller(s) to stop or start broadcast messages. Additionally, it may be used by an ECU +/// to inform other nodes that the sender is about to suspend its normal broadcast due to +/// commands other than a SAE J1939 DM13 command received on that same network segment. +/// The broadcast messages stopped, started, or suspended may be on networks other than SAE J1939. +/// This is not a message to ignore all communications. It is a message to minimize network traffic. +/// +/// @attention It is recognized that some network messages may be required to continue even during +/// the "stop broadcast" condition. You MUST handle this in your application, as the stack cannot +/// decide what messages are required without context. In other words, you must opt-in to make +/// your application layer messages adhere to DM13 requests by explicitly calling the functions +/// on this protocol (using get_diagnostic_protocol_by_internal_control_function) +/// to check if you should send it. +/// +/// @author Adrian Del Grosso +/// +/// @copyright 2022 Adrian Del Grosso +//================================================================================================ + +#include "isobus_diagnostic_protocol.hpp" + +#include "can_general_parameter_group_numbers.hpp" +#include "can_network_manager.hpp" +#include "can_parameter_group_number_request_protocol.hpp" +#include "can_stack_logger.hpp" +#include "system_timing.hpp" + +#include + +namespace isobus +{ + DiagnosticProtocol::DiagnosticTroubleCode::DiagnosticTroubleCode(std::uint32_t spn, FailureModeIdentifier failureMode, LampStatus lamp) : + suspectParameterNumber(spn), + failureModeIdentifier(failureMode), + lampState(lamp) + { + } + + bool DiagnosticProtocol::DiagnosticTroubleCode::operator==(const DiagnosticTroubleCode &obj) const + { + return ((suspectParameterNumber == obj.suspectParameterNumber) && + (failureModeIdentifier == obj.failureModeIdentifier) && + (lampState == obj.lampState)); + } + + std::uint8_t DiagnosticProtocol::DiagnosticTroubleCode::get_occurrence_count() const + { + return occurrenceCount; + } + + std::uint32_t DiagnosticProtocol::DiagnosticTroubleCode::get_suspect_parameter_number() const + { + return suspectParameterNumber; + } + + DiagnosticProtocol::FailureModeIdentifier DiagnosticProtocol::DiagnosticTroubleCode::get_failure_mode_identifier() const + { + return failureModeIdentifier; + } + + DiagnosticProtocol::DiagnosticProtocol(std::shared_ptr internalControlFunction, NetworkType networkType) : + ControlFunctionFunctionalitiesMessageInterface(internalControlFunction), + myControlFunction(internalControlFunction), + networkType(networkType), + txFlags(static_cast(TransmitFlags::NumberOfFlags), process_flags, this) + { + ecuIdentificationFields.resize(static_cast(ECUIdentificationFields::NumberOfFields)); + + for (auto &ecuIDField : ecuIdentificationFields) + { + ecuIDField = "*"; + } + } + + DiagnosticProtocol::~DiagnosticProtocol() + { + terminate(); + } + + bool DiagnosticProtocol::initialize() + { + bool retVal = false; + if (!initialized) + { + initialized = true; + CANNetworkManager::CANNetwork.add_protocol_parameter_group_number_callback(static_cast(CANLibParameterGroupNumber::DiagnosticMessage22), process_message, this); + CANNetworkManager::CANNetwork.add_protocol_parameter_group_number_callback(static_cast(CANLibParameterGroupNumber::DiagnosticMessage13), process_message, this); + CANNetworkManager::CANNetwork.add_global_parameter_group_number_callback(static_cast(CANLibParameterGroupNumber::DiagnosticMessage13), process_message, this); + + if (auto requestProtocol = myControlFunction->get_pgn_request_protocol().lock()) + { + requestProtocol->register_pgn_request_callback(static_cast(CANLibParameterGroupNumber::DiagnosticMessage1), process_parameter_group_number_request, this); + requestProtocol->register_pgn_request_callback(static_cast(CANLibParameterGroupNumber::DiagnosticMessage2), process_parameter_group_number_request, this); + requestProtocol->register_pgn_request_callback(static_cast(CANLibParameterGroupNumber::DiagnosticMessage3), process_parameter_group_number_request, this); + requestProtocol->register_pgn_request_callback(static_cast(CANLibParameterGroupNumber::DiagnosticMessage11), process_parameter_group_number_request, this); + requestProtocol->register_pgn_request_callback(static_cast(CANLibParameterGroupNumber::ProductIdentification), process_parameter_group_number_request, this); + requestProtocol->register_pgn_request_callback(static_cast(CANLibParameterGroupNumber::DiagnosticProtocolIdentification), process_parameter_group_number_request, this); + requestProtocol->register_pgn_request_callback(static_cast(CANLibParameterGroupNumber::SoftwareIdentification), process_parameter_group_number_request, this); + requestProtocol->register_pgn_request_callback(static_cast(CANLibParameterGroupNumber::ECUIdentificationInformation), process_parameter_group_number_request, this); + } + retVal = true; + } + else + { + CANStackLogger::warn("[DP] DiagnosticProtocol's initialize() called when already initialized"); + } + return retVal; + } + + bool DiagnosticProtocol::get_initialized() const + { + return initialized; + } + + void DiagnosticProtocol::terminate() + { + if (initialized) + { + initialized = false; + + if (auto requestProtocol = myControlFunction->get_pgn_request_protocol().lock()) + { + requestProtocol->remove_pgn_request_callback(static_cast(CANLibParameterGroupNumber::DiagnosticMessage1), process_parameter_group_number_request, this); + requestProtocol->remove_pgn_request_callback(static_cast(CANLibParameterGroupNumber::DiagnosticMessage2), process_parameter_group_number_request, this); + requestProtocol->remove_pgn_request_callback(static_cast(CANLibParameterGroupNumber::DiagnosticMessage3), process_parameter_group_number_request, this); + requestProtocol->remove_pgn_request_callback(static_cast(CANLibParameterGroupNumber::DiagnosticMessage11), process_parameter_group_number_request, this); + requestProtocol->remove_pgn_request_callback(static_cast(CANLibParameterGroupNumber::ProductIdentification), process_parameter_group_number_request, this); + requestProtocol->remove_pgn_request_callback(static_cast(CANLibParameterGroupNumber::DiagnosticProtocolIdentification), process_parameter_group_number_request, this); + requestProtocol->remove_pgn_request_callback(static_cast(CANLibParameterGroupNumber::SoftwareIdentification), process_parameter_group_number_request, this); + requestProtocol->remove_pgn_request_callback(static_cast(CANLibParameterGroupNumber::ECUIdentificationInformation), process_parameter_group_number_request, this); + } + CANNetworkManager::CANNetwork.remove_protocol_parameter_group_number_callback(static_cast(CANLibParameterGroupNumber::DiagnosticMessage22), process_message, this); + CANNetworkManager::CANNetwork.remove_protocol_parameter_group_number_callback(static_cast(CANLibParameterGroupNumber::DiagnosticMessage13), process_message, this); + CANNetworkManager::CANNetwork.remove_global_parameter_group_number_callback(static_cast(CANLibParameterGroupNumber::DiagnosticMessage13), process_message, this); + } + } + + void DiagnosticProtocol::update() + { + if (0 != customDM13SuspensionTime) + { + if (SystemTiming::time_expired_ms(lastDM13ReceivedTimestamp, customDM13SuspensionTime)) + { + broadcastState = true; + customDM13SuspensionTime = 0; + } + } + else if (SystemTiming::time_expired_ms(lastDM13ReceivedTimestamp, DM13_TIMEOUT_MS)) + { + broadcastState = true; + } + + if (broadcastState) + { + if (j1939Mode) + { + if (SystemTiming::time_expired_ms(lastDM1SentTimestamp, DM_MAX_FREQUENCY_MS)) + { + txFlags.set_flag(static_cast(TransmitFlags::DM1)); + lastDM1SentTimestamp = SystemTiming::get_timestamp_ms(); + } + } + else + { + if ((0 != activeDTCList.size()) && + (SystemTiming::time_expired_ms(lastDM1SentTimestamp, DM_MAX_FREQUENCY_MS))) + { + txFlags.set_flag(static_cast(TransmitFlags::DM1)); + lastDM1SentTimestamp = SystemTiming::get_timestamp_ms(); + } + } + } + txFlags.process_all_flags(); + ControlFunctionFunctionalitiesMessageInterface.update(); + } + + void DiagnosticProtocol::set_j1939_mode(bool value) + { + j1939Mode = value; + } + + bool DiagnosticProtocol::get_j1939_mode() const + { + return j1939Mode; + } + + void DiagnosticProtocol::clear_active_diagnostic_trouble_codes() + { + inactiveDTCList.insert(std::end(inactiveDTCList), std::begin(activeDTCList), std::end(activeDTCList)); + activeDTCList.clear(); + + if (broadcastState) + { + txFlags.set_flag(static_cast(TransmitFlags::DM1)); + } + } + + void DiagnosticProtocol::clear_inactive_diagnostic_trouble_codes() + { + inactiveDTCList.clear(); + } + + void DiagnosticProtocol::clear_software_id_fields() + { + softwareIdentificationFields.clear(); + } + + void DiagnosticProtocol::set_ecu_id_field(ECUIdentificationFields field, const std::string &value) + { + if (field <= ECUIdentificationFields::NumberOfFields) + { + ecuIdentificationFields[static_cast(field)] = value + "*"; + } + } + + bool DiagnosticProtocol::set_diagnostic_trouble_code_active(const DiagnosticTroubleCode &dtc, bool active) + { + bool retVal = false; + + if (active) + { + // First check to see if it's already active + auto activeLocation = std::find(activeDTCList.begin(), activeDTCList.end(), dtc); + + if (activeDTCList.end() == activeLocation) + { + // Not already active. This is valid + retVal = true; + auto inactiveLocation = std::find(inactiveDTCList.begin(), inactiveDTCList.end(), dtc); + + if (inactiveDTCList.end() != inactiveLocation) + { + inactiveLocation->occurrenceCount++; + activeDTCList.push_back(*inactiveLocation); + inactiveDTCList.erase(inactiveLocation); + } + else + { + activeDTCList.push_back(dtc); + activeDTCList[activeDTCList.size() - 1].occurrenceCount = 1; + + if ((SystemTiming::get_time_elapsed_ms(lastDM1SentTimestamp) > DM_MAX_FREQUENCY_MS) && + broadcastState) + { + txFlags.set_flag(static_cast(TransmitFlags::DM1)); + lastDM1SentTimestamp = SystemTiming::get_timestamp_ms(); + } + } + } + else + { + // Already active! + retVal = false; + } + } + else + { + /// First check to see if it's already in the inactive list + auto inactiveLocation = std::find(inactiveDTCList.begin(), inactiveDTCList.end(), dtc); + + if (inactiveDTCList.end() == inactiveLocation) + { + retVal = true; + auto activeLocation = std::find(activeDTCList.begin(), activeDTCList.end(), dtc); + + if (activeDTCList.end() != activeLocation) + { + inactiveDTCList.push_back(*activeLocation); + activeDTCList.erase(activeLocation); + } + } + else + { + // Already inactive! + retVal = false; + } + } + return retVal; + } + + bool DiagnosticProtocol::get_diagnostic_trouble_code_active(const DiagnosticTroubleCode &dtc) + { + auto activeLocation = std::find(activeDTCList.begin(), activeDTCList.end(), dtc); + bool retVal = false; + + if (activeDTCList.end() != activeLocation) + { + retVal = true; + } + return retVal; + } + + bool DiagnosticProtocol::set_product_identification_code(const std::string &value) + { + bool retVal = false; + + if (value.size() < PRODUCT_IDENTIFICATION_MAX_STRING_LENGTH) + { + productIdentificationCode = value; + retVal = true; + } + return retVal; + } + + bool DiagnosticProtocol::set_product_identification_brand(const std::string &value) + { + bool retVal = false; + + if (value.size() < PRODUCT_IDENTIFICATION_MAX_STRING_LENGTH) + { + productIdentificationBrand = value; + retVal = true; + } + return retVal; + } + + bool DiagnosticProtocol::set_product_identification_model(const std::string &value) + { + bool retVal = false; + + if (value.size() < PRODUCT_IDENTIFICATION_MAX_STRING_LENGTH) + { + productIdentificationModel = value; + retVal = true; + } + return retVal; + } + + void DiagnosticProtocol::set_software_id_field(std::uint32_t index, const std::string &value) + { + if (index >= softwareIdentificationFields.size()) + { + softwareIdentificationFields.resize(index + 1); + } + else if (("" == value) && (index == softwareIdentificationFields.size())) + { + softwareIdentificationFields.pop_back(); + } + softwareIdentificationFields[index] = value; + } + + bool DiagnosticProtocol::suspend_broadcasts(std::uint16_t suspendTime_seconds) + { + broadcastState = false; + lastDM13ReceivedTimestamp = SystemTiming::get_timestamp_ms(); + + if (customDM13SuspensionTime < MAX_DM13_CUSTOM_SUSPEND_TIME_MS) + { + customDM13SuspensionTime = suspendTime_seconds; + } + return send_dm13_announce_suspension(suspendTime_seconds); + } + + bool DiagnosticProtocol::get_broadcast_state() const + { + return broadcastState; + } + + std::uint8_t DiagnosticProtocol::convert_flash_state_to_byte(FlashState flash) const + { + std::uint8_t retVal = 0; + + switch (flash) + { + case FlashState::Slow: + { + retVal = 0x00; + } + break; + + case FlashState::Fast: + { + retVal = 0x01; + } + break; + + default: + { + retVal = 0x03; + } + break; + } + return retVal; + } + + void DiagnosticProtocol::get_active_list_lamp_state_and_flash_state(Lamps targetLamp, FlashState &flash, bool &lampOn) const + { + flash = FlashState::Solid; + lampOn = false; + + for (auto &dtc : activeDTCList) + { + switch (targetLamp) + { + case Lamps::AmberWarningLamp: + { + if (dtc.lampState == LampStatus::AmberWarningLampSolid) + { + lampOn = true; + } + else if (dtc.lampState == LampStatus::AmberWarningLampSlowFlash) + { + lampOn = true; + if (flash != FlashState::Fast) + { + flash = FlashState::Slow; + } + } + else if (dtc.lampState == LampStatus::AmberWarningLampFastFlash) + { + lampOn = true; + flash = FlashState::Fast; + } + } + break; + + case Lamps::MalfunctionIndicatorLamp: + { + if (dtc.lampState == LampStatus::MalfunctionIndicatorLampSolid) + { + lampOn = true; + } + else if (dtc.lampState == LampStatus::MalfunctionIndicatorLampSlowFlash) + { + lampOn = true; + if (flash != FlashState::Fast) + { + flash = FlashState::Slow; + } + } + else if (dtc.lampState == LampStatus::MalfunctionIndicatorLampFastFlash) + { + lampOn = true; + flash = FlashState::Fast; + } + } + break; + + case Lamps::ProtectLamp: + { + if (dtc.lampState == LampStatus::EngineProtectLampSolid) + { + lampOn = true; + } + else if (dtc.lampState == LampStatus::EngineProtectLampSlowFlash) + { + lampOn = true; + if (flash != FlashState::Fast) + { + flash = FlashState::Slow; + } + } + else if (dtc.lampState == LampStatus::EngineProtectLampFastFlash) + { + lampOn = true; + flash = FlashState::Fast; + } + } + break; + + case Lamps::RedStopLamp: + { + if (dtc.lampState == LampStatus::RedStopLampSolid) + { + lampOn = true; + } + else if (dtc.lampState == LampStatus::RedStopLampSlowFlash) + { + lampOn = true; + if (flash != FlashState::Fast) + { + flash = FlashState::Slow; + } + } + else if (dtc.lampState == LampStatus::RedStopLampFastFlash) + { + lampOn = true; + flash = FlashState::Fast; + } + } + break; + + default: + break; + } + } + } + + void DiagnosticProtocol::get_inactive_list_lamp_state_and_flash_state(Lamps targetLamp, FlashState &flash, bool &lampOn) const + { + flash = FlashState::Solid; + lampOn = false; + + for (auto &dtc : inactiveDTCList) + { + switch (targetLamp) + { + case Lamps::AmberWarningLamp: + { + if (dtc.lampState == LampStatus::AmberWarningLampSolid) + { + lampOn = true; + } + else if (dtc.lampState == LampStatus::AmberWarningLampSlowFlash) + { + lampOn = true; + if (flash != FlashState::Fast) + { + flash = FlashState::Slow; + } + } + else if (dtc.lampState == LampStatus::AmberWarningLampFastFlash) + { + lampOn = true; + flash = FlashState::Fast; + } + } + break; + + case Lamps::MalfunctionIndicatorLamp: + { + if (dtc.lampState == LampStatus::MalfunctionIndicatorLampSolid) + { + lampOn = true; + } + else if (dtc.lampState == LampStatus::MalfunctionIndicatorLampSlowFlash) + { + lampOn = true; + if (flash != FlashState::Fast) + { + flash = FlashState::Slow; + } + } + else if (dtc.lampState == LampStatus::MalfunctionIndicatorLampFastFlash) + { + lampOn = true; + flash = FlashState::Fast; + } + } + break; + + case Lamps::ProtectLamp: + { + if (dtc.lampState == LampStatus::EngineProtectLampSolid) + { + lampOn = true; + } + else if (dtc.lampState == LampStatus::EngineProtectLampSlowFlash) + { + lampOn = true; + if (flash != FlashState::Fast) + { + flash = FlashState::Slow; + } + } + else if (dtc.lampState == LampStatus::EngineProtectLampFastFlash) + { + lampOn = true; + flash = FlashState::Fast; + } + } + break; + + case Lamps::RedStopLamp: + { + if (dtc.lampState == LampStatus::RedStopLampSolid) + { + lampOn = true; + } + else if (dtc.lampState == LampStatus::RedStopLampSlowFlash) + { + lampOn = true; + if (flash != FlashState::Fast) + { + flash = FlashState::Slow; + } + } + else if (dtc.lampState == LampStatus::RedStopLampFastFlash) + { + lampOn = true; + flash = FlashState::Fast; + } + } + break; + + default: + break; + } + } + } + + bool DiagnosticProtocol::send_diagnostic_message_1() const + { + bool retVal = false; + + if (nullptr != myControlFunction) + { + std::uint16_t payloadSize = static_cast(activeDTCList.size() * DM_PAYLOAD_BYTES_PER_DTC) + 2; // 2 Bytes (0 and 1) are reserved + + if (payloadSize <= MAX_PAYLOAD_SIZE_BYTES) + { + std::vector buffer; + buffer.resize(payloadSize < CAN_DATA_LENGTH ? CAN_DATA_LENGTH : payloadSize); + + if (get_j1939_mode()) + { + bool tempLampState = false; + FlashState tempLampFlashState = FlashState::Solid; + get_active_list_lamp_state_and_flash_state(Lamps::ProtectLamp, tempLampFlashState, tempLampState); + + /// Encode Protect state and flash + buffer[0] = tempLampState; + buffer[1] = convert_flash_state_to_byte(tempLampFlashState); + + get_active_list_lamp_state_and_flash_state(Lamps::AmberWarningLamp, tempLampFlashState, tempLampState); + + /// Encode amber warning lamp state and flash + buffer[0] |= (static_cast(tempLampState) << 2); + buffer[1] |= (convert_flash_state_to_byte(tempLampFlashState) << 2); + + get_active_list_lamp_state_and_flash_state(Lamps::RedStopLamp, tempLampFlashState, tempLampState); + + /// Encode red stop lamp state and flash + buffer[0] |= (static_cast(tempLampState) << 4); + buffer[1] |= (convert_flash_state_to_byte(tempLampFlashState) << 4); + + get_active_list_lamp_state_and_flash_state(Lamps::MalfunctionIndicatorLamp, tempLampFlashState, tempLampState); + + /// Encode malfunction indicator lamp state and flash + buffer[0] |= (static_cast(tempLampState) << 6); + buffer[1] |= (convert_flash_state_to_byte(tempLampFlashState) << 6); + } + else + { + // ISO 11783 does not use lamp state or lamp flash bytes + buffer[0] = 0xFF; + buffer[1] = 0xFF; + } + + if (activeDTCList.empty()) + { + buffer[2] = 0x00; + buffer[3] = 0x00; + buffer[4] = 0x00; + buffer[5] = 0x00; + buffer[6] = 0xFF; + buffer[7] = 0xFF; + retVal = CANNetworkManager::CANNetwork.send_can_message(static_cast(CANLibParameterGroupNumber::DiagnosticMessage1), + buffer.data(), + CAN_DATA_LENGTH, + myControlFunction); + } + else + { + for (std::size_t i = 0; i < activeDTCList.size(); i++) + { + buffer[2 + (DM_PAYLOAD_BYTES_PER_DTC * i)] = static_cast(activeDTCList[i].suspectParameterNumber & 0xFF); + buffer[3 + (DM_PAYLOAD_BYTES_PER_DTC * i)] = static_cast((activeDTCList[i].suspectParameterNumber >> 8) & 0xFF); + buffer[4 + (DM_PAYLOAD_BYTES_PER_DTC * i)] = (static_cast(((activeDTCList[i].suspectParameterNumber >> 16) & 0xFF) << 5) | (static_cast(activeDTCList[i].failureModeIdentifier) & 0x1F)); + buffer[5 + (DM_PAYLOAD_BYTES_PER_DTC * i)] = (activeDTCList[i].occurrenceCount & 0x7F); + } + + if (payloadSize < CAN_DATA_LENGTH) + { + buffer[6] = 0xFF; + buffer[7] = 0xFF; + payloadSize = CAN_DATA_LENGTH; + } + + retVal = CANNetworkManager::CANNetwork.send_can_message(static_cast(CANLibParameterGroupNumber::DiagnosticMessage1), + buffer.data(), + payloadSize, + myControlFunction); + } + } + } + return retVal; + } + + bool DiagnosticProtocol::send_diagnostic_message_2() const + { + bool retVal = false; + + if (nullptr != myControlFunction) + { + std::uint16_t payloadSize = static_cast(inactiveDTCList.size() * DM_PAYLOAD_BYTES_PER_DTC) + 2; // 2 Bytes (0 and 1) are reserved or used for lamp + flash + + if (payloadSize <= MAX_PAYLOAD_SIZE_BYTES) + { + std::vector buffer; + buffer.resize(payloadSize < CAN_DATA_LENGTH ? CAN_DATA_LENGTH : payloadSize); + + if (get_j1939_mode()) + { + bool tempLampState = false; + FlashState tempLampFlashState = FlashState::Solid; + get_inactive_list_lamp_state_and_flash_state(Lamps::ProtectLamp, tempLampFlashState, tempLampState); + + /// Encode Protect state and flash + buffer[0] = tempLampState; + buffer[1] = convert_flash_state_to_byte(tempLampFlashState); + + get_inactive_list_lamp_state_and_flash_state(Lamps::AmberWarningLamp, tempLampFlashState, tempLampState); + + /// Encode amber warning lamp state and flash + buffer[0] |= (static_cast(tempLampState) << 2); + buffer[1] |= (convert_flash_state_to_byte(tempLampFlashState) << 2); + + get_inactive_list_lamp_state_and_flash_state(Lamps::RedStopLamp, tempLampFlashState, tempLampState); + + /// Encode red stop lamp state and flash + buffer[0] |= (static_cast(tempLampState) << 4); + buffer[1] |= (convert_flash_state_to_byte(tempLampFlashState) << 4); + + get_inactive_list_lamp_state_and_flash_state(Lamps::MalfunctionIndicatorLamp, tempLampFlashState, tempLampState); + + /// Encode malfunction indicator lamp state and flash + buffer[0] |= (static_cast(tempLampState) << 6); + buffer[1] |= (convert_flash_state_to_byte(tempLampFlashState) << 6); + } + else + { + // ISO 11783 does not use lamp state or lamp flash bytes + buffer[0] = 0xFF; + buffer[1] = 0xFF; + } + + if (inactiveDTCList.empty()) + { + buffer[2] = 0x00; + buffer[3] = 0x00; + buffer[4] = 0x00; + buffer[5] = 0x00; + buffer[6] = 0xFF; + buffer[7] = 0xFF; + retVal = CANNetworkManager::CANNetwork.send_can_message(static_cast(CANLibParameterGroupNumber::DiagnosticMessage2), + buffer.data(), + CAN_DATA_LENGTH, + myControlFunction); + } + else + { + for (std::size_t i = 0; i < inactiveDTCList.size(); i++) + { + buffer[2 + (DM_PAYLOAD_BYTES_PER_DTC * i)] = static_cast(inactiveDTCList[i].suspectParameterNumber & 0xFF); + buffer[3 + (DM_PAYLOAD_BYTES_PER_DTC * i)] = static_cast((inactiveDTCList[i].suspectParameterNumber >> 8) & 0xFF); + buffer[4 + (DM_PAYLOAD_BYTES_PER_DTC * i)] = (static_cast(((inactiveDTCList[i].suspectParameterNumber >> 16) & 0xFF) << 5) | (static_cast(inactiveDTCList[i].failureModeIdentifier) & 0x1F)); + buffer[5 + (DM_PAYLOAD_BYTES_PER_DTC * i)] = (inactiveDTCList[i].occurrenceCount & 0x7F); + } + + if (payloadSize < CAN_DATA_LENGTH) + { + buffer[6] = 0xFF; + buffer[7] = 0xFF; + payloadSize = CAN_DATA_LENGTH; + } + + retVal = CANNetworkManager::CANNetwork.send_can_message(static_cast(CANLibParameterGroupNumber::DiagnosticMessage2), + buffer.data(), + payloadSize, + myControlFunction); + } + } + } + return retVal; + } + + bool DiagnosticProtocol::send_diagnostic_protocol_identification() const + { + bool retVal = false; + + if (nullptr != myControlFunction) + { + /// Bit 1 = J1939-73, Bit 2 = ISO 14230, Bit 3 = ISO 15765-3, else Reserved + constexpr std::uint8_t SUPPORTED_DIAGNOSTIC_PROTOCOLS_BITFIELD = 0x01; + std::array buffer; + + buffer.fill(0xFF); // Reserved bytes + buffer[0] = SUPPORTED_DIAGNOSTIC_PROTOCOLS_BITFIELD; + retVal = CANNetworkManager::CANNetwork.send_can_message(static_cast(CANLibParameterGroupNumber::DiagnosticProtocolIdentification), + buffer.data(), + CAN_DATA_LENGTH, + myControlFunction); + } + return retVal; + } + + bool DiagnosticProtocol::send_dm13_announce_suspension(std::uint16_t suspendTime_seconds) const + { + const std::array buffer = { + 0xFF, + 0xFF, + 0xFF, + 0xFF, + static_cast(suspendTime_seconds & 0xFF), + static_cast(suspendTime_seconds >> 8), + 0xFF, + 0xFF + }; + return CANNetworkManager::CANNetwork.send_can_message(static_cast(CANLibParameterGroupNumber::DiagnosticMessage13), + buffer.data(), + buffer.size(), + myControlFunction); + } + + bool DiagnosticProtocol::send_ecu_identification() const + { + std::string ecuIdString = ""; + const std::size_t maxComponent = get_j1939_mode() ? static_cast(ECUIdentificationFields::HardwareID) : static_cast(ECUIdentificationFields::NumberOfFields); + + for (std::size_t i = 0; i < maxComponent; i++) + { + ecuIdString.append(ecuIdentificationFields.at(i)); + } + + std::vector buffer(ecuIdString.begin(), ecuIdString.end()); + return CANNetworkManager::CANNetwork.send_can_message(static_cast(CANLibParameterGroupNumber::ECUIdentificationInformation), + buffer.data(), + buffer.size(), + myControlFunction); + } + + bool DiagnosticProtocol::send_product_identification() const + { + std::string productIdString = productIdentificationCode + "*" + productIdentificationBrand + "*" + productIdentificationModel + "*"; + std::vector buffer(productIdString.begin(), productIdString.end()); + + return CANNetworkManager::CANNetwork.send_can_message(static_cast(CANLibParameterGroupNumber::ProductIdentification), + buffer.data(), + buffer.size(), + myControlFunction); + } + + bool DiagnosticProtocol::send_software_identification() const + { + bool retVal = false; + + if (!softwareIdentificationFields.empty()) + { + std::string softIDString = ""; + + std::for_each(softwareIdentificationFields.begin(), + softwareIdentificationFields.end(), + [&softIDString](const std::string &field) { + softIDString.append(field); + softIDString.append("*"); + }); + + std::vector buffer(softIDString.begin(), softIDString.end()); + retVal = CANNetworkManager::CANNetwork.send_can_message(static_cast(CANLibParameterGroupNumber::SoftwareIdentification), + buffer.data(), + buffer.size(), + myControlFunction); + } + return retVal; + } + + bool DiagnosticProtocol::process_all_dm22_responses() + { + bool retVal = false; + + if (!dm22ResponseQueue.empty()) + { + std::size_t numberOfMessage = dm22ResponseQueue.size(); + + for (std::size_t i = 0; i < numberOfMessage; i++) + { + std::array buffer; + DM22Data currentMessageData = dm22ResponseQueue.back(); + + if (currentMessageData.nack) + { + if (currentMessageData.clearActive) + { + buffer[0] = static_cast(DM22ControlByte::NegativeAcknowledgeOfActiveDTCClear); + buffer[1] = currentMessageData.nackIndicator; + } + else + { + buffer[0] = static_cast(DM22ControlByte::NegativeAcknowledgeOfPreviouslyActiveDTCClear); + buffer[1] = currentMessageData.nackIndicator; + } + } + else + { + if (currentMessageData.clearActive) + { + buffer[0] = static_cast(DM22ControlByte::PositiveAcknowledgeOfActiveDTCClear); + buffer[1] = 0xFF; + } + else + { + buffer[0] = static_cast(DM22ControlByte::PositiveAcknowledgeOfPreviouslyActiveDTCClear); + buffer[1] = 0xFF; + } + } + + buffer[2] = 0xFF; + buffer[3] = 0xFF; + buffer[4] = 0xFF; + buffer[5] = static_cast(currentMessageData.suspectParameterNumber & 0xFF); + buffer[6] = static_cast((currentMessageData.suspectParameterNumber >> 8) & 0xFF); + buffer[7] = static_cast(((currentMessageData.suspectParameterNumber >> 16) << 5) & 0xE0); + buffer[7] |= (currentMessageData.failureModeIdentifier & 0x1F); + + retVal = CANNetworkManager::CANNetwork.send_can_message(static_cast(CANLibParameterGroupNumber::DiagnosticMessage22), + buffer.data(), + buffer.size(), + myControlFunction, + currentMessageData.destination); + if (retVal) + { + dm22ResponseQueue.pop_back(); + } + } + } + return retVal; + } + + void DiagnosticProtocol::process_message(const CANMessage &message) + { + if (((nullptr == message.get_destination_control_function()) && + (BROADCAST_CAN_ADDRESS == message.get_identifier().get_destination_address())) || + (message.get_destination_control_function() == myControlFunction)) + { + switch (message.get_identifier().get_parameter_group_number()) + { + case static_cast(CANLibParameterGroupNumber::DiagnosticMessage13): + { + if (parse_j1939_network_states(message)) + { + lastDM13ReceivedTimestamp = SystemTiming::get_timestamp_ms(); + } + } + break; + + case static_cast(CANLibParameterGroupNumber::DiagnosticMessage22): + { + if (CAN_DATA_LENGTH == message.get_data_length()) + { + const auto &messageData = message.get_data(); + + DM22Data tempDM22Data; + bool wasDTCCleared = false; + + tempDM22Data.suspectParameterNumber = messageData.at(5); + tempDM22Data.suspectParameterNumber |= (messageData.at(6) << 8); + tempDM22Data.suspectParameterNumber |= (((messageData.at(7) & 0xE0) >> 5) << 16); + tempDM22Data.failureModeIdentifier = (messageData.at(7) & 0x1F); + tempDM22Data.destination = message.get_source_control_function(); + tempDM22Data.nackIndicator = 0; + tempDM22Data.clearActive = false; + + switch (messageData.at(0)) + { + case static_cast(DM22ControlByte::RequestToClearActiveDTC): + { + tempDM22Data.clearActive = true; + + for (auto dtc = activeDTCList.begin(); dtc != activeDTCList.end(); dtc++) + { + if ((tempDM22Data.suspectParameterNumber == dtc->suspectParameterNumber) && + (tempDM22Data.failureModeIdentifier == static_cast(dtc->failureModeIdentifier))) + { + inactiveDTCList.push_back(*dtc); + activeDTCList.erase(dtc); + wasDTCCleared = true; + tempDM22Data.nack = false; + + dm22ResponseQueue.push_back(tempDM22Data); + txFlags.set_flag(static_cast(TransmitFlags::DM22)); + break; + } + } + + if (!wasDTCCleared) + { + tempDM22Data.nack = true; + + // Since we didn't find the DTC in the active list, we check the inactive to determine the proper NACK reason + for (const auto &dtc : inactiveDTCList) + { + if ((tempDM22Data.suspectParameterNumber == dtc.suspectParameterNumber) && + (tempDM22Data.failureModeIdentifier == static_cast(dtc.failureModeIdentifier))) + { + // The DTC was active, but is inactive now, so we NACK with the proper reason + tempDM22Data.nackIndicator = static_cast(DM22NegativeAcknowledgeIndicator::DTCNoLongerActive); + break; + } + } + + if (0 == tempDM22Data.nackIndicator) + { + // DTC is in neither list. NACK with the reason that we don't know anything about it + tempDM22Data.nackIndicator = static_cast(DM22NegativeAcknowledgeIndicator::UnknownOrDoesNotExist); + } + dm22ResponseQueue.push_back(tempDM22Data); + txFlags.set_flag(static_cast(TransmitFlags::DM22)); + } + } + break; + + case static_cast(DM22ControlByte::RequestToClearPreviouslyActiveDTC): + { + for (auto dtc = inactiveDTCList.begin(); dtc != inactiveDTCList.end(); dtc++) + { + if ((tempDM22Data.suspectParameterNumber == dtc->suspectParameterNumber) && + (tempDM22Data.failureModeIdentifier == static_cast(dtc->failureModeIdentifier))) + { + inactiveDTCList.erase(dtc); + wasDTCCleared = true; + tempDM22Data.nack = false; + + dm22ResponseQueue.push_back(tempDM22Data); + txFlags.set_flag(static_cast(TransmitFlags::DM22)); + break; + } + } + + if (!wasDTCCleared) + { + tempDM22Data.nack = true; + + // Since we didn't find the DTC in the inactive list, we check the active to determine the proper NACK reason + for (const auto &dtc : activeDTCList) + { + if ((tempDM22Data.suspectParameterNumber == dtc.suspectParameterNumber) && + (tempDM22Data.failureModeIdentifier == static_cast(dtc.failureModeIdentifier))) + { + // The DTC was inactive, but is active now, so we NACK with the proper reason + tempDM22Data.nackIndicator = static_cast(DM22NegativeAcknowledgeIndicator::DTCNoLongerPreviouslyActive); + break; + } + } + + if (0 == tempDM22Data.nackIndicator) + { + // DTC is in neither list. NACK with the reason that we don't know anything about it + tempDM22Data.nackIndicator = static_cast(DM22NegativeAcknowledgeIndicator::UnknownOrDoesNotExist); + } + dm22ResponseQueue.push_back(tempDM22Data); + txFlags.set_flag(static_cast(TransmitFlags::DM22)); + } + } + break; + + default: + break; + } + } + } + break; + + default: + break; + } + } + } + + void DiagnosticProtocol::process_message(const CANMessage &message, void *parent) + { + if (nullptr != parent) + { + reinterpret_cast(parent)->process_message(message); + } + } + + bool DiagnosticProtocol::parse_j1939_network_states(const CANMessage &message) + { + bool retVal = false; + + if ((CAN_DATA_LENGTH == message.get_data_length()) && + (static_cast(CANLibParameterGroupNumber::DiagnosticMessage13) == message.get_identifier().get_parameter_group_number())) + { + const auto &messageData = message.get_data(); + + auto command = static_cast((messageData[0] & (DM13_NETWORK_BITMASK << (DM13_BITS_PER_NETWORK * static_cast(networkType)))) >> (DM13_BITS_PER_NETWORK * static_cast(networkType))); + switch (command) + { + case StopStartCommand::StopBroadcast: + { + broadcastState = false; + + switch (static_cast(messageData.at(3) & 0x0F)) + { + case SuspendSignalState::TemporarySuspension: + case SuspendSignalState::PartialTemporarySuspension: + { + std::uint16_t suspensionTime = message.get_uint16_at(3); + + if (suspensionTime < MAX_DM13_CUSTOM_SUSPEND_TIME_MS) + { + customDM13SuspensionTime = suspensionTime; + } + else + { + customDM13SuspensionTime = 0; + } + } + break; + + case SuspendSignalState::IndefiniteSuspension: + case SuspendSignalState::PartialIndefiniteSuspension: + { + customDM13SuspensionTime = 0; + } + break; + + default: + break; + } + } + break; + + case StopStartCommand::StartBroadcast: + broadcastState = true; + break; + + default: + break; + } + + // Check current data link + command = static_cast((messageData[0] & (DM13_NETWORK_BITMASK << (DM13_BITS_PER_NETWORK * static_cast(NetworkType::CurrentDataLink)))) >> (DM13_BITS_PER_NETWORK * static_cast(NetworkType::CurrentDataLink))); + switch (command) + { + case StopStartCommand::StopBroadcast: + { + broadcastState = false; + + switch (static_cast(messageData.at(3) & 0x0F)) + { + case SuspendSignalState::TemporarySuspension: + case SuspendSignalState::PartialTemporarySuspension: + { + std::uint16_t suspensionTime = message.get_uint16_at(3); + + if (suspensionTime < MAX_DM13_CUSTOM_SUSPEND_TIME_MS) + { + customDM13SuspensionTime = suspensionTime; + } + else + { + customDM13SuspensionTime = 0; + } + } + break; + + case SuspendSignalState::IndefiniteSuspension: + case SuspendSignalState::PartialIndefiniteSuspension: + { + customDM13SuspensionTime = 0; + } + break; + + default: + break; + } + } + break; + + case StopStartCommand::StartBroadcast: + { + broadcastState = true; + customDM13SuspensionTime = 0; + } + break; + + default: + break; + } + retVal = true; + } + return retVal; + } + + bool DiagnosticProtocol::process_parameter_group_number_request(std::uint32_t parameterGroupNumber, + std::shared_ptr requestingControlFunction, + bool &acknowledge, + AcknowledgementType &acknowledgementType) + { + bool retVal = false; + acknowledge = false; + acknowledgementType = AcknowledgementType::Negative; + + if (nullptr != requestingControlFunction) + { + switch (parameterGroupNumber) + { + case static_cast(CANLibParameterGroupNumber::DiagnosticMessage1): + { + txFlags.set_flag(static_cast(TransmitFlags::DM1)); + retVal = true; + } + break; + + case static_cast(CANLibParameterGroupNumber::DiagnosticMessage2): + { + txFlags.set_flag(static_cast(TransmitFlags::DM2)); + retVal = true; + } + break; + + case static_cast(CANLibParameterGroupNumber::DiagnosticMessage3): + { + clear_inactive_diagnostic_trouble_codes(); + acknowledge = true; + acknowledgementType = AcknowledgementType::Positive; + retVal = true; + } + break; + + case static_cast(CANLibParameterGroupNumber::DiagnosticMessage11): + { + clear_active_diagnostic_trouble_codes(); + acknowledge = true; + acknowledgementType = AcknowledgementType::Positive; + retVal = true; + } + break; + + case static_cast(CANLibParameterGroupNumber::ProductIdentification): + { + txFlags.set_flag(static_cast(TransmitFlags::ProductIdentification)); + retVal = true; + } + break; + + case static_cast(CANLibParameterGroupNumber::DiagnosticProtocolIdentification): + { + txFlags.set_flag(static_cast(TransmitFlags::DiagnosticProtocolID)); + retVal = true; + } + break; + + case static_cast(CANLibParameterGroupNumber::SoftwareIdentification): + { + txFlags.set_flag(static_cast(TransmitFlags::SoftwareIdentification)); + retVal = true; + } + break; + + case static_cast(CANLibParameterGroupNumber::ECUIdentificationInformation): + { + txFlags.set_flag(static_cast(TransmitFlags::ECUIdentification)); + retVal = true; + } + break; + + default: + { + // This PGN request is not handled by the diagnostic protocol + } + break; + } + } + return retVal; + } + + bool DiagnosticProtocol::process_parameter_group_number_request(std::uint32_t parameterGroupNumber, + std::shared_ptr requestingControlFunction, + bool &acknowledge, + AcknowledgementType &acknowledgementType, + void *parentPointer) + { + bool retVal = false; + + if (nullptr != parentPointer) + { + retVal = reinterpret_cast(parentPointer)->process_parameter_group_number_request(parameterGroupNumber, requestingControlFunction, acknowledge, acknowledgementType); + } + return retVal; + } + + void DiagnosticProtocol::process_flags(std::uint32_t flag, void *parentPointer) + { + if (nullptr != parentPointer) + { + auto *parent = reinterpret_cast(parentPointer); + bool transmitSuccessful = false; + + switch (flag) + { + case static_cast(TransmitFlags::DM1): + { + transmitSuccessful = parent->send_diagnostic_message_1(); + + if (transmitSuccessful) + { + parent->lastDM1SentTimestamp = SystemTiming::get_timestamp_ms(); + } + } + break; + + case static_cast(TransmitFlags::DM2): + { + transmitSuccessful = parent->send_diagnostic_message_2(); + } + break; + + case static_cast(TransmitFlags::DiagnosticProtocolID): + { + transmitSuccessful = parent->send_diagnostic_protocol_identification(); + } + break; + + case static_cast(TransmitFlags::ProductIdentification): + { + transmitSuccessful = parent->send_product_identification(); + } + break; + + case static_cast(TransmitFlags::DM22): + { + transmitSuccessful = parent->process_all_dm22_responses(); + } + break; + + case static_cast(TransmitFlags::ECUIdentification): + { + transmitSuccessful = parent->send_ecu_identification(); + } + break; + + case static_cast(TransmitFlags::SoftwareIdentification): + { + transmitSuccessful = parent->send_software_identification(); + } + break; + + default: + break; + } + + if (false == transmitSuccessful) + { + parent->txFlags.set_flag(flag); + } + } + } + +} // namespace isobus diff --git a/src/isobus_diagnostic_protocol.hpp b/src/isobus_diagnostic_protocol.hpp new file mode 100644 index 0000000..e116c0d --- /dev/null +++ b/src/isobus_diagnostic_protocol.hpp @@ -0,0 +1,510 @@ +//================================================================================================ +/// @file isobus_diagnostic_protocol.hpp +/// +/// @brief A protocol that handles the ISO 11783-12 Diagnostic Protocol and some J1939 DMs. +/// @details This protocol manages many of the messages defined in ISO 11783-12 +/// and a subset of the messages defined in SAE J1939-73 and SAE J1939-71. +/// The ISO-11783 definition of some of these is based on the J1939 definition with some tweaks. +/// You can select if you want the protocol to behave like J1939 by calling set_j1939_mode. +/// One of the messages this protocol supports is the DM1 message. +/// The DM1 is sent via BAM, which has some implications to your application, +/// as only 1 BAM can be active at a time. This message +/// is sent at 1 Hz. In ISOBUS mode, unlike in J1939, the message is discontinued when no DTCs are active to +/// minimize bus load. Also, ISO-11783 does not utilize or support lamp status. +/// Other messages this protocol supports include: DM2, DM3, DM11, DM13, DM22, software ID, and Product ID. +/// +/// @note DM13 has two primary functions. It may be used as a command, from either a tool or an +/// ECU, directed to a single controller or to all controllers to request the receiving +/// controller(s) to stop or start broadcast messages. Additionally, it may be used by an ECU +/// to inform other nodes that the sender is about to suspend its normal broadcast due to +/// commands other than a SAE J1939 DM13 command received on that same network segment. +/// The broadcast messages stopped, started, or suspended may be on networks other than SAE J1939. +/// This is not a message to ignore all communications. It is a message to minimize network traffic. +/// +/// @attention It is recognized that some network messages may be required to continue even during +/// the "stop broadcast" condition. You MUST handle this in your application, as the stack cannot +/// decide what messages are required without context. In other words, you must opt-in to make +/// your application layer messages adhere to DM13 requests by explicitly calling the functions +/// on this protocol (using get_diagnostic_protocol_by_internal_control_function) +/// to check if you should send it. +/// +/// @author Adrian Del Grosso +/// +/// @copyright 2022 Adrian Del Grosso +//================================================================================================ + +#ifndef ISOBUS_DIAGNOSTIC_PROTOCOL_HPP +#define ISOBUS_DIAGNOSTIC_PROTOCOL_HPP + +#include "can_internal_control_function.hpp" +#include "can_protocol.hpp" +#include "isobus_functionalities.hpp" +#include "processing_flags.hpp" + +#include +#include +#include +#include + +namespace isobus +{ + //================================================================================================ + /// @class DiagnosticProtocol + /// @brief Manages the DM1, DM2, and DM3 messages for ISO11783 or J1939 + //================================================================================================ + class DiagnosticProtocol + { + public: + /// @brief Enumerates the different fields in the ECU identification message + enum class ECUIdentificationFields : std::uint8_t + { + PartNumber = 0, ///< The part number of the physical ECU + SerialNumber, ///< The serial number of the physical ECU + Location, ///< The location of the ECU within a network + Type, ///< The type of ECU. One example of a use of the ECU type could be for classifying ECU capabilities, such as I/O. + ManufacturerName, ///< Manufacturer name string + HardwareID, ///< ISO 11783 only, This parameter is used to associate the hardware version of an ECU connected to the ISO 11783 network to a conformance test report of that hardware + NumberOfFields ///< The number of fields currently defined in the ISO standard + }; + + /// @brief The DTC lamp status as defined in J1939-73. Not used when in ISO11783 mode + enum class LampStatus + { + None, + MalfunctionIndicatorLampSolid, ///< A lamp used to relay only emissions-related trouble code information + MalfunctionIndicatorLampSlowFlash, ///< A lamp used to relay only emissions-related trouble code information + MalfunctionIndicatorLampFastFlash, ///< A lamp used to relay only emissions-related trouble code information + RedStopLampSolid, ///< This lamp is used to relay trouble code information that is of a severe-enough condition that it warrants stopping the vehicle + RedStopLampSlowFlash, ///< This lamp is used to relay trouble code information that is of a severe-enough condition that it warrants stopping the vehicle + RedStopLampFastFlash, ///< This lamp is used to relay trouble code information that is of a severe-enough condition that it warrants stopping the vehicle + AmberWarningLampSolid, ///< This lamp is used to relay trouble code information that is reporting a problem with the vehicle system but the vehicle need not be immediately stopped. + AmberWarningLampSlowFlash, ///< This lamp is used to relay trouble code information that is reporting a problem with the vehicle system but the vehicle need not be immediately stopped. + AmberWarningLampFastFlash, ///< This lamp is used to relay trouble code information that is reporting a problem with the vehicle system but the vehicle need not be immediately stopped. + EngineProtectLampSolid, ///< This lamp is used to relay trouble code information that is reporting a problem with a vehicle system that is most probably not electronic sub-system related + EngineProtectLampSlowFlash, ///< This lamp is used to relay trouble code information that is reporting a problem with a vehicle system that is most probably not electronic sub-system related + EngineProtectLampFastFlash ///< This lamp is used to relay trouble code information that is reporting a problem with a vehicle system that is most probably not electronic sub-system related + }; + + /// @brief FMI as defined in ISO11783-12 Annex E + enum class FailureModeIdentifier + { + DataValidAboveNormalMostSevere = 0, ///< Condition is above normal as determined by the predefined most severe level limits for that particular measure of the condition + DataValidBelowNormalMostSevere = 1, ///< Condition is below normal as determined by the predefined most severe level limits for that particular measure of the condition + DataErratic = 2, ///< Erratic or intermittent data include all measurements that change at a rate not considered possible in real - world conditions + VoltageAboveNormal = 3, ///< A voltage signal, data or otherwise, is above the predefined limits that bound the range + VoltageBelowNormal = 4, ///< A voltage signal, data or otherwise, is below the predefined limits that bound the range + CurrentBelowNormal = 5, ///< A current signal, data or otherwise, is below the predefined limits that bound the range + CurrentAboveNormal = 6, ///< A current signal, data or otherwise, is above the predefined limits that bound the range + MechanicalSystemNotResponding = 7, ///< Any fault that is detected as the result of an improper mechanical adjustment, an improper response or action of a mechanical system + AbnormalFrequency = 8, ///< Any frequency or PWM signal that is outside the predefined limits which bound the signal range for frequency or duty cycle + AbnormalUpdateRate = 9, ///< Any failure that is detected when receipt of data through the data network is not at the update rate expected or required + AbnormalRateOfChange = 10, ///< Any data, exclusive of FMI 2, that are considered valid but which are changing at a rate that is outside the predefined limits that bound the rate of change for the system + RootCauseNotKnown = 11, ///< It has been detected that a failure has occurred in a particular subsystem but the exact nature of the fault is not known + BadIntelligentDevice = 12, ///< Internal diagnostic procedures have determined that the failure is one which requires the replacement of the ECU + OutOfCalibration = 13, ///< A failure that can be identified as the result of improper calibration + SpecialInstructions = 14, ///< Used when the on-board system can isolate the failure to a small number of choices but not to a single point of failure. See 11783-12 Annex E + DataValidAboveNormalLeastSevere = 15, ///< Condition is above what would be considered normal as determined by the predefined least severe level limits for that particular measure of the condition + DataValidAboveNormalModeratelySevere = 16, ///< Condition is above what would be considered normal as determined by the predefined moderately severe level limits for that particular measure of the condition + DataValidBelowNormalLeastSevere = 17, ///< Condition is below what would be considered normal as determined by the predefined least severe level limits for that particular measure of the condition + DataValidBelowNormalModeratelySevere = 18, ///< Condition is below what would be considered normal as determined by the predefined moderately severe level limits for that particular measure of the condition + ReceivedNetworkDataInError = 19, ///< Any failure that is detected when the data received through the network are found replaced by the �error indicator� value 0xFE + ConditionExists = 31 ///< The condition that is identified by the SPN exists when no applicable FMI exists (any other error) + }; + + /// @brief A set of transmit flags to manage sending DM1, DM2, and protocol ID + enum class TransmitFlags + { + DM1 = 0, ///< A flag to manage sending the DM1 message + DM2, ///< A flag to manage sending the DM2 message + DiagnosticProtocolID, ///< A flag to manage sending the Diagnostic protocol ID message + ProductIdentification, ///< A flag to manage sending the product identification message + DM22, ///< Process queued up DM22 responses + ECUIdentification, ///< A flag to manage sending the ECU ID message + SoftwareIdentification, ///< A flag to manage sending the software ID message + + NumberOfFlags ///< The number of flags in the enum + }; + + /// @brief Enumerates the different networks in the DM13 + enum class NetworkType : std::uint8_t + { + SAEJ1939Network1PrimaryVehicleNetwork = 0, + SAEJ1922Network = 1, + SAEJ1587Network = 2, + CurrentDataLink = 3, + OtherManufacturerSpecifiedPort = 4, + SAEJ1850Network = 5, + ISO9141Network = 6, + SAEJ1939Network2 = 7, + SAEJ1939Network4 = 8, + ProprietaryNetwork2 = 9, + ProprietaryNetwork1 = 10, + SAEJ1939Network3 = 11, + SAEJ1939Network5 = 25, + SAEJ1939Network6 = 26, + SAEJ1939Network7 = 27, + SAEJ1939Network8 = 28, + SAEJ1939Network11 = 29, + SAEJ1939Network10 = 30, + SAEJ1939Network9 = 31, + Reserved = 32 + }; + + /// @brief Enumerates the commands in the DM13 + enum class StopStartCommand : std::uint8_t + { + StopBroadcast = 0, ///< Stop broadcast + StartBroadcast = 1, ///< Start broadcast + Reserved = 2, ///< SAE Reserved + DontCareNoAction = 3 ///< Don’t Care/take no action (leave as is) + }; + + /// @brief Enumerates the different suspend signals for DM13 + enum class SuspendSignalState : std::uint8_t + { + IndefiniteSuspension = 0, ///< Indefinite suspension of all broadcasts + PartialIndefiniteSuspension = 1, ///< Indefinite suspension of some messages + TemporarySuspension = 2, ///< Temporary suspension of all broadcasts + PartialTemporarySuspension = 3, ///< Temporary suspension of some messages + Resuming = 4, ///< Resuming normal broadcast pattern + NotAvailable = 15 /// < N/A + }; + + //================================================================================================ + /// @class DiagnosticTroubleCode + /// @brief A storage class for describing a complete DTC + //================================================================================================ + class DiagnosticTroubleCode + { + public: + /// @brief Constructor for a DTC, sets default values at construction time + DiagnosticTroubleCode() = default; + + /// @brief Constructor for a DTC, sets all values explicitly + /// @param[in] spn The suspect parameter number + /// @param[in] fmi The failure mode indicator + /// @param[in] lamp The J1939 lamp status. Set to `None` if you don't care about J1939 + DiagnosticTroubleCode(std::uint32_t spn, FailureModeIdentifier fmi, LampStatus lamp); + + /// @brief A useful way to compare DTC objects to each other for equality + /// @param[in] obj The "rhs" of the comparison + /// @returns `true` if the objects were equal + bool operator==(const DiagnosticTroubleCode &obj) const; + + /// @brief Returns the occurrence count, which will be kept track of by the protocol + /// @returns The occurrence count (0 to 126 with 127 being not available) + std::uint8_t get_occurrence_count() const; + + /// @brief Returns the suspect parameter number + /// @returns The suspect parameter number + std::uint32_t get_suspect_parameter_number() const; + + /// @brief Returns the failure mode indicator + /// @returns The failure mode indicator + FailureModeIdentifier get_failure_mode_identifier() const; + + private: + friend class DiagnosticProtocol; + std::uint32_t suspectParameterNumber = 0xFFFFFFFF; ///< This 19-bit number is used to identify the item for which diagnostics are being reported + FailureModeIdentifier failureModeIdentifier = FailureModeIdentifier::ConditionExists; ///< The FMI defines the type of failure detected in the sub-system identified by an SPN + LampStatus lampState = LampStatus::None; ///< The J1939 lamp state for this DTC + std::uint8_t occurrenceCount = 0; ///< Number of times the DTC has been active (0 to 126 with 127 being not available) + }; + + /// @brief The constructor for this protocol + /// @param[in] internalControlFunction The internal control function that owns this protocol and will be used to send messages + /// @param[in] networkType The type of diagnostic network that this protocol will reflect + DiagnosticProtocol(std::shared_ptr internalControlFunction, + NetworkType networkType = NetworkType::ProprietaryNetwork1); + + /// @brief The destructor for this protocol + ~DiagnosticProtocol(); + + /// @brief The protocol's initializer function + bool initialize(); + + /// @brief Returns if the protocol has been initialized + /// @returns true if the protocol has been initialized, otherwise false + bool get_initialized() const; + + /// @brief The protocol's terminate function + void terminate(); + + /// @brief Updates the diagnostic protocol + void update(); + + /// @brief Enables the protocol to run in J1939 mode instead of ISO11783 mode + /// @details See ISO11783-12 and J1939-73 for a complete explanation of the differences + /// @param[in] value The desired mode. `true` for J1939 mode, `false` for ISO11783 mode + void set_j1939_mode(bool value); + + /// @brief Returns `true` if the protocol is in J1939 mode instead of ISO11783 mode, `false` if using ISO11783 mode + /// @returns `true` if the protocol is in J1939 mode instead of ISO11783 mode, `false` if using ISO11783 mode + bool get_j1939_mode() const; + + /// @brief Use this interface to configure your CF's functionalities. + /// This info will be reported to any ECU that requests it and you are + /// required to support it on an ISOBUS. + /// @note By default, this interface will only report that your ECU is a + /// "Minimum Control Function". + ControlFunctionFunctionalities ControlFunctionFunctionalitiesMessageInterface; + + /// @brief Clears the list of active DTCs and makes them all inactive + void clear_active_diagnostic_trouble_codes(); + + /// @brief Clears the list of inactive DTCs and clears occurrence counts + void clear_inactive_diagnostic_trouble_codes(); + + /// @brief Clears all previously configured software ID fields set with set_software_id_field + void clear_software_id_fields(); + + /// @brief Sets one of the ECU identification strings for the ECU ID message + /// @details See ECUIdentificationFields for a brief description of the fields + /// @note The fields in this message are optional and separated by an ASCII "*". It is not necessary to include parametric + /// data for all fields. Any additional ECU identification fields defined in the future will be appended at the end. + /// @attention Do not include the "*" character in your field values and only use HardwareID when not in J1939 mode. + /// @param[in] field The field to set + /// @param[in] value The string value associated with the ECU ID field + void set_ecu_id_field(ECUIdentificationFields field, const std::string &value); + + /// @brief Adds a DTC to the active list, or removes one from the active list + /// @details When you call this function with a DTC and `true`, it will be added to the DM1 message. + /// When you call it with a DTC and `false` it will be moved to the inactive list. + /// If you get `false` as a return value, either the DTC was already in the target state or the data was not valid + /// @param[in] dtc A diagnostic trouble code whose state should be altered + /// @param[in] active Sets if the DTC is currently active or not + /// @returns True if the DTC was added/removed from the list, false if DTC was not valid or target state is invalid + bool set_diagnostic_trouble_code_active(const DiagnosticTroubleCode &dtc, bool active); + + /// @brief Returns if a DTC is active + /// @param[in] dtc A diagnostic trouble code whose state should be altered + /// @returns `true` if the DTC was in the active list + bool get_diagnostic_trouble_code_active(const DiagnosticTroubleCode &dtc); + + /// @brief Sets the product ID code used in the diagnostic protocol "Product Identification" message (PGN 0xFC8D) + /// @details The product identification code, as assigned by the manufacturer, corresponds with the number on the + /// type plate of a product. For vehicles, this number can be the same as the VIN. For stand-alone systems, such as VTs, + /// this number can be the same as the ECU ID number. The combination of the product identification code and brand shall + /// make the product globally unique. + /// @param value The ascii product identification code, up to 50 characters long + /// @returns true if the value was set, false if the string is too long + bool set_product_identification_code(const std::string &value); + + /// @brief Sets the product identification brand used in the diagnostic protocol "Product Identification" message (PGN 0xFC8D) + /// @details The product identification brand specifies the brand of a product. The combination of the product ID code and brand + /// shall make the product unique in the world. + /// @param value The ascii product brand, up to 50 characters long + /// @returns true if the value was set, false if the string is too long + bool set_product_identification_brand(const std::string &value); + + /// @brief Sets the product identification model used in the diagnostic protocol "Product Identification" message (PGN 0xFC8D) + /// @details The product identification model specifies a unique product within a brand. + /// @param value The ascii model string, up to 50 characters + /// @returns true if the value was set, false if the string is too long + bool set_product_identification_model(const std::string &value); + + /// @brief Adds an ascii string to this internal control function's software ID + /// @details Use this to identify the software version of your application. + /// Separate fields will be transmitted with a `*` delimeter. + /// For example, if your main application's version is 1.00, and you have a bootloader + /// that is version 2.00, you could set field `0` to be "App v1.00" and + /// field `1` to be "Bootloader v2.00", and it will be transmitted on request as: + /// "App v1.00*Bootloader v2.00*" in accordance with ISO 11783-12 + /// You can remove a field by setting it to "" + /// @param[in] index The field index to set + /// @param[in] value The software ID string to add + void set_software_id_field(std::uint32_t index, const std::string &value); + + /// @brief Informs the diagnostic protocol that you are going to suspend broadcasts + /// @details When you call this, DM1 and other broadcasts that come from this protocol will be stopped for the duration specified. + /// @param[in] suspendTime_seconds If you know the time for which broadcasts will be suspended, put it here, otherwise 0xFFFF + /// @returns `true` if the message was sent, otherwise `false` + bool suspend_broadcasts(std::uint16_t suspendTime_seconds = 0xFFFF); + + /// @brief Gets the current broadcast state for the connected network type + /// @returns True if broadcasts are currently allowed, false if broadcasts are suspended for the connected network + bool get_broadcast_state() const; + + private: + /// @brief Lists the different lamps in J1939-73 + enum class Lamps + { + MalfunctionIndicatorLamp, ///< The "MIL" + RedStopLamp, ///< The "RSL" + AmberWarningLamp, ///< The "AWL" + ProtectLamp ///< The engine protect lamp + }; + + /// @brief Enumerates lamp flash states in J1939 + enum class FlashState + { + Solid, ///< Solid / no flash + Slow, ///< Slow flash + Fast ///< Fast flash + }; + + /// @brief The DM22 multiplexer bytes. All bytes not given a value here are reserved by SAE + enum class DM22ControlByte : std::uint8_t + { + RequestToClearPreviouslyActiveDTC = 0x01, ///< Clear a previously active DTC + PositiveAcknowledgeOfPreviouslyActiveDTCClear = 0x02, ///< ACK for clearing a previously active DTC + NegativeAcknowledgeOfPreviouslyActiveDTCClear = 0x03, ///< NACK for clearing a previously active DTC + RequestToClearActiveDTC = 0x11, ///< Clear an active DTC + PositiveAcknowledgeOfActiveDTCClear = 0x12, ///< ACK clearing an active DTC + NegativeAcknowledgeOfActiveDTCClear = 0x13 ///< NACK clearing an active DTC + }; + + /// @brief The negative acknowledge (NACK) reasons for a DM22 message + enum class DM22NegativeAcknowledgeIndicator : std::uint8_t + { + General = 0x00, ///< General negative acknowledge + AccessDenied = 0x01, ///< Security denied access + UnknownOrDoesNotExist = 0x02, ///< The DTC is unknown or does not exist + DTCNoLongerPreviouslyActive = 0x03, ///< The DTC in in the active list but it was requested to clear from inactive list + DTCNoLongerActive = 0x04 ///< DTC is inactive, not active, but active was requested to be cleared + }; + + /// @brief A structure to hold data about DM22 responses we need to send + struct DM22Data + { + std::shared_ptr destination; ///< Destination for the DM22 message + std::uint32_t suspectParameterNumber; ///< SPN of the DTC for the DM22 + std::uint8_t failureModeIdentifier; ///< FMI of the DTC for the DM22 + std::uint8_t nackIndicator; ///< The NACK reason, if applicable + bool clearActive; ///< true if the DM22 was for an active DTC, false for previously active + bool nack; ///< true if we are sending a NACK instead of PACK. Determines if we use nackIndicator + }; + + static constexpr std::uint32_t DM_MAX_FREQUENCY_MS = 1000; ///< You are technically allowed to send more than this under limited circumstances, but a hard limit saves 4 RAM bytes per DTC and has BAM benefits + static constexpr std::uint32_t DM13_HOLD_SIGNAL_TRANSMIT_INTERVAL_MS = 5000; ///< Defined in 5.7.13.13 SPN 1236 + static constexpr std::uint32_t DM13_TIMEOUT_MS = 6000; ///< The timeout in 5.7.13 after which nodes shall revert back to the normal broadcast state + static constexpr std::uint16_t MAX_PAYLOAD_SIZE_BYTES = 1785; ///< DM 1 and 2 are limited to the BAM message max, because ETP does not allow global destinations + static constexpr std::uint16_t MAX_DM13_CUSTOM_SUSPEND_TIME_MS = 64255; ///< The max valid value for a DM13 suspension time in milliseconds + static constexpr std::uint8_t DM_PAYLOAD_BYTES_PER_DTC = 4; ///< The number of payload bytes per DTC that gets encoded into the messages + static constexpr std::uint8_t PRODUCT_IDENTIFICATION_MAX_STRING_LENGTH = 50; ///< The max string length allowed in the fields of product ID, as defined in ISO 11783-12 + static constexpr std::uint8_t DM13_NUMBER_OF_J1939_NETWORKS = 11; ///< The number of networks in DM13 that are set aside for J1939 + static constexpr std::uint8_t DM13_NETWORK_BITMASK = 0x03; ///< Used to mask the network SPN values + static constexpr std::uint8_t DM13_BITS_PER_NETWORK = 2; ///< Number of bits for the network SPNs + + /// @brief A utility function to get the CAN representation of a FlashState + /// @param flash The flash state to convert + /// @returns The two bit lamp state for CAN + std::uint8_t convert_flash_state_to_byte(FlashState flash) const; + + /// @brief A utility function that will clean up PGN registrations + void deregister_all_pgns(); + + /// @brief This is a way to find the overall lamp states to report + /// @details This searches the active DTC list to find if a lamp is on or off, and to find the overall flash state for that lamp. + /// Basically, since the lamp states are global to the CAN message, we need a way to resolve the "total" lamp state from the list. + /// @param[in] targetLamp The lamp to find the status of + /// @param[out] flash How the lamp should be flashing + /// @param[out] lampOn If the lamp state is on for any DTC + void get_active_list_lamp_state_and_flash_state(Lamps targetLamp, FlashState &flash, bool &lampOn) const; + + /// @brief This is a way to find the overall lamp states to report + /// @details This searches the inactive DTC list to find if a lamp is on or off, and to find the overall flash state for that lamp. + /// Basically, since the lamp states are global to the CAN message, we need a way to resolve the "total" lamp state from the list. + /// @param[in] targetLamp The lamp to find the status of + /// @param[out] flash How the lamp should be flashing + /// @param[out] lampOn If the lamp state is on for any DTC + void get_inactive_list_lamp_state_and_flash_state(Lamps targetLamp, FlashState &flash, bool &lampOn) const; + + /// @brief Sends a DM1 encoded CAN message + /// @returns true if the message was sent, otherwise false + bool send_diagnostic_message_1() const; + + /// @brief Sends a DM2 encoded CAN message + /// @returns true if the message was sent, otherwise false + bool send_diagnostic_message_2() const; + + /// @brief Sends a message that identifies which diagnostic protocols are supported + /// @returns true if the message was sent, otherwise false + bool send_diagnostic_protocol_identification() const; + + /// @brief Sends the DM13 to alert network devices of impending suspended broadcasts + /// @param suspendTime_seconds The number of seconds that the broadcast will be suspended for + /// @returns `true` if the message was sent, otherwise `false` + bool send_dm13_announce_suspension(std::uint16_t suspendTime_seconds) const; + + /// @brief Sends the ECU ID message + /// @returns true if the message was sent + bool send_ecu_identification() const; + + /// @brief Sends the product identification message (PGN 0xFC8D) + /// @returns true if the message was sent, otherwise false + bool send_product_identification() const; + + /// @brief Sends the software ID message + /// @returns true if the message was sent, otherwise false + bool send_software_identification() const; + + /// @brief Processes any DM22 responses from the queue + /// @details We queue responses so that we can do Tx retries if needed + /// @returns true if queue was completely processed, false if messages remain that could not be sent + bool process_all_dm22_responses(); + + /// @brief A generic way for a protocol to process a received message + /// @param[in] message A received CAN message + void process_message(const CANMessage &message); + + /// @brief A generic way for a protocol to process a received message + /// @param[in] message A received CAN message + /// @param[in] parent Provides the context to the actual TP manager object + static void process_message(const CANMessage &message, void *parent); + + /// @brief Parses out the DM13 J1939 network states from a CAN message + /// @param[in] message The message to parse from + /// @returns `true` if the message was parsed, `false` if the message was invalid + bool parse_j1939_network_states(const CANMessage &message); + + /// @brief Handles PGN requests for the diagnostic protocol + /// @param[in] parameterGroupNumber The PGN being requested + /// @param[in] requestingControlFunction The control function that is requesting the PGN + /// @param[out] acknowledge Tells the PGN request protocol if it should respond to the request + /// @param[out] acknowledgementType The type of acknowledgement to send to the requestor + /// @returns true if any callback was able to handle the PGN request + bool process_parameter_group_number_request(std::uint32_t parameterGroupNumber, + std::shared_ptr requestingControlFunction, + bool &acknowledge, + AcknowledgementType &acknowledgementType); + + // @brief Handles PGN requests for the diagnostic protocol + /// @param[in] parameterGroupNumber The PGN being requested + /// @param[in] requestingControlFunction The control function that is requesting the PGN + /// @param[out] acknowledge Tells the PGN request protocol if it should respond to the request + /// @param[out] acknowledgementType The type of acknowledgement to send to the requestor + /// @param[in] parentPointer Generic context variable, usually a pointer to the class that the callback was registered for + /// @returns true if any callback was able to handle the PGN request + static bool process_parameter_group_number_request(std::uint32_t parameterGroupNumber, + std::shared_ptr requestingControlFunction, + bool &acknowledge, + AcknowledgementType &acknowledgementType, + void *parentPointer); + + /// @brief A generic callback for a the class to process flags from the `ProcessingFlags` + /// @param[in] flag The flag to process + /// @param[in] parentPointer A generic context pointer to reference a specific instance of this protocol in the callback + static void process_flags(std::uint32_t flag, void *parentPointer); + + std::shared_ptr myControlFunction; ///< The internal control function that this protocol will send from + NetworkType networkType; ///< The diagnostic network type that this protocol will use + std::vector activeDTCList; ///< Keeps track of all the active DTCs + std::vector inactiveDTCList; ///< Keeps track of all the previously active DTCs + std::vector dm22ResponseQueue; ///< Maintaining a list of DM22 responses we need to send to allow for retrying in case of Tx failures + std::vector ecuIdentificationFields; ///< Stores the ECU ID fields so we can transmit them when ECU ID's PGN is requested + std::vector softwareIdentificationFields; ///< Stores the Software ID fields so we can transmit them when the PGN is requested + ProcessingFlags txFlags; ///< An instance of the processing flags to handle retries of some messages + std::string productIdentificationCode; ///< The product identification code for sending the product identification message + std::string productIdentificationBrand; ///< The product identification brand for sending the product identification message + std::string productIdentificationModel; ///< The product identification model name for sending the product identification message + std::uint32_t lastDM1SentTimestamp = 0; ///< A timestamp in milliseconds of the last time a DM1 was sent + std::uint32_t lastDM13ReceivedTimestamp = 0; ///< A timestamp in milliseconds when we last got a DM13 message + std::uint16_t customDM13SuspensionTime = 0; ///< If using a non-standard DM13 suspension time, this tracks that duration in milliseconds + bool broadcastState = true; ///< Bitfield for tracking the network broadcast state for DM13 + bool j1939Mode = false; ///< Tells the protocol to operate according to J1939 instead of ISO11783 + bool initialized = false; ///< Stores if the interface has been initialized + }; +} + +#endif // ISOBUS_DIAGNOSTIC_PROTOCOL_HPP diff --git a/src/isobus_functionalities.cpp b/src/isobus_functionalities.cpp new file mode 100644 index 0000000..b597a31 --- /dev/null +++ b/src/isobus_functionalities.cpp @@ -0,0 +1,938 @@ +//================================================================================================ +/// @file isobus_functionalities.cpp +/// +/// @brief Implements the management of the ISOBUS control function functionalities message. +/// @author Adrian Del Grosso +/// +/// @copyright 2023 Adrian Del Grosso +//================================================================================================ +#include "isobus_functionalities.hpp" +#include "can_general_parameter_group_numbers.hpp" +#include "can_stack_logger.hpp" + +#include +#include +#include + +namespace isobus +{ + ControlFunctionFunctionalities::ControlFunctionFunctionalities(std::shared_ptr sourceControlFunction) : + myControlFunction(sourceControlFunction), + txFlags(static_cast(TransmitFlags::NumberOfFlags), process_flags, this) + { + set_functionality_is_supported(Functionalities::MinimumControlFunction, 1, true); // Support the absolute minimum by default + + if (auto pgnRequestProtocol = sourceControlFunction->get_pgn_request_protocol().lock()) + { + pgnRequestProtocol->register_pgn_request_callback(static_cast(CANLibParameterGroupNumber::ControlFunctionFunctionalities), pgn_request_handler, this); + } + else + { + CANStackLogger::error("[DP]: Failed to register PGN request callback for ControlFunctionFunctionalities due to the protocol being expired"); + } + } + + ControlFunctionFunctionalities::~ControlFunctionFunctionalities() + { + if (auto pgnRequestProtocol = myControlFunction->get_pgn_request_protocol().lock()) + { + pgnRequestProtocol->remove_pgn_request_callback(static_cast(CANLibParameterGroupNumber::ControlFunctionFunctionalities), pgn_request_handler, this); + } + } + + void ControlFunctionFunctionalities::set_functionality_is_supported(Functionalities functionality, std::uint8_t functionalityGeneration, bool isSupported) + { +#if !defined CAN_STACK_DISABLE_THREADS && !defined ARDUINO + const std::lock_guard lock(functionalitiesMutex); +#endif + + auto existingFunctionality = get_functionality(functionality); + + if ((supportedFunctionalities.end() == existingFunctionality) && isSupported) + { + FunctionalityData newFunctionality(functionality); + newFunctionality.configure_default_data(); + newFunctionality.generation = functionalityGeneration; + supportedFunctionalities.emplace_back(newFunctionality); + } + else if ((supportedFunctionalities.end() != existingFunctionality) && (!isSupported)) + { + if (Functionalities::MinimumControlFunction == functionality) + { + CANStackLogger::warn("[DP]: You are disabling minimum control function functionality reporting! This is not recommended."); + } + supportedFunctionalities.erase(existingFunctionality); + } + } + + bool ControlFunctionFunctionalities::get_functionality_is_supported(Functionalities functionality) + { + bool retVal = false; +#if !defined CAN_STACK_DISABLE_THREADS && !defined ARDUINO + const std::lock_guard lock(functionalitiesMutex); +#endif + + auto existingFunctionality = get_functionality(functionality); + + if (supportedFunctionalities.end() != existingFunctionality) + { + retVal = true; + } + return retVal; + } + + std::uint8_t ControlFunctionFunctionalities::get_functionality_generation(Functionalities functionality) + { + std::uint8_t retVal = 0; + + auto existingFunctionality = get_functionality(functionality); + + if (supportedFunctionalities.end() != existingFunctionality) + { + retVal = existingFunctionality->generation; + } + return retVal; + } + + void ControlFunctionFunctionalities::set_minimum_control_function_option_state(MinimumControlFunctionOptions option, bool optionState) + { +#if !defined CAN_STACK_DISABLE_THREADS && !defined ARDUINO + const std::lock_guard lock(functionalitiesMutex); +#endif + + auto existingFunctionality = get_functionality(Functionalities::MinimumControlFunction); + + if ((supportedFunctionalities.end() != existingFunctionality) && + (option < MinimumControlFunctionOptions::Reserved)) + { + existingFunctionality->set_bit_in_option(0, static_cast(option), optionState); + } + } + + bool ControlFunctionFunctionalities::get_minimum_control_function_option_state(MinimumControlFunctionOptions option) + { + return get_functionality_byte_option(Functionalities::MinimumControlFunction, 0, static_cast(option)); + } + + void ControlFunctionFunctionalities::set_aux_O_inputs_option_state(AuxOOptions option, bool optionState) + { +#if !defined CAN_STACK_DISABLE_THREADS && !defined ARDUINO + const std::lock_guard lock(functionalitiesMutex); +#endif + + auto existingFunctionality = get_functionality(Functionalities::AuxOInputs); + + if ((supportedFunctionalities.end() != existingFunctionality) && + (option < AuxOOptions::Reserved)) + { + existingFunctionality->set_bit_in_option(0, static_cast(option), optionState); + } + } + + bool ControlFunctionFunctionalities::get_aux_O_inputs_option_state(AuxOOptions option) + { + return get_functionality_byte_option(Functionalities::AuxOInputs, 0, static_cast(option)); + } + + void ControlFunctionFunctionalities::set_aux_O_functions_option_state(AuxOOptions option, bool optionState) + { +#if !defined CAN_STACK_DISABLE_THREADS && !defined ARDUINO + const std::lock_guard lock(functionalitiesMutex); +#endif + + auto existingFunctionality = get_functionality(Functionalities::AuxOFunctions); + + if ((supportedFunctionalities.end() != existingFunctionality) && + (option < AuxOOptions::Reserved)) + { + existingFunctionality->set_bit_in_option(0, static_cast(option), optionState); + } + } + + bool ControlFunctionFunctionalities::get_aux_O_functions_option_state(AuxOOptions option) + { + return get_functionality_byte_option(Functionalities::AuxOFunctions, 0, static_cast(option)); + } + + void ControlFunctionFunctionalities::set_aux_N_inputs_option_state(AuxNOptions option, bool optionState) + { +#if !defined CAN_STACK_DISABLE_THREADS && !defined ARDUINO + const std::lock_guard lock(functionalitiesMutex); +#endif + + auto existingFunctionality = get_functionality(Functionalities::AuxNInputs); + + if ((supportedFunctionalities.end() != existingFunctionality) && + (option < AuxNOptions::Reserved)) + { + existingFunctionality->set_bit_in_option(option > AuxNOptions::SupportsType7Function ? 1 : 0, + option > AuxNOptions::SupportsType7Function ? static_cast(static_cast(option) >> 8) : static_cast(option), + optionState); + } + } + + bool ControlFunctionFunctionalities::get_aux_N_inputs_option_state(AuxNOptions option) + { + return get_functionality_byte_option(Functionalities::AuxNInputs, 0, static_cast(option)); + } + + void ControlFunctionFunctionalities::set_aux_N_functions_option_state(AuxNOptions option, bool optionState) + { +#if !defined CAN_STACK_DISABLE_THREADS && !defined ARDUINO + const std::lock_guard lock(functionalitiesMutex); +#endif + + auto existingFunctionality = get_functionality(Functionalities::AuxNFunctions); + + if ((supportedFunctionalities.end() != existingFunctionality) && + (option < AuxNOptions::Reserved)) + { + existingFunctionality->set_bit_in_option(option > AuxNOptions::SupportsType7Function ? 1 : 0, + option > AuxNOptions::SupportsType7Function ? static_cast(static_cast(option) >> 8) : static_cast(option), + optionState); + } + } + + bool ControlFunctionFunctionalities::get_aux_N_functions_option_state(AuxNOptions option) + { + return get_functionality_byte_option(Functionalities::AuxNFunctions, 0, static_cast(option)); + } + + void ControlFunctionFunctionalities::set_task_controller_geo_server_option_state(TaskControllerGeoServerOptions option, bool optionState) + { +#if !defined CAN_STACK_DISABLE_THREADS && !defined ARDUINO + const std::lock_guard lock(functionalitiesMutex); +#endif + + auto existingFunctionality = get_functionality(Functionalities::TaskControllerGeoServer); + + if ((supportedFunctionalities.end() != existingFunctionality) && + (option < TaskControllerGeoServerOptions::Reserved)) + { + existingFunctionality->set_bit_in_option(0, static_cast(option), optionState); + } + } + + bool ControlFunctionFunctionalities::get_task_controller_geo_server_option_state(TaskControllerGeoServerOptions option) + { + return get_functionality_byte_option(Functionalities::TaskControllerGeoServer, 0, static_cast(option)); + } + + void ControlFunctionFunctionalities::set_task_controller_geo_client_option(std::uint8_t numberOfControlChannels) + { +#if !defined CAN_STACK_DISABLE_THREADS && !defined ARDUINO + const std::lock_guard lock(functionalitiesMutex); +#endif + + auto existingFunctionality = get_functionality(Functionalities::TaskControllerGeoClient); + + if ((supportedFunctionalities.end() != existingFunctionality) && + (numberOfControlChannels > 0)) + { + existingFunctionality->serializedValue.at(0) = numberOfControlChannels; + } + } + + std::uint8_t ControlFunctionFunctionalities::get_task_controller_geo_client_option() + { +#if !defined CAN_STACK_DISABLE_THREADS && !defined ARDUINO + const std::lock_guard lock(functionalitiesMutex); +#endif + auto existingFunctionality = get_functionality(Functionalities::TaskControllerGeoClient); + std::uint8_t retVal = 0; + + if (supportedFunctionalities.end() != existingFunctionality) + { + retVal = existingFunctionality->serializedValue.at(0); + } + return retVal; + } + + void ControlFunctionFunctionalities::set_task_controller_section_control_server_option_state(std::uint8_t numberOfSupportedBooms, std::uint8_t numberOfSupportedSections) + { +#if !defined CAN_STACK_DISABLE_THREADS && !defined ARDUINO + const std::lock_guard lock(functionalitiesMutex); +#endif + + auto existingFunctionality = get_functionality(Functionalities::TaskControllerSectionControlServer); + + if ((supportedFunctionalities.end() != existingFunctionality) && + (numberOfSupportedBooms > 0) && + (numberOfSupportedSections > 0)) + { + existingFunctionality->serializedValue.at(0) = numberOfSupportedBooms; + existingFunctionality->serializedValue.at(1) = numberOfSupportedSections; + } + } + + std::uint8_t ControlFunctionFunctionalities::get_task_controller_section_control_server_number_supported_booms() + { +#if !defined CAN_STACK_DISABLE_THREADS && !defined ARDUINO + const std::lock_guard lock(functionalitiesMutex); +#endif + auto existingFunctionality = get_functionality(Functionalities::TaskControllerSectionControlServer); + std::uint8_t retVal = 0; + + if (supportedFunctionalities.end() != existingFunctionality) + { + retVal = existingFunctionality->serializedValue.at(0); + } + return retVal; + } + + std::uint8_t ControlFunctionFunctionalities::get_task_controller_section_control_server_number_supported_sections() + { +#if !defined CAN_STACK_DISABLE_THREADS && !defined ARDUINO + const std::lock_guard lock(functionalitiesMutex); +#endif + auto existingFunctionality = get_functionality(Functionalities::TaskControllerSectionControlServer); + std::uint8_t retVal = 0; + + if (supportedFunctionalities.end() != existingFunctionality) + { + retVal = existingFunctionality->serializedValue.at(1); + } + return retVal; + } + + void ControlFunctionFunctionalities::set_task_controller_section_control_client_option_state(std::uint8_t numberOfSupportedBooms, std::uint8_t numberOfSupportedSections) + { +#if !defined CAN_STACK_DISABLE_THREADS && !defined ARDUINO + const std::lock_guard lock(functionalitiesMutex); +#endif + + auto existingFunctionality = get_functionality(Functionalities::TaskControllerSectionControlClient); + + if ((supportedFunctionalities.end() != existingFunctionality) && + (numberOfSupportedBooms > 0) && + (numberOfSupportedSections > 0)) + { + existingFunctionality->serializedValue.at(0) = numberOfSupportedBooms; + existingFunctionality->serializedValue.at(1) = numberOfSupportedSections; + } + } + + std::uint8_t ControlFunctionFunctionalities::get_task_controller_section_control_client_number_supported_booms() + { +#if !defined CAN_STACK_DISABLE_THREADS && !defined ARDUINO + const std::lock_guard lock(functionalitiesMutex); +#endif + auto existingFunctionality = get_functionality(Functionalities::TaskControllerSectionControlClient); + std::uint8_t retVal = 0; + + if (supportedFunctionalities.end() != existingFunctionality) + { + retVal = existingFunctionality->serializedValue.at(0); + } + return retVal; + } + + std::uint8_t ControlFunctionFunctionalities::get_task_controller_section_control_client_number_supported_sections() + { +#if !defined CAN_STACK_DISABLE_THREADS && !defined ARDUINO + const std::lock_guard lock(functionalitiesMutex); +#endif + auto existingFunctionality = get_functionality(Functionalities::TaskControllerSectionControlClient); + std::uint8_t retVal = 0; + + if (supportedFunctionalities.end() != existingFunctionality) + { + retVal = existingFunctionality->serializedValue.at(1); + } + return retVal; + } + + void ControlFunctionFunctionalities::set_basic_tractor_ECU_server_option_state(BasicTractorECUOptions option, bool optionState) + { +#if !defined CAN_STACK_DISABLE_THREADS && !defined ARDUINO + const std::lock_guard lock(functionalitiesMutex); +#endif + + auto existingFunctionality = get_functionality(Functionalities::BasicTractorECUServer); + + if ((supportedFunctionalities.end() != existingFunctionality) && + (option < BasicTractorECUOptions::Reserved)) + { + existingFunctionality->set_bit_in_option(0, static_cast(option), optionState); + } + } + + bool ControlFunctionFunctionalities::get_basic_tractor_ECU_server_option_state(BasicTractorECUOptions option) + { + bool retVal = false; + + // This one is handled differently to handle the 0 value + if (BasicTractorECUOptions::TECUNotMeetingCompleteClass1Requirements == option) + { +#if !defined CAN_STACK_DISABLE_THREADS && !defined ARDUINO + const std::lock_guard lock(functionalitiesMutex); +#endif + auto existingFunctionality = get_functionality(Functionalities::BasicTractorECUServer); + + if (supportedFunctionalities.end() != existingFunctionality) + { + retVal = (0 == existingFunctionality->serializedValue.at(0)); + } + } + else + { + retVal = get_functionality_byte_option(Functionalities::BasicTractorECUServer, 0, static_cast(option)); + } + return retVal; + } + + void ControlFunctionFunctionalities::set_basic_tractor_ECU_implement_client_option_state(BasicTractorECUOptions option, bool optionState) + { +#if !defined CAN_STACK_DISABLE_THREADS && !defined ARDUINO + const std::lock_guard lock(functionalitiesMutex); +#endif + + auto existingFunctionality = get_functionality(Functionalities::BasicTractorECUImplementClient); + + if ((supportedFunctionalities.end() != existingFunctionality) && + (option < BasicTractorECUOptions::Reserved)) + { + existingFunctionality->set_bit_in_option(0, static_cast(option), optionState); + } + } + + bool ControlFunctionFunctionalities::get_basic_tractor_ECU_implement_client_option_state(BasicTractorECUOptions option) + { + bool retVal = false; + + // This one is handled differently to handle the 0 value + if (BasicTractorECUOptions::TECUNotMeetingCompleteClass1Requirements == option) + { +#if !defined CAN_STACK_DISABLE_THREADS && !defined ARDUINO + const std::lock_guard lock(functionalitiesMutex); +#endif + auto existingFunctionality = get_functionality(Functionalities::BasicTractorECUImplementClient); + + if (supportedFunctionalities.end() != existingFunctionality) + { + retVal = (0 == existingFunctionality->serializedValue.at(0)); + } + } + else + { + retVal = get_functionality_byte_option(Functionalities::BasicTractorECUImplementClient, 0, static_cast(option)); + } + return retVal; + } + + void ControlFunctionFunctionalities::set_tractor_implement_management_server_option_state(TractorImplementManagementOptions option, bool optionState) + { +#if !defined CAN_STACK_DISABLE_THREADS && !defined ARDUINO + const std::lock_guard lock(functionalitiesMutex); +#endif + + auto existingFunctionality = get_functionality(Functionalities::TractorImplementManagementServer); + + if (supportedFunctionalities.end() != existingFunctionality) + { + if (TractorImplementManagementOptions::NoOptions != option) + { + existingFunctionality->set_bit_in_option(get_tim_option_byte_index(option), 1 << get_tim_option_bit_index(option), optionState); + } + else + { + CANStackLogger::debug("[DP]: Can't set the No Options TIM option, disable the other ones instead."); + } + } + } + + bool ControlFunctionFunctionalities::get_tractor_implement_management_server_option_state(TractorImplementManagementOptions option) + { + bool retVal = true; + + if (TractorImplementManagementOptions::NoOptions == option) + { +#if !defined CAN_STACK_DISABLE_THREADS && !defined ARDUINO + const std::lock_guard lock(functionalitiesMutex); +#endif + auto existingFunctionality = get_functionality(Functionalities::TractorImplementManagementServer); + + if (supportedFunctionalities.end() != existingFunctionality) + { + for (const auto ¤tByte : existingFunctionality->serializedValue) + { + retVal &= (0 == currentByte); + } + } + } + else + { + retVal = get_functionality_byte_option(Functionalities::TractorImplementManagementServer, get_tim_option_byte_index(option), 1 << get_tim_option_bit_index(option)); + } + return retVal; + } + + void ControlFunctionFunctionalities::set_tractor_implement_management_server_aux_valve_option(std::uint8_t auxValveIndex, bool stateSupported, bool flowSupported) + { +#if !defined CAN_STACK_DISABLE_THREADS && !defined ARDUINO + const std::lock_guard lock(functionalitiesMutex); +#endif + + auto existingFunctionality = get_functionality(Functionalities::TractorImplementManagementServer); + + if ((supportedFunctionalities.end() != existingFunctionality) && (auxValveIndex < NUMBER_TIM_AUX_VALVES)) + { + existingFunctionality->set_bit_in_option(auxValveIndex / 4, 1 << (2 * (auxValveIndex % 4)), stateSupported); + existingFunctionality->set_bit_in_option(auxValveIndex / 4, 1 << (2 * (auxValveIndex % 4) + 1), flowSupported); + } + } + + bool ControlFunctionFunctionalities::get_tractor_implement_management_server_aux_valve_state_supported(std::uint8_t auxValveIndex) + { +#if !defined CAN_STACK_DISABLE_THREADS && !defined ARDUINO + const std::lock_guard lock(functionalitiesMutex); +#endif + bool retVal = false; + + auto existingFunctionality = get_functionality(Functionalities::TractorImplementManagementServer); + + if ((supportedFunctionalities.end() != existingFunctionality) && (auxValveIndex < NUMBER_TIM_AUX_VALVES)) + { + retVal = existingFunctionality->get_bit_in_option(auxValveIndex / 4, 1 << (2 * (auxValveIndex % 4))); + } + return retVal; + } + + bool ControlFunctionFunctionalities::get_tractor_implement_management_server_aux_valve_flow_supported(std::uint8_t auxValveIndex) + { +#if !defined CAN_STACK_DISABLE_THREADS && !defined ARDUINO + const std::lock_guard lock(functionalitiesMutex); +#endif + bool retVal = false; + + auto existingFunctionality = get_functionality(Functionalities::TractorImplementManagementServer); + + if ((supportedFunctionalities.end() != existingFunctionality) && (auxValveIndex < NUMBER_TIM_AUX_VALVES)) + { + retVal = existingFunctionality->get_bit_in_option(auxValveIndex / 4, 1 << (2 * (auxValveIndex % 4) + 1)); + } + return retVal; + } + + void ControlFunctionFunctionalities::set_tractor_implement_management_client_option_state(TractorImplementManagementOptions option, bool optionState) + { +#if !defined CAN_STACK_DISABLE_THREADS && !defined ARDUINO + const std::lock_guard lock(functionalitiesMutex); +#endif + + auto existingFunctionality = get_functionality(Functionalities::TractorImplementManagementClient); + + if (supportedFunctionalities.end() != existingFunctionality) + { + existingFunctionality->set_bit_in_option(get_tim_option_byte_index(option), 1 << get_tim_option_bit_index(option), optionState); + } + } + + bool ControlFunctionFunctionalities::get_tractor_implement_management_client_option_state(TractorImplementManagementOptions option) + { + bool retVal = true; + + if (TractorImplementManagementOptions::NoOptions == option) + { +#if !defined CAN_STACK_DISABLE_THREADS && !defined ARDUINO + const std::lock_guard lock(functionalitiesMutex); +#endif + auto existingFunctionality = get_functionality(Functionalities::TractorImplementManagementClient); + + if (supportedFunctionalities.end() != existingFunctionality) + { + for (const auto ¤tByte : existingFunctionality->serializedValue) + { + retVal &= (0 == currentByte); + } + } + } + else + { + std::uint8_t optionBit = get_tim_option_bit_index(option); + + if (optionBit < std::numeric_limits::digits) + { + retVal = get_functionality_byte_option(Functionalities::TractorImplementManagementClient, get_tim_option_byte_index(option), 1 << get_tim_option_bit_index(option)); + } + else + { + // This should be impossible, if you see this please report a bug on our GitHub. + // Generally this means that a new TIM option was added but not considered in get_tim_option_bit_index. + assert(false); + retVal = false; + } + } + return retVal; + } + + void ControlFunctionFunctionalities::set_tractor_implement_management_client_aux_valve_option(std::uint8_t auxValveIndex, bool stateSupported, bool flowSupported) + { +#if !defined CAN_STACK_DISABLE_THREADS && !defined ARDUINO + const std::lock_guard lock(functionalitiesMutex); +#endif + auto existingFunctionality = get_functionality(Functionalities::TractorImplementManagementClient); + + if ((supportedFunctionalities.end() != existingFunctionality) && (auxValveIndex < NUMBER_TIM_AUX_VALVES)) + { + existingFunctionality->set_bit_in_option(auxValveIndex / 4, 1 << (2 * (auxValveIndex % 4)), stateSupported); + existingFunctionality->set_bit_in_option(auxValveIndex / 4, 1 << (2 * (auxValveIndex % 4) + 1), flowSupported); + } + } + + bool ControlFunctionFunctionalities::get_tractor_implement_management_client_aux_valve_state_supported(std::uint8_t auxValveIndex) + { +#if !defined CAN_STACK_DISABLE_THREADS && !defined ARDUINO + const std::lock_guard lock(functionalitiesMutex); +#endif + bool retVal = false; + + auto existingFunctionality = get_functionality(Functionalities::TractorImplementManagementClient); + + if ((supportedFunctionalities.end() != existingFunctionality) && (auxValveIndex < NUMBER_TIM_AUX_VALVES)) + { + retVal = existingFunctionality->get_bit_in_option(auxValveIndex / 4, 1 << (2 * (auxValveIndex % 4))); + } + return retVal; + } + + bool ControlFunctionFunctionalities::get_tractor_implement_management_client_aux_valve_flow_supported(std::uint8_t auxValveIndex) + { +#if !defined CAN_STACK_DISABLE_THREADS && !defined ARDUINO + const std::lock_guard lock(functionalitiesMutex); +#endif + bool retVal = false; + + auto existingFunctionality = get_functionality(Functionalities::TractorImplementManagementClient); + + if ((supportedFunctionalities.end() != existingFunctionality) && (auxValveIndex < NUMBER_TIM_AUX_VALVES)) + { + retVal = existingFunctionality->get_bit_in_option(auxValveIndex / 4, 1 << (2 * (auxValveIndex % 4) + 1)); + } + return retVal; + } + + void ControlFunctionFunctionalities::update() + { + txFlags.process_all_flags(); + } + + ControlFunctionFunctionalities::FunctionalityData::FunctionalityData(Functionalities functionalityToStore) : + functionality(functionalityToStore) + { + } + + void ControlFunctionFunctionalities::FunctionalityData::configure_default_data() + { + switch (functionality) + { + case Functionalities::MinimumControlFunction: + case Functionalities::UniversalTerminalServer: + case Functionalities::UniversalTerminalWorkingSet: + case Functionalities::AuxOInputs: + case Functionalities::AuxOFunctions: + case Functionalities::TaskControllerBasicServer: + case Functionalities::TaskControllerBasicClient: + case Functionalities::TaskControllerGeoServer: + case Functionalities::TaskControllerGeoClient: + case Functionalities::BasicTractorECUServer: + case Functionalities::BasicTractorECUImplementClient: + case Functionalities::FileServer: + case Functionalities::FileServerClient: + { + serializedValue.resize(1); + serializedValue.at(0) = 0x00; // No options + } + break; + + case Functionalities::AuxNInputs: + case Functionalities::AuxNFunctions: + { + serializedValue.resize(2); + serializedValue.at(0) = 0x00; // No options + serializedValue.at(1) = 0x00; // No options + } + break; + + case Functionalities::TaskControllerSectionControlServer: + case Functionalities::TaskControllerSectionControlClient: + { + serializedValue.resize(2); + serializedValue.at(0) = 0x01; // 1 Boom supported (1 is the minimum) + serializedValue.at(1) = 0x01; // 1 Section Supported (1 is the minimum) + } + break; + + case Functionalities::TractorImplementManagementServer: + case Functionalities::TractorImplementManagementClient: + { + CANStackLogger::warn("[DP]: You have configured TIM as a CF functionality, but the library doesn't support TIM at this time. Do you have an external TIM implementation?"); + serializedValue.resize(15); // TIM has a lot of options. https://www.isobus.net/isobus/option + std::fill(serializedValue.begin(), serializedValue.end(), 0x00); // Support nothing by default + } + break; + + default: + { + CANStackLogger::error("[DP]: An invalid control function functionality was added. It's values will be ignored."); + } + break; + } + } + + void ControlFunctionFunctionalities::FunctionalityData::set_bit_in_option(std::uint8_t byteIndex, std::uint8_t bit, bool value) + { + if ((byteIndex < serializedValue.size()) && (bit < std::numeric_limits::max())) + { + if (value) + { + serializedValue.at(byteIndex) = (serializedValue.at(byteIndex) | bit); + } + else + { + serializedValue.at(byteIndex) = (serializedValue.at(byteIndex) & ~(bit)); + } + } + } + + bool ControlFunctionFunctionalities::FunctionalityData::get_bit_in_option(std::uint8_t byteIndex, std::uint8_t bit) + { + bool retVal = false; + + if ((byteIndex < serializedValue.size()) && (bit < std::numeric_limits::max())) + { + retVal = (0 != (serializedValue.at(byteIndex) & (bit))); + } + return retVal; + } + + std::list::iterator ControlFunctionFunctionalities::get_functionality(Functionalities functionalityToRetrieve) + { + return std::find_if(supportedFunctionalities.begin(), supportedFunctionalities.end(), [functionalityToRetrieve](const FunctionalityData ¤tFunctionality) { return currentFunctionality.functionality == functionalityToRetrieve; }); + } + + bool ControlFunctionFunctionalities::get_functionality_byte_option(Functionalities functionality, std::uint8_t byteIndex, std::uint8_t option) + { +#if !defined CAN_STACK_DISABLE_THREADS && !defined ARDUINO + const std::lock_guard lock(functionalitiesMutex); +#endif + bool retVal = false; + + auto existingFunctionality = get_functionality(functionality); + + if (supportedFunctionalities.end() != existingFunctionality) + { + retVal = existingFunctionality->get_bit_in_option(byteIndex, option); + } + return retVal; + } + + void ControlFunctionFunctionalities::get_message_content(std::vector &messageData) + { +#if !defined CAN_STACK_DISABLE_THREADS && !defined ARDUINO + const std::lock_guard lock(functionalitiesMutex); +#endif + messageData.clear(); + messageData.reserve(supportedFunctionalities.size() * 4); // Approximate, but pretty close unless you have TIM. + messageData.push_back(0xFF); // Each control function shall respond with byte 1 set to FF + messageData.push_back(static_cast(supportedFunctionalities.size())); + + for (const auto &functionality : supportedFunctionalities) + { + messageData.push_back(static_cast(functionality.functionality)); + messageData.push_back(functionality.generation); + messageData.push_back(static_cast(functionality.serializedValue.size())); + + for (const auto &dataByte : functionality.serializedValue) + { + messageData.push_back(dataByte); + } + } + + while (messageData.size() < CAN_DATA_LENGTH) + { + messageData.push_back(0xFF); + } + } + + std::uint8_t ControlFunctionFunctionalities::get_tim_option_byte_index(TractorImplementManagementOptions option) const + { + std::uint8_t retVal = 0xFF; + + switch (option) + { + case TractorImplementManagementOptions::FrontPTODisengagementIsSupported: + case TractorImplementManagementOptions::FrontPTOEngagementCCWIsSupported: + case TractorImplementManagementOptions::FrontPTOengagementCWIsSupported: + case TractorImplementManagementOptions::FrontPTOspeedCCWIsSupported: + case TractorImplementManagementOptions::FrontPTOspeedCWIsSupported: + { + retVal = 9; + } + break; + + case TractorImplementManagementOptions::RearPTODisengagementIsSupported: + case TractorImplementManagementOptions::RearPTOEngagementCCWIsSupported: + case TractorImplementManagementOptions::RearPTOEngagementCWIsSupported: + case TractorImplementManagementOptions::RearPTOSpeedCCWIsSupported: + case TractorImplementManagementOptions::RearPTOSpeedCWIsSupported: + { + retVal = 10; + } + break; + + case TractorImplementManagementOptions::FrontHitchMotionIsSupported: + case TractorImplementManagementOptions::FrontHitchPositionIsSupported: + { + retVal = 11; + } + break; + + case TractorImplementManagementOptions::RearHitchMotionIsSupported: + case TractorImplementManagementOptions::RearHitchPositionIsSupported: + { + retVal = 12; + } + break; + + case TractorImplementManagementOptions::VehicleSpeedInForwardDirectionIsSupported: + case TractorImplementManagementOptions::VehicleSpeedInReverseDirectionIsSupported: + case TractorImplementManagementOptions::VehicleSpeedStartMotionIsSupported: + case TractorImplementManagementOptions::VehicleSpeedStopMotionIsSupported: + case TractorImplementManagementOptions::VehicleSpeedForwardSetByServerIsSupported: + case TractorImplementManagementOptions::VehicleSpeedReverseSetByServerIsSupported: + case TractorImplementManagementOptions::VehicleSpeedChangeDirectionIsSupported: + { + retVal = 13; + } + break; + + case TractorImplementManagementOptions::GuidanceCurvatureIsSupported: + { + retVal = 14; + } + break; + + default: + { + // Option not handled. This is normal for NoOption but others should be considered. + } + break; + } + return retVal; + } + + std::uint8_t ControlFunctionFunctionalities::get_tim_option_bit_index(TractorImplementManagementOptions option) const + { + std::uint8_t retVal = 0xFF; + + switch (option) + { + case TractorImplementManagementOptions::FrontPTODisengagementIsSupported: + case TractorImplementManagementOptions::RearPTODisengagementIsSupported: + case TractorImplementManagementOptions::FrontHitchMotionIsSupported: + case TractorImplementManagementOptions::RearHitchMotionIsSupported: + case TractorImplementManagementOptions::VehicleSpeedInForwardDirectionIsSupported: + case TractorImplementManagementOptions::GuidanceCurvatureIsSupported: + { + retVal = 0; + } + break; + + case TractorImplementManagementOptions::FrontPTOEngagementCCWIsSupported: + case TractorImplementManagementOptions::RearPTOEngagementCCWIsSupported: + case TractorImplementManagementOptions::VehicleSpeedInReverseDirectionIsSupported: + { + retVal = 1; + } + break; + + case TractorImplementManagementOptions::FrontHitchPositionIsSupported: + case TractorImplementManagementOptions::RearHitchPositionIsSupported: + case TractorImplementManagementOptions::VehicleSpeedStartMotionIsSupported: + { + retVal = 2; + } + break; + + case TractorImplementManagementOptions::FrontPTOengagementCWIsSupported: + case TractorImplementManagementOptions::RearPTOEngagementCWIsSupported: + case TractorImplementManagementOptions::VehicleSpeedStopMotionIsSupported: + { + retVal = 3; + } + break; + + case TractorImplementManagementOptions::FrontPTOspeedCCWIsSupported: + case TractorImplementManagementOptions::RearPTOSpeedCCWIsSupported: + case TractorImplementManagementOptions::VehicleSpeedForwardSetByServerIsSupported: + { + retVal = 4; + } + break; + + case TractorImplementManagementOptions::FrontPTOspeedCWIsSupported: + case TractorImplementManagementOptions::RearPTOSpeedCWIsSupported: + case TractorImplementManagementOptions::VehicleSpeedReverseSetByServerIsSupported: + { + retVal = 5; + } + break; + + case TractorImplementManagementOptions::VehicleSpeedChangeDirectionIsSupported: + { + retVal = 6; + } + break; + + default: + { + // Option not handled. This is normal for NoOption but others should be considered. + } + break; + } + return retVal; + } + + bool ControlFunctionFunctionalities::pgn_request_handler(std::uint32_t parameterGroupNumber, + std::shared_ptr, + bool &acknowledge, + AcknowledgementType &, + void *parentPointer) + { + assert(nullptr != parentPointer); + auto targetInterface = static_cast(parentPointer); + bool retVal = false; + + if (static_cast(CANLibParameterGroupNumber::ControlFunctionFunctionalities) == parameterGroupNumber) + { + acknowledge = false; + targetInterface->txFlags.set_flag(static_cast(TransmitFlags::ControlFunctionFunctionalitiesMessage)); + retVal = true; + } + return retVal; + } + + void ControlFunctionFunctionalities::process_flags(std::uint32_t flag, void *parentPointer) + { + assert(nullptr != parentPointer); + auto targetInterface = static_cast(parentPointer); + bool transmitSuccessful = true; + + if (static_cast(TransmitFlags::ControlFunctionFunctionalitiesMessage) == flag) + { + std::vector messageBuffer; + targetInterface->get_message_content(messageBuffer); + transmitSuccessful = CANNetworkManager::CANNetwork.send_can_message(static_cast(CANLibParameterGroupNumber::ControlFunctionFunctionalities), + messageBuffer.data(), + messageBuffer.size(), + targetInterface->myControlFunction, + nullptr); + } + + if (!transmitSuccessful) + { + targetInterface->txFlags.set_flag(flag); + } + } +} // namespace isobus diff --git a/src/isobus_functionalities.hpp b/src/isobus_functionalities.hpp new file mode 100644 index 0000000..38a84ee --- /dev/null +++ b/src/isobus_functionalities.hpp @@ -0,0 +1,480 @@ +//================================================================================================ +/// @file isobus_functionalities.hpp +/// +/// @brief Defines a class that manages the control function functionalities message data. +/// (PGN 64654, 0xFC8E) as defined in ISO11783-12 +/// +/// @details The parameters defined here can be found at https://www.isobus.net/isobus/option +/// +/// @author Adrian Del Grosso +/// +/// @copyright 2023 Adrian Del Grosso +//================================================================================================ +#ifndef ISOBUS_FUNCTIONALITIES_HPP +#define ISOBUS_FUNCTIONALITIES_HPP + +#include "can_internal_control_function.hpp" +#include "can_parameter_group_number_request_protocol.hpp" +#include "can_protocol.hpp" +#include "processing_flags.hpp" + +#include +#include + +#if !defined CAN_STACK_DISABLE_THREADS && !defined ARDUINO +#include +#endif + +namespace isobus +{ + class DiagnosticProtocol; // Forward declaration + + /// @brief Manages the control function functionalities message + class ControlFunctionFunctionalities + { + public: + /// @brief Enumerates the different functionalities that an ISOBUS ECU can report + /// in the control function functionalities message. + enum class Functionalities : std::uint8_t + { + MinimumControlFunction = 0, + UniversalTerminalServer = 1, + UniversalTerminalWorkingSet = 2, + AuxOInputs = 3, + AuxOFunctions = 4, + AuxNInputs = 5, + AuxNFunctions = 6, + TaskControllerBasicServer = 7, + TaskControllerBasicClient = 8, + TaskControllerGeoServer = 9, + TaskControllerGeoClient = 10, + TaskControllerSectionControlServer = 11, + TaskControllerSectionControlClient = 12, + BasicTractorECUServer = 13, + BasicTractorECUImplementClient = 14, + TractorImplementManagementServer = 15, + TractorImplementManagementClient = 16, + FileServer = 17, + FileServerClient = 18, + + ReservedRangeBegin = 19, + MaxFunctionalityReserved = 255 + }; + + /// @brief This parameter reports which minimum control function functionality options are supported. + enum class MinimumControlFunctionOptions : std::uint8_t + { + NoOptions = 0x00, + Type1ECUInternalWeakTermination = 0x01, + Type2ECUInternalEndPointTermination = 0x02, + SupportOfHeartbeatProducer = 0x04, + SupportOfHeartbeatConsumer = 0x08, + + Reserved = 0xF0 + }; + + /// @brief This parameter reports which auxiliary control type 1 functionality type functions are supported by + /// an implement working set auxiliary function or an auxiliary function input unit. + enum class AuxOOptions : std::uint8_t + { + NoOptions = 0x00, + SupportsType0Function = 0x01, + SupportsType1Function = 0x02, + SupportsType2Function = 0x04, + + Reserved = 0xF8 + }; + + /// @brief This parameter reports which auxiliary control type 2 functionality type functions are supported by + /// an implement working set auxiliary function or an auxiliary function input unit. + enum class AuxNOptions : std::uint16_t + { + NoOptions = 0x00, + SupportsType0Function = 0x01, + SupportsType1Function = 0x02, + SupportsType2Function = 0x04, + SupportsType3Function = 0x08, + SupportsType4Function = 0x10, + SupportsType5Function = 0x20, + SupportsType6Function = 0x40, + SupportsType7Function = 0x80, + SupportsType8Function = 0x100, + SupportsType9Function = 0x200, + SupportsType10Function = 0x400, + SupportsType11Function = 0x800, + SupportsType12Function = 0x1000, + SupportsType13Function = 0x2000, + SupportsType14Function = 0x4000, + + Reserved = 0x8000 + }; + + /// @brief This option byte reports which task controller geo functionality options are supported by an + /// implement working set master or a task controller. + enum class TaskControllerGeoServerOptions : std::uint8_t + { + NoOptions = 0x00, + PolygonBasedPrescriptionMapsAreSupported = 0x01, + + Reserved = 0xFE + }; + + /// @brief This parameter reports which tractor ECU class and functionality options are supported by an implement + /// working set master or a tractor ECU. + enum class BasicTractorECUOptions : std::uint8_t + { + TECUNotMeetingCompleteClass1Requirements = 0x00, + Class1NoOptions = 0x01, + Class2NoOptions = 0x02, + ClassRequiredLighting = 0x04, + NavigationOption = 0x08, + FrontHitchOption = 0x10, + GuidanceOption = 0x20, + Reserved = 0xC0 + }; + + /// @brief This parameter reports which TIM options are supported by a TIM server or an implement + /// working set master + enum class TractorImplementManagementOptions + { + NoOptions = 0, + FrontPTODisengagementIsSupported, + FrontPTOEngagementCCWIsSupported, + FrontPTOengagementCWIsSupported, + FrontPTOspeedCCWIsSupported, + FrontPTOspeedCWIsSupported, + RearPTODisengagementIsSupported, + RearPTOEngagementCCWIsSupported, + RearPTOEngagementCWIsSupported, + RearPTOSpeedCCWIsSupported, + RearPTOSpeedCWIsSupported, + FrontHitchMotionIsSupported, + FrontHitchPositionIsSupported, + RearHitchMotionIsSupported, + RearHitchPositionIsSupported, + VehicleSpeedInForwardDirectionIsSupported, + VehicleSpeedInReverseDirectionIsSupported, + VehicleSpeedStartMotionIsSupported, + VehicleSpeedStopMotionIsSupported, + VehicleSpeedForwardSetByServerIsSupported, + VehicleSpeedReverseSetByServerIsSupported, + VehicleSpeedChangeDirectionIsSupported, + GuidanceCurvatureIsSupported + }; + + /// @brief Constructor for a ControlFunctionFunctionalities object + /// @param[in] sourceControlFunction The control function to use when sending messages + explicit ControlFunctionFunctionalities(std::shared_ptr sourceControlFunction); + + /// @brief Destructor for a ControlFunctionFunctionalities object + ~ControlFunctionFunctionalities(); + + /// @brief Adds or removes a supported functionality + /// @param[in] functionality The functionality to change + /// @param[in] functionalityGeneration The generation of the functionality to report + /// @param[in] isSupported If true, this class will add reporting as such on the ISOBUS. If false, it will not be reported on the bus. + /// @note Minimum Control Function is enabled by default, and generally should not be disabled. + void set_functionality_is_supported(Functionalities functionality, std::uint8_t functionalityGeneration, bool isSupported); + + /// @brief Returns if a functionality was previously configured with set_functionality_is_supported + /// @param[in] functionality The functionality to check against + /// @returns true if the specified functionality will be reported as being supported, otherwise false + bool get_functionality_is_supported(Functionalities functionality); + + /// @brief Returns the generation that was set for the specified functionality when + /// set_functionality_is_supported was called for that functionality. + /// @param[in] functionality The functionality to check against + /// @returns The generation associated with the specified functionality, or 0 if not configured + std::uint8_t get_functionality_generation(Functionalities functionality); + + /// @brief Sets a minimum control function functionality option to a new state. + /// @details Options set to true will be reported as "supported" to a requester of your + /// control function functionalities within the "minimum control function" functionality section of the message. + /// @param[in] option The option to set + /// @param[in] optionState The state to set for the associated option + void set_minimum_control_function_option_state(MinimumControlFunctionOptions option, bool optionState); + + /// @brief Returns the current state of the specified minimum control function functionality option + /// @param[in] option The option to check the state of + /// @returns The state of the option. If true, the option is being reported as "supported". + bool get_minimum_control_function_option_state(MinimumControlFunctionOptions option); + + /// @brief Sets an AUX-O inputs functionality option to a new state. + /// @details Options set to true will be reported as "supported" to a requester of your + /// control function functionalities within the "AUX-O inputs" functionality section of the message. + /// @param[in] option The option to set + /// @param[in] optionState The state to set for the associated option + void set_aux_O_inputs_option_state(AuxOOptions option, bool optionState); + + /// @brief Gets the state of an AUX-O inputs functionality option. + /// @param[in] option The option to get + /// @returns The state of the option. If true, the option is being reported as "supported". + bool get_aux_O_inputs_option_state(AuxOOptions option); + + /// @brief Sets an AUX-O functions functionality option to a new state. + /// @details Options set to true will be reported as "supported" to a requester of your + /// control function functionalities within the "AUX-O functions" functionality section of the message. + /// @param[in] option The option to set + /// @param[in] optionState The state to set for the associated option + void set_aux_O_functions_option_state(AuxOOptions option, bool optionState); + + /// @brief Gets the state of an AUX-O functions functionality option. + /// @param[in] option The option to get + /// @returns The state of the option. If true, the option is being reported as "supported". + bool get_aux_O_functions_option_state(AuxOOptions option); + + /// @brief Sets an AUX-N inputs functionality option to a new state. + /// @details Options set to true will be reported as "supported" to a requester of your + /// control function functionalities within the "AUX-N inputs" functionality section of the message. + /// @param[in] option The option to set + /// @param[in] optionState The state to set for the associated option + void set_aux_N_inputs_option_state(AuxNOptions option, bool optionState); + + /// @brief Gets the state of an AUX-N inputs functionality option. + /// @param[in] option The option to get + /// @returns The state of the option. If true, the option is being reported as "supported". + bool get_aux_N_inputs_option_state(AuxNOptions option); + + /// @brief Sets an AUX-N functions functionality option to a new state. + /// @details Options set to true will be reported as "supported" to a requester of your + /// control function functionalities within the "AUX-N functions" functionality section of the message. + /// @param[in] option The option to set + /// @param[in] optionState The state to set for the associated option + void set_aux_N_functions_option_state(AuxNOptions option, bool optionState); + + /// @brief Gets the state of an AUX-N functions functionality option. + /// @param[in] option The option to get + /// @returns The state of the option. If true, the option is being reported as "supported". + bool get_aux_N_functions_option_state(AuxNOptions option); + + /// @brief Sets a task controller geo server functionality option to a new state. + /// @details Options set to true will be reported as "supported" to a requester of your + /// control function functionalities within the "task controller geo server" functionality section of the message. + /// @param[in] option The option to set + /// @param[in] optionState The state to set for the associated option + void set_task_controller_geo_server_option_state(TaskControllerGeoServerOptions option, bool optionState); + + /// @brief Gets the state of a TC GEO server functionality option. + /// @param[in] option The option to get + /// @returns The state of the option. If true, the option is being reported as "supported". + bool get_task_controller_geo_server_option_state(TaskControllerGeoServerOptions option); + + /// @brief Sets a task controller geo client's only functionality option, which is the number of control channels. + /// @details The value you set will be reported to the requestor of your CF's functionalities + /// within the "task controller geo client" functionality section of the message. + /// @param[in] numberOfControlChannels The number of control channels your ECU supports for TC GEO (client side) + void set_task_controller_geo_client_option(std::uint8_t numberOfControlChannels); + + /// @brief Gets the state of the only TC GEO client functionality option, which is the number of control channels. + /// @returns The number of supported TC GEO client control channels that are supported + std::uint8_t get_task_controller_geo_client_option(); + + /// @brief Sets a task controller section control server's options + /// @details The values set here will be reported as "supported" to a requester of your CF's functionalities + /// @param[in] numberOfSupportedBooms The number of booms your application TC server supports + /// @param[in] numberOfSupportedSections The number of sections your application TC server supports + void set_task_controller_section_control_server_option_state(std::uint8_t numberOfSupportedBooms, std::uint8_t numberOfSupportedSections); + + /// @brief Gets the number of supported booms for the TC section control server functionality + /// @returns The number of supported booms being reported in the TC section control server functionality + std::uint8_t get_task_controller_section_control_server_number_supported_booms(); + + /// @brief Gets the number of supported sections for the TC section control server functionality + /// @returns The number of supported sections being reported in the TC section control server functionality + std::uint8_t get_task_controller_section_control_server_number_supported_sections(); + + /// @brief Sets a task controller section control client's options + /// @details The values set here will be reported as "supported" to a requester of your CF's functionalities + /// @param[in] numberOfSupportedBooms The number of booms your application TC client supports + /// @param[in] numberOfSupportedSections The number of sections your application TC client supports + void set_task_controller_section_control_client_option_state(std::uint8_t numberOfSupportedBooms, std::uint8_t numberOfSupportedSections); + + /// @brief Gets the number of supported booms for the TC section control client functionality + /// @returns The number of supported booms being reported in the TC section control client functionality + std::uint8_t get_task_controller_section_control_client_number_supported_booms(); + + /// @brief Gets the number of supported sections for the TC section control client functionality + /// @returns The number of supported sections being reported in the TC section control client functionality + std::uint8_t get_task_controller_section_control_client_number_supported_sections(); + + /// @brief Sets a tractor ECU server functionality option to a new state. + /// @details The values set here will be reported as "supported" to a requester of your CF's functionalities + /// @param[in] option The option to set + /// @param[in] optionState The state to set for the associated option + void set_basic_tractor_ECU_server_option_state(BasicTractorECUOptions option, bool optionState); + + /// @brief Gets the state of a basic tractor ECU server functionality option. + /// @param[in] option The option to get + /// @returns The state of the option. If true, the option is being reported as "supported". + bool get_basic_tractor_ECU_server_option_state(BasicTractorECUOptions option); + + /// @brief Sets a tractor ECU client functionality option to a new state. + /// @details The values set here will be reported as "supported" to a requester of your CF's functionalities + /// @param[in] option The option to set + /// @param[in] optionState The state to set for the associated option + void set_basic_tractor_ECU_implement_client_option_state(BasicTractorECUOptions option, bool optionState); + + /// @brief Gets the state of a basic tractor ECU implement client functionality option. + /// @param[in] option The option to get + /// @returns The state of the option. If true, the option is being reported as "supported". + bool get_basic_tractor_ECU_implement_client_option_state(BasicTractorECUOptions option); + + /// @brief Sets a tractor implement management (TIM) server functionality option to a new state. + /// @details The values set here will be reported as "supported" to a requester of your CF's functionalities + /// @param[in] option The option to set + /// @param[in] optionState The state to set for the associated option + void set_tractor_implement_management_server_option_state(TractorImplementManagementOptions option, bool optionState); + + /// @brief Gets the state of a basic tractor implement management client functionality option. + /// @param[in] option The option to get + /// @returns The state of the option. If true, the option is being reported as "supported". + bool get_tractor_implement_management_server_option_state(TractorImplementManagementOptions option); + + /// @brief Sets a tractor implement management (TIM) server aux valve's functionality options to a new state + /// @details The values set here will be reported as "supported" to a requester of your CF's functionalities + /// @param[in] auxValveIndex The index of the aux valve to set, between 0 and 31 + /// @param[in] stateSupported Set to true to indicate you support the state of this valve, otherwise false + /// @param[in] flowSupported Set to true to indicate your support aux valve flow with this valve + void set_tractor_implement_management_server_aux_valve_option(std::uint8_t auxValveIndex, bool stateSupported, bool flowSupported); + + /// @brief Returns if a particular aux valve's state control is supported in the TIM server functionality + /// @param[in] auxValveIndex The index of the aux valve to check + /// @returns true if the aux valve's state you specified is being reported as "supported". + bool get_tractor_implement_management_server_aux_valve_state_supported(std::uint8_t auxValveIndex); + + /// @brief Returns if a particular aux valve's flow control is supported in the TIM server functionality + /// @param[in] auxValveIndex The index of the aux valve to check + /// @returns true if the aux valve you specified is being reported as "supported". + bool get_tractor_implement_management_server_aux_valve_flow_supported(std::uint8_t auxValveIndex); + + /// @brief Sets a tractor implement management (TIM) client functionality option to a new state. + /// @details The values set here will be reported as "supported" to a requester of your CF's functionalities + /// @param[in] option The option to set + /// @param[in] optionState The state to set for the associated option + void set_tractor_implement_management_client_option_state(TractorImplementManagementOptions option, bool optionState); + + /// @brief Gets the state of a TIM client functionality option. + /// @param[in] option The option to get + /// @returns The state of the option. If true, the option is being reported as "supported". + bool get_tractor_implement_management_client_option_state(TractorImplementManagementOptions option); + + /// @brief Sets a tractor implement management (TIM) client aux valve's functionality options to a new state + /// @details The values set here will be reported as "supported" to a requester of your CF's functionalities + /// @param[in] auxValveIndex The index of the aux valve to set, between 0 and 31 + /// @param[in] stateSupported Set to true to indicate you support the state of this valve, otherwise false + /// @param[in] flowSupported Set to true to indicate your support aux valve flow with this valve + void set_tractor_implement_management_client_aux_valve_option(std::uint8_t auxValveIndex, bool stateSupported, bool flowSupported); + + /// @brief Returns if a particular aux valve's state control is supported in the TIM client functionality + /// @param[in] auxValveIndex The index of the aux valve to check + /// @returns true if the aux valve's state you specified is being reported as "supported". + bool get_tractor_implement_management_client_aux_valve_state_supported(std::uint8_t auxValveIndex); + + /// @brief Returns if a particular aux valve's flow control is supported in the TIM client functionality + /// @param[in] auxValveIndex The index of the aux valve to check + /// @returns true if the aux valve you specified is being reported as "supported". + bool get_tractor_implement_management_client_aux_valve_flow_supported(std::uint8_t auxValveIndex); + + /// @brief The diagnostic protocol will call this update function, make sure to call DiagnosticProtocol::update() in your update loop + void update(); + + protected: + /// @brief Populates a vector with the message data needed to send PGN 0xFC8E + /// @param[in,out] messageData The buffer to populate with data (will be cleared before use) + void get_message_content(std::vector &messageData); + + private: + /// @brief Stores the raw byte data associated with a functionality based on + /// what the user has enabled and what options the user has set for that functionality. + class FunctionalityData + { + public: + /// @brief Constructor for a FunctionalityData object + /// @param[in] functionalityToStore The ISO functionality that the object will represent + explicit FunctionalityData(Functionalities functionalityToStore); + + /// @brief Sets up default data associated to the functionality the object is representing + /// @details This sets up the serializedValue to be a valid "no options" default set of bytes. + void configure_default_data(); + + /// @brief A helper function to set a particular option bit to some value within an option byte + /// @note If the parameters are out of range, the data will not be modified + /// @param[in] byteIndex The options byte index inside which the bit should be set + /// @param[in] bit The bit offset index of the bit you want to set, between 0 and 7 + /// @param[in] value The new value for the desired bit + void set_bit_in_option(std::uint8_t byteIndex, std::uint8_t bit, bool value); + + /// @brief A helper function to get a particular bit's value in the specified option byte + /// @note If the parameters are out of range, this will return zero. + /// @param[in] byteIndex The options byte index inside which the bit should be retrieved + /// @param[in] bit The bit offset index of the bit you want to get, between 0 and 7 + /// @returns The state of the requested bit + bool get_bit_in_option(std::uint8_t byteIndex, std::uint8_t bit); + + Functionalities functionality = Functionalities::MinimumControlFunction; ///< The functionality associated with this data + std::vector serializedValue; ///< The raw message data value for this functionality + std::uint8_t generation = 1; ///< The generation of the functionality supported + }; + + /// @brief Enumerates a set of flags representing messages to be transmitted by this interfaces + enum class TransmitFlags : std::uint32_t + { + ControlFunctionFunctionalitiesMessage = 0, ///< A flag to send the CF Functionalities message + + NumberOfFlags ///< The number of flags enumerated in this enum + }; + + /// @brief Checks for the existence of a functionality in the list of previously configured functionalities + /// and returns an iterator to that functionality in the list + /// @param[in] functionalityToRetrieve The functionality to return + /// @returns Iterator to the desired functionality, or supportedFunctionalities.end() if not found + std::list::iterator get_functionality(Functionalities functionalityToRetrieve); + + /// @brief A wrapper to to get an option from the first byte of a functionalities' data + /// @param[in] functionality The functionality associated to the option being retrieved + /// @param[in] byteIndex The index of the option byte to query within + /// @param[in] option The option bit to get the state for + /// @returns The state of the bit that was requested + bool get_functionality_byte_option(Functionalities functionality, std::uint8_t byteIndex, std::uint8_t option); + + /// @brief Returns the byte index of the specified TIM option in the CF Functionalities message data + /// associated with wither TIM server or TIM client functionalities. + /// @param[in] option The option for which you want to know the byte index + /// @returns The byte index of the specified option in the TIM functionalities' message data + std::uint8_t get_tim_option_byte_index(TractorImplementManagementOptions option) const; + + /// @brief Returns the bit offset of a specified TIM functionality option into the + /// TIM client and server functionality message data + /// @param[in] option The option for which you want to know the bit index into the TIM functionality message data + /// @returns The bit offset/index of the specified option int the TIM functionality message data + std::uint8_t get_tim_option_bit_index(TractorImplementManagementOptions option) const; + + /// @brief Handles PGN requests for the control function functionalities message + /// @param[in] parameterGroupNumber The PGN that was requested + /// @param[in] requestingControlFunction The control function that is requesting the PGN + /// @param[out] acknowledge Tells the PGN request protocol to ACK ack the request + /// @param[out] acknowledgeType Tells the PGN request protocol what kind of ACK to use + /// @param[in] parentPointer A generic context variable, usually the "this" pointer of the registrant for callbacks + static bool pgn_request_handler(std::uint32_t parameterGroupNumber, + std::shared_ptr requestingControlFunction, + bool &acknowledge, + AcknowledgementType &acknowledgeType, + void *parentPointer); + + /// @brief Processes set transmit flags to send messages + /// @param[in] flag The flag to process + /// @param[in] parentPointer A pointer back to an instance of this interface + static void process_flags(std::uint32_t flag, void *parentPointer); + + static constexpr std::uint8_t NUMBER_TIM_AUX_VALVES_PER_BYTE = 4; ///< The number of aux valves per byte of TIM functionality message data + static constexpr std::uint8_t NUMBER_TIM_AUX_VALVES = 32; ///< The max number of TIM aux valves + + std::shared_ptr myControlFunction; ///< The control function to send messages as + std::list supportedFunctionalities; ///< A list of all configured functionalities and their data + ProcessingFlags txFlags; ///< Handles retries for sending the CF functionalities message +#if !defined CAN_STACK_DISABLE_THREADS && !defined ARDUINO + std::mutex functionalitiesMutex; ///< Since messages come in on a different thread than the main app (probably), this mutex protects the functionality data +#endif + }; +} // namespace isobus +#endif // ISOBUS_FUNCTIONALITIES_HPP diff --git a/src/isobus_guidance_interface.cpp b/src/isobus_guidance_interface.cpp new file mode 100644 index 0000000..22ae8f8 --- /dev/null +++ b/src/isobus_guidance_interface.cpp @@ -0,0 +1,558 @@ +//================================================================================================ +/// @file isobus_guidance_interface.cpp +/// +/// @brief Implements an interface for sending and receiving ISOBUS guidance messages. +/// These messages are used to steer ISOBUS compliant machines, steering valves, and +/// implements in general. +/// +/// @attention Please use extreme care if you try to steer a machine with this interface! +/// Remember that this library is licensed under The MIT License, and that by obtaining a +/// copy of this library and of course by attempting to steer a machine with it, you are agreeing +/// to our license. +/// +/// @note These messages are expected to be deprecated or at least made redundant in favor +/// of Tractor Implement Management (TIM) at some point by the AEF, though the timeline on that +/// is not known at the time of writing this, and it's likely that many machines will +/// continue to support this interface going forward due to its simplicity over TIM. +/// +/// @author Adrian Del Grosso +/// +/// @copyright 2023 Adrian Del Grosso +//================================================================================================ +#include "isobus_guidance_interface.hpp" +#include "can_general_parameter_group_numbers.hpp" +#include "can_message.hpp" +#include "can_network_manager.hpp" +#include "can_stack_logger.hpp" +#include "system_timing.hpp" + +#include +#include +#include +#include + +namespace isobus +{ + AgriculturalGuidanceInterface::AgriculturalGuidanceInterface(std::shared_ptr source, + std::shared_ptr destination, + bool enableSendingSystemCommandPeriodically, + bool enableSendingMachineInfoPeriodically) : + guidanceMachineInfoTransmitData(GuidanceMachineInfo(enableSendingMachineInfoPeriodically ? source : nullptr)), + guidanceSystemCommandTransmitData(GuidanceSystemCommand(enableSendingSystemCommandPeriodically ? source : nullptr)), + txFlags(static_cast(TransmitFlags::NumberOfFlags), process_flags, this), + destinationControlFunction(destination) + { + } + + AgriculturalGuidanceInterface::~AgriculturalGuidanceInterface() + { + if (initialized) + { + CANNetworkManager::CANNetwork.remove_any_control_function_parameter_group_number_callback(static_cast(CANLibParameterGroupNumber::AgriculturalGuidanceMachineInfo), process_rx_message, this); + CANNetworkManager::CANNetwork.remove_any_control_function_parameter_group_number_callback(static_cast(CANLibParameterGroupNumber::AgriculturalGuidanceSystemCommand), process_rx_message, this); + } + } + + AgriculturalGuidanceInterface::GuidanceSystemCommand::GuidanceSystemCommand(std::shared_ptr sender) : + controlFunction(sender) + { + } + + bool AgriculturalGuidanceInterface::GuidanceSystemCommand::set_status(CurvatureCommandStatus newStatus) + { + if (commandedStatus != newStatus) + { + commandedStatus = newStatus; + return true; + } + return false; + } + + AgriculturalGuidanceInterface::GuidanceSystemCommand::CurvatureCommandStatus AgriculturalGuidanceInterface::GuidanceSystemCommand::get_status() const + { + return commandedStatus; + } + + bool AgriculturalGuidanceInterface::GuidanceSystemCommand::set_curvature(float curvature) + { + if (std::fabs(commandedCurvature - curvature) > std::numeric_limits::epsilon()) + { + commandedCurvature = curvature; + return true; + } + return false; + } + + float AgriculturalGuidanceInterface::GuidanceSystemCommand::get_curvature() const + { + return commandedCurvature; + } + + std::shared_ptr AgriculturalGuidanceInterface::GuidanceSystemCommand::get_sender_control_function() const + { + return controlFunction; + } + + void AgriculturalGuidanceInterface::GuidanceSystemCommand::set_timestamp_ms(std::uint32_t timestamp) + { + timestamp_ms = timestamp; + } + + std::uint32_t AgriculturalGuidanceInterface::GuidanceSystemCommand::get_timestamp_ms() const + { + return timestamp_ms; + } + + AgriculturalGuidanceInterface::GuidanceMachineInfo::GuidanceMachineInfo(std::shared_ptr sender) : + controlFunction(sender) + { + } + + bool AgriculturalGuidanceInterface::GuidanceMachineInfo::set_estimated_curvature(float curvature) + { + if (std::fabs(estimatedCurvature - curvature) > std::numeric_limits::epsilon()) + { + estimatedCurvature = curvature; + return true; + } + return false; + } + + float AgriculturalGuidanceInterface::GuidanceMachineInfo::get_estimated_curvature() const + { + return estimatedCurvature; + } + + bool AgriculturalGuidanceInterface::GuidanceMachineInfo::set_mechanical_system_lockout_state(MechanicalSystemLockout state) + { + if (mechanicalSystemLockoutState != state) + { + mechanicalSystemLockoutState = state; + return true; + } + return false; + } + + AgriculturalGuidanceInterface::GuidanceMachineInfo::MechanicalSystemLockout AgriculturalGuidanceInterface::GuidanceMachineInfo::get_mechanical_system_lockout() const + { + return mechanicalSystemLockoutState; + } + + bool AgriculturalGuidanceInterface::GuidanceMachineInfo::set_guidance_steering_system_readiness_state(GenericSAEbs02SlotValue state) + { + if (guidanceSteeringSystemReadinessState != state) + { + guidanceSteeringSystemReadinessState = state; + return true; + } + return false; + } + + AgriculturalGuidanceInterface::GuidanceMachineInfo::GenericSAEbs02SlotValue AgriculturalGuidanceInterface::GuidanceMachineInfo::get_guidance_steering_system_readiness_state() const + { + return guidanceSteeringSystemReadinessState; + } + + bool AgriculturalGuidanceInterface::GuidanceMachineInfo::set_guidance_steering_input_position_status(GenericSAEbs02SlotValue state) + { + if (guidanceSteeringInputPositionStatus != state) + { + guidanceSteeringInputPositionStatus = state; + return true; + } + return false; + } + + AgriculturalGuidanceInterface::GuidanceMachineInfo::GenericSAEbs02SlotValue AgriculturalGuidanceInterface::GuidanceMachineInfo::get_guidance_steering_input_position_status() const + { + return guidanceSteeringInputPositionStatus; + } + + bool AgriculturalGuidanceInterface::GuidanceMachineInfo::set_request_reset_command_status(RequestResetCommandStatus state) + { + if (requestResetCommandStatus != state) + { + requestResetCommandStatus = state; + return true; + } + return false; + } + + AgriculturalGuidanceInterface::GuidanceMachineInfo::RequestResetCommandStatus AgriculturalGuidanceInterface::GuidanceMachineInfo::get_request_reset_command_status() const + { + return requestResetCommandStatus; + } + + bool AgriculturalGuidanceInterface::GuidanceMachineInfo::set_guidance_limit_status(GuidanceLimitStatus status) + { + if (guidanceLimitStatus != status) + { + guidanceLimitStatus = status; + return true; + } + return false; + } + + AgriculturalGuidanceInterface::GuidanceMachineInfo::GuidanceLimitStatus AgriculturalGuidanceInterface::GuidanceMachineInfo::get_guidance_limit_status() const + { + return guidanceLimitStatus; + } + + bool AgriculturalGuidanceInterface::GuidanceMachineInfo::set_guidance_system_command_exit_reason_code(std::uint8_t exitCode) + { + if (guidanceSystemCommandExitReasonCode != exitCode) + { + guidanceSystemCommandExitReasonCode = exitCode; + return true; + } + return false; + } + + std::uint8_t AgriculturalGuidanceInterface::GuidanceMachineInfo::get_guidance_system_command_exit_reason_code() const + { + return guidanceSystemCommandExitReasonCode; + } + + bool AgriculturalGuidanceInterface::GuidanceMachineInfo::set_guidance_system_remote_engage_switch_status(GenericSAEbs02SlotValue switchStatus) + { + if (guidanceSystemRemoteEngageSwitchStatus != switchStatus) + { + guidanceSystemRemoteEngageSwitchStatus = switchStatus; + return true; + } + return false; + } + + AgriculturalGuidanceInterface::GuidanceMachineInfo::GenericSAEbs02SlotValue AgriculturalGuidanceInterface::GuidanceMachineInfo::get_guidance_system_remote_engage_switch_status() const + { + return guidanceSystemRemoteEngageSwitchStatus; + } + + std::shared_ptr AgriculturalGuidanceInterface::GuidanceMachineInfo::get_sender_control_function() const + { + return controlFunction; + } + + void AgriculturalGuidanceInterface::GuidanceMachineInfo::set_timestamp_ms(std::uint32_t timestamp) + { + timestamp_ms = timestamp; + } + + std::uint32_t AgriculturalGuidanceInterface::GuidanceMachineInfo::get_timestamp_ms() const + { + return timestamp_ms; + } + + void AgriculturalGuidanceInterface::initialize() + { + if (!initialized) + { + if ((nullptr != guidanceSystemCommandTransmitData.get_sender_control_function()) || (nullptr != guidanceMachineInfoTransmitData.get_sender_control_function())) + { + // Make sure you know what you are doing... consider reviewing the guidance messaging in ISO 11783-7 if you haven't already. + CANStackLogger::warn("[Guidance]: Use extreme caution! You have configured the ISOBUS guidance interface with the ability to steer a machine."); + } + CANNetworkManager::CANNetwork.add_any_control_function_parameter_group_number_callback(static_cast(CANLibParameterGroupNumber::AgriculturalGuidanceMachineInfo), process_rx_message, this); + CANNetworkManager::CANNetwork.add_any_control_function_parameter_group_number_callback(static_cast(CANLibParameterGroupNumber::AgriculturalGuidanceSystemCommand), process_rx_message, this); + initialized = true; + } + } + + bool AgriculturalGuidanceInterface::get_initialized() const + { + return initialized; + } + + std::size_t AgriculturalGuidanceInterface::get_number_received_guidance_system_command_sources() const + { + return receivedGuidanceSystemCommandMessages.size(); + } + + std::size_t AgriculturalGuidanceInterface::get_number_received_guidance_machine_info_message_sources() const + { + return receivedGuidanceMachineInfoMessages.size(); + } + + std::shared_ptr AgriculturalGuidanceInterface::get_received_guidance_machine_info(std::size_t index) + { + std::shared_ptr retVal = nullptr; + + if (index < receivedGuidanceMachineInfoMessages.size()) + { + retVal = receivedGuidanceMachineInfoMessages.at(index); + } + return retVal; + } + + std::shared_ptr AgriculturalGuidanceInterface::get_received_guidance_system_command(std::size_t index) + { + std::shared_ptr retVal = nullptr; + + if (index < receivedGuidanceSystemCommandMessages.size()) + { + retVal = receivedGuidanceSystemCommandMessages.at(index); + } + return retVal; + } + + EventDispatcher, bool> &AgriculturalGuidanceInterface::get_guidance_machine_info_event_publisher() + { + return guidanceMachineInfoEventPublisher; + } + + EventDispatcher, bool> &AgriculturalGuidanceInterface::get_guidance_system_command_event_publisher() + { + return guidanceSystemCommandEventPublisher; + } + + bool AgriculturalGuidanceInterface::send_guidance_system_command() const + { + bool retVal = false; + + if (nullptr != guidanceSystemCommandTransmitData.get_sender_control_function()) + { + float scaledCurvature = std::roundf(4 * ((guidanceSystemCommandTransmitData.get_curvature() + CURVATURE_COMMAND_OFFSET_INVERSE_KM) / CURVATURE_COMMAND_RESOLUTION_PER_BIT)) / 4.0f; + std::uint16_t encodedCurvature = ZERO_CURVATURE_INVERSE_KM; + + if (guidanceMachineInfoTransmitData.get_estimated_curvature() > CURVATURE_COMMAND_MAX_INVERSE_KM) + { + encodedCurvature = 32127 + ZERO_CURVATURE_INVERSE_KM; // Clamp to maximum value + CANStackLogger::warn("[Guidance]: Transmitting a commanded curvature clamped to maximum value. Verify guidance calculations are accurate!"); + } + else if (scaledCurvature < 0) // 0 In this case is -8032 km-1 due to the addition of the offset earlier + { + encodedCurvature = 0; // Clamp to minimum value + CANStackLogger::warn("[Guidance]: Transmitting a commanded curvature clamped to minimum value. Verify guidance calculations are accurate!"); + } + else + { + encodedCurvature = static_cast(scaledCurvature); + } + + std::array buffer = { static_cast(encodedCurvature & 0xFF), + static_cast((encodedCurvature >> 8) & 0xFF), + static_cast(static_cast(guidanceSystemCommandTransmitData.get_status()) | static_cast(0xFC)), + 0xFF, + 0xFF, + 0xFF, + 0xFF, + 0xFF }; + + retVal = CANNetworkManager::CANNetwork.send_can_message(static_cast(CANLibParameterGroupNumber::AgriculturalGuidanceSystemCommand), + buffer.data(), + buffer.size(), + std::static_pointer_cast(guidanceSystemCommandTransmitData.get_sender_control_function()), + destinationControlFunction, + CANIdentifier::Priority3); + } + return retVal; + } + + bool AgriculturalGuidanceInterface::send_guidance_machine_info() const + { + bool retVal = false; + + if (nullptr != guidanceMachineInfoTransmitData.get_sender_control_function()) + { + float scaledCurvature = std::roundf(4 * ((guidanceMachineInfoTransmitData.get_estimated_curvature() + CURVATURE_COMMAND_OFFSET_INVERSE_KM) / CURVATURE_COMMAND_RESOLUTION_PER_BIT)) / 4.0f; + std::uint16_t encodedCurvature = ZERO_CURVATURE_INVERSE_KM; + + if (guidanceMachineInfoTransmitData.get_estimated_curvature() > CURVATURE_COMMAND_MAX_INVERSE_KM) + { + encodedCurvature = 32127 + ZERO_CURVATURE_INVERSE_KM; // Clamp to maximum value + CANStackLogger::warn("[Guidance]: Transmitting an estimated curvature clamped to maximum value. Verify guidance calculations are accurate!"); + } + else if (scaledCurvature < 0) // 0 In this case is -8032 km-1 due to the addition of the offset earlier + { + encodedCurvature = 0; // Clamp to minimum value + CANStackLogger::warn("[Guidance]: Transmitting an estimated curvature clamped to minimum value. Verify guidance calculations are accurate!"); + } + else + { + encodedCurvature = static_cast(scaledCurvature); + } + + std::array buffer = { + static_cast(encodedCurvature & 0xFF), + static_cast((encodedCurvature >> 8) & 0xFF), + static_cast((static_cast(guidanceMachineInfoTransmitData.get_mechanical_system_lockout()) & 0x03) | + ((static_cast(guidanceMachineInfoTransmitData.get_guidance_steering_system_readiness_state()) & 0x03) << 2) | + ((static_cast(guidanceMachineInfoTransmitData.get_guidance_steering_input_position_status()) & 0x03) << 4) | + ((static_cast(guidanceMachineInfoTransmitData.get_request_reset_command_status()) & 0x03) << 6)), + static_cast(static_cast(guidanceMachineInfoTransmitData.get_guidance_limit_status()) << 5), + static_cast((guidanceMachineInfoTransmitData.get_guidance_system_command_exit_reason_code() & 0x3F) | + (static_cast(guidanceMachineInfoTransmitData.get_guidance_system_remote_engage_switch_status()) << 6)), + 0xFF, // Reserved + 0xFF, // Reserved + 0xFF // Reserved + }; + + retVal = CANNetworkManager::CANNetwork.send_can_message(static_cast(CANLibParameterGroupNumber::AgriculturalGuidanceMachineInfo), + buffer.data(), + buffer.size(), + std::static_pointer_cast(guidanceMachineInfoTransmitData.get_sender_control_function()), + destinationControlFunction, + CANIdentifier::Priority3); + } + return retVal; + } + + void AgriculturalGuidanceInterface::update() + { + if (initialized) + { + receivedGuidanceMachineInfoMessages.erase(std::remove_if(receivedGuidanceMachineInfoMessages.begin(), + receivedGuidanceMachineInfoMessages.end(), + [](std::shared_ptr guidanceInfo) { + return SystemTiming::time_expired_ms(guidanceInfo->get_timestamp_ms(), GUIDANCE_MESSAGE_TIMEOUT_MS); + }), + receivedGuidanceMachineInfoMessages.end()); + receivedGuidanceSystemCommandMessages.erase(std::remove_if(receivedGuidanceSystemCommandMessages.begin(), + receivedGuidanceSystemCommandMessages.end(), + [](std::shared_ptr guidanceCommand) { + return SystemTiming::time_expired_ms(guidanceCommand->get_timestamp_ms(), GUIDANCE_MESSAGE_TIMEOUT_MS); + }), + receivedGuidanceSystemCommandMessages.end()); + + if (SystemTiming::time_expired_ms(guidanceMachineInfoTransmitTimestamp_ms, GUIDANCE_MESSAGE_TX_INTERVAL_MS) && + (nullptr != guidanceMachineInfoTransmitData.get_sender_control_function())) + { + txFlags.set_flag(static_cast(TransmitFlags::SendGuidanceMachineInfo)); + guidanceMachineInfoTransmitTimestamp_ms = SystemTiming::get_timestamp_ms(); + } + if (SystemTiming::time_expired_ms(guidanceSystemCommandTransmitTimestamp_ms, GUIDANCE_MESSAGE_TX_INTERVAL_MS) && + (nullptr != guidanceSystemCommandTransmitData.get_sender_control_function())) + { + txFlags.set_flag(static_cast(TransmitFlags::SendGuidanceSystemCommand)); + guidanceSystemCommandTransmitTimestamp_ms = SystemTiming::get_timestamp_ms(); + } + txFlags.process_all_flags(); + } + else + { + CANStackLogger::error("[Guidance]: Guidance interface has not been initialized yet."); + } + } + + void AgriculturalGuidanceInterface::process_flags(std::uint32_t flag, void *parentPointer) + { + if (nullptr != parentPointer) + { + auto targetInterface = static_cast(parentPointer); + bool transmitSuccessful = false; + + switch (flag) + { + case static_cast(TransmitFlags::SendGuidanceMachineInfo): + { + transmitSuccessful = targetInterface->send_guidance_machine_info(); + } + break; + + case static_cast(TransmitFlags::SendGuidanceSystemCommand): + { + transmitSuccessful = targetInterface->send_guidance_system_command(); + } + break; + + default: + break; + } + + if (false == transmitSuccessful) + { + targetInterface->txFlags.set_flag(flag); + } + } + } + + void AgriculturalGuidanceInterface::process_rx_message(const CANMessage &message, void *parentPointer) + { + assert(nullptr != parentPointer); + auto targetInterface = static_cast(parentPointer); + + switch (message.get_identifier().get_parameter_group_number()) + { + case static_cast(CANLibParameterGroupNumber::AgriculturalGuidanceSystemCommand): + { + if (CAN_DATA_LENGTH == message.get_data_length()) + { + if (message.get_source_control_function() != nullptr) + { + auto result = std::find_if(targetInterface->receivedGuidanceSystemCommandMessages.begin(), + targetInterface->receivedGuidanceSystemCommandMessages.end(), + [&message](const std::shared_ptr &receivedCommand) { + return (nullptr != receivedCommand) && (receivedCommand->get_sender_control_function() == message.get_source_control_function()); + }); + + if (result == targetInterface->receivedGuidanceSystemCommandMessages.end()) + { + // There is no existing message object from this control function, so create a new one + targetInterface->receivedGuidanceSystemCommandMessages.push_back(std::make_shared(message.get_source_control_function())); + result = targetInterface->receivedGuidanceSystemCommandMessages.end() - 1; + } + + auto guidanceCommand = *result; + bool changed = false; + + changed |= guidanceCommand->set_curvature((message.get_uint16_at(0) * CURVATURE_COMMAND_RESOLUTION_PER_BIT) - CURVATURE_COMMAND_OFFSET_INVERSE_KM); + changed |= guidanceCommand->set_status(static_cast(message.get_uint8_at(2) & 0x03)); + guidanceCommand->set_timestamp_ms(SystemTiming::get_timestamp_ms()); + + targetInterface->guidanceSystemCommandEventPublisher.call(guidanceCommand, changed); + } + } + else + { + CANStackLogger::warn("[Guidance]: Received a malformed guidance system command message. DLC must be 8."); + } + } + break; + + case static_cast(CANLibParameterGroupNumber::AgriculturalGuidanceMachineInfo): + { + if (CAN_DATA_LENGTH == message.get_data_length()) + { + if (message.get_source_control_function() != nullptr) + { + auto result = std::find_if(targetInterface->receivedGuidanceMachineInfoMessages.cbegin(), + targetInterface->receivedGuidanceMachineInfoMessages.cend(), + [&message](const std::shared_ptr &receivedInfo) { + return (nullptr != receivedInfo) && (receivedInfo->get_sender_control_function() == message.get_source_control_function()); + }); + + if (result == targetInterface->receivedGuidanceMachineInfoMessages.cend()) + { + // There is no existing message object from this control function, so create a new one + targetInterface->receivedGuidanceMachineInfoMessages.push_back(std::make_shared(message.get_source_control_function())); + result = targetInterface->receivedGuidanceMachineInfoMessages.cend() - 1; + } + + auto machineInfo = *result; + bool changed = false; + + changed |= machineInfo->set_estimated_curvature((message.get_uint16_at(0) * CURVATURE_COMMAND_RESOLUTION_PER_BIT) - CURVATURE_COMMAND_OFFSET_INVERSE_KM); + changed |= machineInfo->set_mechanical_system_lockout_state(static_cast(message.get_uint8_at(2) & 0x03)); + changed |= machineInfo->set_guidance_steering_system_readiness_state(static_cast((message.get_uint8_at(2) >> 2) & 0x03)); + changed |= machineInfo->set_guidance_steering_input_position_status(static_cast((message.get_uint8_at(2) >> 4) & 0x03)); + changed |= machineInfo->set_request_reset_command_status(static_cast((message.get_uint8_at(2) >> 6) & 0x03)); + changed |= machineInfo->set_guidance_limit_status(static_cast(message.get_uint8_at(3) >> 5)); + changed |= machineInfo->set_guidance_system_command_exit_reason_code(message.get_uint8_at(4) & 0x3F); + changed |= machineInfo->set_guidance_system_remote_engage_switch_status(static_cast((message.get_uint8_at(4) >> 6) & 0x03)); + machineInfo->set_timestamp_ms(SystemTiming::get_timestamp_ms()); + + targetInterface->guidanceMachineInfoEventPublisher.call(machineInfo, changed); + } + } + else + { + CANStackLogger::warn("[Guidance]: Received a malformed guidance machine info message. DLC must be 8."); + } + } + break; + + default: + break; + } + } +} // namespace isobus diff --git a/src/isobus_guidance_interface.hpp b/src/isobus_guidance_interface.hpp new file mode 100644 index 0000000..5772fff --- /dev/null +++ b/src/isobus_guidance_interface.hpp @@ -0,0 +1,405 @@ +//================================================================================================ +/// @file isobus_guidance_interface.hpp +/// +/// @brief Defines an interface for sending and receiving ISOBUS guidance messages. +/// These messages are used to steer ISOBUS compliant machines, steering valves, and +/// implements in general. +/// +/// @attention Please use extreme care if you try to steer a machine with this interface! +/// Remember that this library is licensed under The MIT License, and that by obtaining a +/// copy of this library and of course by attempting to steer a machine with it, you are agreeing +/// to our license. +/// +/// @note These messages are expected to be deprecated or at least made redundant in favor +/// of Tractor Implement Management (TIM) at some point by the AEF, though the timeline on that +/// is not known at the time of writing this, and it's likely that many machines will +/// continue to support this interface going forward due to its simplicity over TIM. +/// +/// @author Adrian Del Grosso +/// +/// @copyright 2023 Adrian Del Grosso +//================================================================================================ +#ifndef ISOBUS_GUIDANCE_INTERFACE_HPP +#define ISOBUS_GUIDANCE_INTERFACE_HPP + +#include "can_internal_control_function.hpp" +#include "event_dispatcher.hpp" +#include "processing_flags.hpp" + +#include +#include + +namespace isobus +{ + /// @brief An interface for sending and receiving ISOBUS guidance messages + class AgriculturalGuidanceInterface + { + public: + /// @brief Constructor for a AgriculturalGuidanceInterface + /// @param[in] source The internal control function to use when sending messages, or nullptr for listen only + /// @param[in] destination The destination control function for transmitted messages, or nullptr for broadcasts + /// @param[in] enableSendingSystemCommandPeriodically If true, the system command message will be sent periodically. This should (only) be used by an guidance application trying to steer a machine. + /// @param[in] enableSendingMachineInfoPeriodically If true, the machine info message will be sent periodically. This should (only) be used by the steering controller itself. + AgriculturalGuidanceInterface(std::shared_ptr source, + std::shared_ptr destination, + bool enableSendingSystemCommandPeriodically = false, + bool enableSendingMachineInfoPeriodically = false); + + /// @brief Destructor for the AgriculturalGuidanceInterface + ~AgriculturalGuidanceInterface(); + + /// @brief An interface for sending the agricultural + /// guidance system command message. + /// + /// @details This message is sent by an automatic guidance control system to the + /// machine steering system. It provides steering commands + /// and serves as heartbeat between guidance system and steering control system. + class GuidanceSystemCommand + { + public: + /// @brief This parameter indicates whether the guidance system is + /// attempting to control steering with this command. + enum class CurvatureCommandStatus : std::uint8_t + { + NotIntendedToSteer = 0, ///< Steering Disengaged + IntendedToSteer = 1, ///< Steering Engaged + Error = 2, + NotAvailable = 3 + }; + + /// @brief Constructor for a GuidanceSystemCommand + /// @param[in] sender The control function that is sending this message + explicit GuidanceSystemCommand(std::shared_ptr sender); + + /// @brief Sets the curvature command status that will be encoded into + /// the CAN message. This parameter indicates whether the guidance system is + /// attempting to control steering with this command + /// @param[in] newStatus The status to encode into the message + /// @returns True if the status changed, false otherwise + bool set_status(CurvatureCommandStatus newStatus); + + /// @brief Returns the curvature command status that is active in the guidance system + /// @returns The curvature command status + CurvatureCommandStatus get_status() const; + + /// @brief Desired course curvature over ground that a machine's + /// steering system is required to achieve. + /// @details The value you set here will be encoded into the + /// guidance curvature command message. + /// + /// The desired path is determined by the automatic guidance system expressed + /// as the inverse of the instantaneous radius of curvature of the turn. + /// Curvature is positive when the vehicle is moving forward and turning to the driver's right + /// + /// @param[in] curvature Commanded curvature in km^-1 (inverse kilometers). Range is -8032 to 8031.75 km-1 + /// @returns True if the curvature changed, false otherwise + bool set_curvature(float curvature); + + /// @brief Returns the curvature value that is currently be trying to be achieved by the guidance system + /// @returns Commanded curvature in km^-1 (inverse kilometers). Range is -8032 to 8031.75 km-1 + float get_curvature() const; + + /// @brief Returns a pointer to the sender of the message. If an ICF is the sender, returns the ICF being used to transmit from. + /// @returns The control function sending this instance of the guidance system command message + std::shared_ptr get_sender_control_function() const; + + /// @brief Sets the timestamp for when the message was received or sent + /// @param[in] timestamp The timestamp, in milliseconds, when the message was sent or received + void set_timestamp_ms(std::uint32_t timestamp); + + /// @brief Returns the timestamp for when the message was received, in milliseconds + /// @returns The timestamp for when the message was received, in milliseconds + std::uint32_t get_timestamp_ms() const; + + private: + std::shared_ptr const controlFunction; ///< The CF that is sending the message + float commandedCurvature = 0.0f; ///< The commanded curvature in km^-1 (inverse kilometers) + std::uint32_t timestamp_ms = 0; ///< A timestamp for when the message was released in milliseconds + CurvatureCommandStatus commandedStatus = CurvatureCommandStatus::NotAvailable; ///< The current status for the command + }; + + /// @brief An interface for sending and receiving the ISOBUS agricultural guidance machine message + class GuidanceMachineInfo + { + public: + /// @brief State of a lockout switch that allows operators to + /// disable automatic steering system functions. + /// @details https://www.isobus.net/isobus/pGNAndSPN/1221?type=SPN + enum class MechanicalSystemLockout : std::uint8_t + { + NotActive = 0, + Active = 1, + Error = 2, + NotAvailable = 3 + }; + + /// @brief Machine steering system request to the automatic guidance system to + /// change Curvature Command Status state from "Intended to steer" to "Not intended to steer". + enum class RequestResetCommandStatus : std::uint8_t + { + ResetNotRequired = 0, + ResetRequired = 1, + Error = 2, + NotAvailable = 3 + }; + + /// @brief A typical, generic 2 bit value in J1939 with no superseding definition in ISO 11783 + enum class GenericSAEbs02SlotValue : std::uint8_t + { + DisabledOffPassive = 0, + EnabledOnActive = 1, + ErrorIndication = 2, + NotAvailableTakeNoAction = 3 + }; + + /// @brief This parameter is used to report the steering system's present limit status + /// associated with guidance commands that are persistent (i.e. not transient/temporary/one-shot). + enum class GuidanceLimitStatus : std::uint8_t + { + NotLimited = 0, ///< Not limited + OperatorLimitedControlled = 1, ///< Request cannot be implemented + LimitedHigh = 2, ///< Only lower command values result in a change + LimitedLow = 3, ///< Only higher command values result in a change + Reserved_1 = 4, ///< Reserved + Reserved_2 = 5, ///< Reserved + NonRecoverableFault = 6, ///< Non-recoverable fault + NotAvailable = 7 ///< Parameter not supported + }; + + /// @brief This parameter is used to indicate why the guidance system cannot currently accept remote + /// commands or has most recently stopped accepting remote commands. + enum class GuidanceSystemCommandExitReasonCode + { + NoReasonAllClear = 0, + RequiredLevelOfOperatorPresenceAwarenessNotDetected = 1, + ImplementReleasedControlOfFunction = 2, + OperatorOverrideOfFunction = 3, + OperatorControlNotInValidPosition = 4, + RemoteCommandTimeout = 5, + RemoteCommandOutOfRangeInvalid = 6, + FunctionNotCalibrated = 7, + OperatorControlFault = 8, + FunctionFault = 9, + HydraulicOilLevelTooLow = 20, + HydraulicOilTemperatureTooLow = 21, + VehicleTransmissionGearDoesNotAllowRemoteCommands = 22, ///< park, etc. + VehicleSpeedTooLow = 23, + VehicleSpeedTooHigh = 24, + AlternateGuidanceSystemActive = 25, + ControlUnitInDiagnosticMode = 26, + Error = 62, + NotAvailable = 63 ///< Parameter not supported + }; + + /// @brief Constructor for a GuidanceMachineInfo + /// @param[in] sender The control function that is sending this message + explicit GuidanceMachineInfo(std::shared_ptr sender); + + /// @brief Sets the estimated course curvature over ground for the machine. + /// @param[in] curvature The curvature in km^-1 (inverse kilometers). Range is -8032 to 8031.75 km-1 + /// @returns True if the curvature changed, false otherwise + bool set_estimated_curvature(float curvature); + + /// @brief Returns the estimated curvature that was previously set with set_estimated_curvature + /// @returns The estimated curvature in km^-1 (inverse kilometers). Range is -8032 to 8031.75 km-1 + float get_estimated_curvature() const; + + /// @brief Sets the mechanical system lockout state + /// @param[in] state The mechanical system lockout state to report + /// @returns True if the mechanical system lockout state changed, false otherwise + bool set_mechanical_system_lockout_state(MechanicalSystemLockout state); + + /// @brief Returns the mechanical system lockout state + /// @returns The mechanical system lockout state being reported + MechanicalSystemLockout get_mechanical_system_lockout() const; + + /// @brief Sets the guidance system's readiness state to report + /// @param[in] state The state to report. See definition of GenericSAEbs02SlotValue + /// @returns True if the guidance steering system readiness state changed, false otherwise + bool set_guidance_steering_system_readiness_state(GenericSAEbs02SlotValue state); + + /// @brief Returns the guidance system's readiness state for steering + /// @returns The guidance system's readiness state for steering + GenericSAEbs02SlotValue get_guidance_steering_system_readiness_state() const; + + /// @brief Sets the guidance steering input position state + /// @param[in] state The state to set for the guidance steering input position + /// @returns True if the guidance steering input position status changed, false otherwise + bool set_guidance_steering_input_position_status(GenericSAEbs02SlotValue state); + + /// @brief Returns the guidance steering input position state + /// @returns Guidance steering input position state + GenericSAEbs02SlotValue get_guidance_steering_input_position_status() const; + + /// @brief Sets the request reset command to report + /// @details Machine steering system request to the automatic guidance system to + /// change Curvature Command Status state from "Intended to steer" to "Not intended to steer". + /// @param[in] state The request reset command state to report + /// @returns True if the request reset command status changed, false otherwise + bool set_request_reset_command_status(RequestResetCommandStatus state); + + /// @brief Returns the reported request reset command + /// @details Machine steering system request to the automatic guidance system to + /// change Curvature Command Status state from "Intended to steer" to "Not intended to steer". + /// @returns The reported request reset command + RequestResetCommandStatus get_request_reset_command_status() const; + + /// @brief Sets the reported guidance limit status + /// @details This parameter is used to report the steering system's present + /// limit status associated with guidance commands that are persistent + /// (i.e. not transient/temporary/one-shot). + /// @param[in] status The limit status to report + /// @returns True if the guidance limit status changed, false otherwise + bool set_guidance_limit_status(GuidanceLimitStatus status); + + /// @brief Returns the reported guidance limit status + /// @details This parameter is used to report the steering system's present + /// limit status associated with guidance commands that are persistent + /// (i.e. not transient/temporary/one-shot). + /// @returns The reported guidance limit status + GuidanceLimitStatus get_guidance_limit_status() const; + + /// @brief Sets the exit code for the guidance system + /// @details This parameter is used to indicate why the guidance system cannot currently accept + /// remote commands or has most recently stopped accepting remote commands. + /// @returns The exit code for the guidance system + bool set_guidance_system_command_exit_reason_code(std::uint8_t exitCode); + + /// @brief Returns the exit code for the guidance system + /// @details This parameter is used to indicate why the guidance system cannot currently accept + /// remote commands or has most recently stopped accepting remote commands. + /// @returns The exit code for the guidance system + std::uint8_t get_guidance_system_command_exit_reason_code() const; + + /// @brief Sets the state for the steering engage switch + /// @param[in] switchStatus The engage switch state to report + /// @returns True if the engage switch state changed, false otherwise + bool set_guidance_system_remote_engage_switch_status(GenericSAEbs02SlotValue switchStatus); + + /// @brief Returns the state for the steering engage switch + /// @returns The state for the steering engage switch + GenericSAEbs02SlotValue get_guidance_system_remote_engage_switch_status() const; + + /// @brief Returns a pointer to the sender of the message. If an ICF is the sender, returns the ICF being used to transmit from. + /// @returns The control function sending this instance of the guidance system command message + std::shared_ptr get_sender_control_function() const; + + /// @brief Sets the timestamp for when the message was received or sent + /// @param[in] timestamp The timestamp, in milliseconds, when the message was sent or received + void set_timestamp_ms(std::uint32_t timestamp); + + /// @brief Returns the timestamp for when the message was received, in milliseconds + /// @returns The timestamp for when the message was received, in milliseconds + std::uint32_t get_timestamp_ms() const; + + private: + std::shared_ptr const controlFunction; ///< The CF that is sending the message + float estimatedCurvature = 0.0f; ///< Curvature in km^-1 (inverse kilometers). Range is -8032 to 8031.75 km-1 (SPN 5238) + std::uint32_t timestamp_ms = 0; ///< A timestamp for when the message was released in milliseconds + MechanicalSystemLockout mechanicalSystemLockoutState = MechanicalSystemLockout::NotAvailable; ///< The reported state of the mechanical system lockout switch (SPN 5243) + GenericSAEbs02SlotValue guidanceSteeringSystemReadinessState = GenericSAEbs02SlotValue::NotAvailableTakeNoAction; ///< The reported state of the steering system's readiness to steer (SPN 5242) + GenericSAEbs02SlotValue guidanceSteeringInputPositionStatus = GenericSAEbs02SlotValue::NotAvailableTakeNoAction; ///< The reported state of the steering input position. (SPN 5241) + GenericSAEbs02SlotValue guidanceSystemRemoteEngageSwitchStatus = GenericSAEbs02SlotValue::NotAvailableTakeNoAction; ///< The reported state of the remote engage switch (SPN 9726) + RequestResetCommandStatus requestResetCommandStatus = RequestResetCommandStatus::NotAvailable; ///< The reported state of the request reset command (SPN 5240) + GuidanceLimitStatus guidanceLimitStatus = GuidanceLimitStatus::NotAvailable; ///< The steering system's present limit status associated with guidance commands that are persistent (SPN 5726) + std::uint8_t guidanceSystemCommandExitReasonCode = static_cast(GuidanceSystemCommandExitReasonCode::NotAvailable); ///< The exit code for guidance, stored as a u8 to preserve manufacturer specific values (SPN 5725) + }; + + /// @brief Sets up the class and registers it to receive callbacks from the network manager for processing + /// guidance messages. The class will not receive messages if this is not called. + void initialize(); + + /// @brief Returns if the interface has been initialized + /// @returns true if initialize has been called for this interface, otherwise false + bool get_initialized() const; + + /// @brief Use this to configure the transmission of the guidance machine info message from your application. If you pass in an internal control function + /// to the constructor of this class, then this message is available to be sent. + GuidanceMachineInfo guidanceMachineInfoTransmitData; + + /// @brief Use this to configure transmission the guidance system command message from your application. If you pass in an internal control function + /// to the constructor of this class, then this message is available to be sent. + GuidanceSystemCommand guidanceSystemCommandTransmitData; + + /// @brief Returns the number of received, unique guidance system command sources + /// @returns The number of CFs sending the guidance system command either as a broadcast, or to our internal control function + std::size_t get_number_received_guidance_system_command_sources() const; + + /// @brief Returns the number of received, unique guidance machine info message sources + /// @returns The number of CFs sending the guidance machine info message either as a broadcast, or to our internal control function + std::size_t get_number_received_guidance_machine_info_message_sources() const; + + /// @brief Returns the content of the agricultural guidance machine info message + /// based on the index of the sender. Use this to read the received messages' content. + /// @param[in] index An index of senders of the agricultural guidance machine info message + /// @note Only one device on the bus will send this normally, but we provide a generic way to get + /// an arbitrary number of these commands. So generally using only index 0 will be acceptable. + std::shared_ptr get_received_guidance_machine_info(std::size_t index); + + /// @brief Returns the content of the agricultural guidance curvature command message + /// based on the index of the sender. Use this to read the received messages' content. + /// @param[in] index An index of senders of the agricultural guidance curvature command message + /// @note Only one device on the bus will send this normally, but we provide a generic way to get + /// an arbitrary number of these commands. So generally using only index 0 will be acceptable. + std::shared_ptr get_received_guidance_system_command(std::size_t index); + + /// @brief Returns an event dispatcher which you can use to get callbacks when new/updated guidance machine info messages are received. + /// @returns The event publisher for guidance machine info messages + EventDispatcher, bool> &get_guidance_machine_info_event_publisher(); + + /// @brief Returns an event dispatcher which you can use to get callbacks when new/updated guidance system command messages are received. + /// @returns The event publisher for guidance system command messages + EventDispatcher, bool> &get_guidance_system_command_event_publisher(); + + /// @brief Call this cyclically to update the interface. Transmits messages if needed and processes + /// timeouts for received messages. + void update(); + + protected: + /// @brief Enumerates a set of flags to manage transmitting messages owned by this interface + enum class TransmitFlags : std::uint32_t + { + SendGuidanceSystemCommand = 0, ///< A flag to manage sending the guidance system command message + SendGuidanceMachineInfo, ///< A flag to manage sending the guidance machine info message + + NumberOfFlags ///< The number of flags in this enumeration + }; + + /// @brief Processes one flag (which sends the associated message) + /// @param[in] flag The flag to process + /// @param[in] parentPointer A pointer to the interface instance + static void process_flags(std::uint32_t flag, void *parentPointer); + + /// @brief Processes a CAN message + /// @param[in] message The CAN message being received + /// @param[in] parentPointer A context variable to find the relevant instance of this class + static void process_rx_message(const CANMessage &message, void *parentPointer); + + static constexpr std::uint32_t GUIDANCE_MESSAGE_TX_INTERVAL_MS = 100; ///< How often guidance messages are sent, defined in ISO 11783-7 + static constexpr std::uint32_t GUIDANCE_MESSAGE_TIMEOUT_MS = 150; ///< Amount of time before a guidance message is stale. We currently tolerate 50ms of delay. + static constexpr float CURVATURE_COMMAND_OFFSET_INVERSE_KM = 8032.0f; ///< Constant offset for curvature being sent on the bus in km-1 + static constexpr float CURVATURE_COMMAND_MAX_INVERSE_KM = 8031.75f; ///< The maximum curvature that can be encoded once scaling is applied + static constexpr float CURVATURE_COMMAND_RESOLUTION_PER_BIT = 0.25f; ///< The resolution of the message in km-1 per bit + static constexpr std::uint16_t ZERO_CURVATURE_INVERSE_KM = 32128; ///< This is the value for zero km-1 for 0.25 km-1 per bit + + /// @brief Sends the agricultural guidance machine info message based on the configured content of guidanceMachineInfoTransmitData + /// @returns true if the message was sent, otherwise false + bool send_guidance_machine_info() const; + + /// @brief Sends the agricultural guidance system command message based on the configured content of guidanceSystemCommandTransmitData + /// @returns true if the message was sent, otherwise false + bool send_guidance_system_command() const; + + ProcessingFlags txFlags; ///< Tx flag for sending messages periodically + EventDispatcher, bool> guidanceMachineInfoEventPublisher; ///< An event publisher for notifying when new guidance machine info messages are received + EventDispatcher, bool> guidanceSystemCommandEventPublisher; ///< An event publisher for notifying when new guidance system commands are received + std::shared_ptr destinationControlFunction; ///< The optional destination to which messages will be sent. If nullptr it will be broadcast instead. + std::vector> receivedGuidanceMachineInfoMessages; ///< A list of all received estimated curvatures + std::vector> receivedGuidanceSystemCommandMessages; ///< A list of all received curvature commands and statuses + std::uint32_t guidanceSystemCommandTransmitTimestamp_ms = 0; ///< Timestamp used to know when to transmit the guidance system command message + std::uint32_t guidanceMachineInfoTransmitTimestamp_ms = 0; ///< Timestamp used to know when to transmit the guidance machine info message + bool initialized = false; ///< Stores if the interface has been initialized + }; +} // namespace isobus + +#endif // ISOBUS_GUIDANCE_HPP \ No newline at end of file diff --git a/src/isobus_language_command_interface.cpp b/src/isobus_language_command_interface.cpp new file mode 100644 index 0000000..beca8f0 --- /dev/null +++ b/src/isobus_language_command_interface.cpp @@ -0,0 +1,232 @@ +//================================================================================================ +/// @file isobus_language_command_interface.cpp +/// +/// @brief Defines a set of values found in the isobus language command message from +/// ISO11783-7 commonly used in VT and TC communication +/// @author Adrian Del Grosso +/// +/// @copyright 2023 Adrian Del Grosso +//================================================================================================ +#include "isobus_language_command_interface.hpp" + +#include "can_constants.hpp" +#include "can_general_parameter_group_numbers.hpp" +#include "can_parameter_group_number_request_protocol.hpp" +#include "can_partnered_control_function.hpp" +#include "can_stack_logger.hpp" +#include "system_timing.hpp" +#include "to_string.hpp" + +#include + +namespace isobus +{ + LanguageCommandInterface::LanguageCommandInterface(std::shared_ptr sourceControlFunction) : + myControlFunction(sourceControlFunction), + myPartner(nullptr) + { + } + + LanguageCommandInterface::LanguageCommandInterface(std::shared_ptr sourceControlFunction, std::shared_ptr filteredControlFunction) : + myControlFunction(sourceControlFunction), + myPartner(filteredControlFunction) + { + } + + LanguageCommandInterface ::~LanguageCommandInterface() + { + if (initialized) + { + CANNetworkManager::CANNetwork.remove_global_parameter_group_number_callback(static_cast(CANLibParameterGroupNumber::LanguageCommand), process_rx_message, this); + } + } + + void LanguageCommandInterface::initialize() + { + if (!initialized) + { + if (nullptr != myControlFunction) + { + CANNetworkManager::CANNetwork.add_global_parameter_group_number_callback(static_cast(CANLibParameterGroupNumber::LanguageCommand), process_rx_message, this); + initialized = true; + } + else + { + CANStackLogger::error("[VT/TC]: Language command interface is missing an internal control function, and will not be functional."); + } + } + else + { + CANStackLogger::warn("[VT/TC]: Language command interface has been initialized, but is being initialized again."); + } + } + + void LanguageCommandInterface::set_partner(std::shared_ptr filteredControlFunction) + { + myPartner = filteredControlFunction; + } + + bool LanguageCommandInterface::get_initialized() const + { + return initialized; + } + + bool LanguageCommandInterface::send_request_language_command() const + { + bool retVal = false; + + if (initialized) + { + retVal = ParameterGroupNumberRequestProtocol::request_parameter_group_number(static_cast(CANLibParameterGroupNumber::LanguageCommand), myControlFunction, myPartner); + } + else + { + // Make sure you call initialize first! + CANStackLogger::error("[VT/TC]: Language command interface is being used without being initialized!"); + } + return retVal; + } + + std::string LanguageCommandInterface::get_country_code() const + { + return countryCode; + } + + std::string LanguageCommandInterface::get_language_code() const + { + return languageCode; + } + + std::uint32_t LanguageCommandInterface::get_language_command_timestamp() const + { + return languageCommandTimestamp_ms; + } + + LanguageCommandInterface::DecimalSymbols LanguageCommandInterface::get_commanded_decimal_symbol() const + { + return decimalSymbol; + } + + LanguageCommandInterface::TimeFormats LanguageCommandInterface::get_commanded_time_format() const + { + return timeFormat; + } + + LanguageCommandInterface::DateFormats LanguageCommandInterface::get_commanded_date_format() const + { + return dateFormat; + } + + LanguageCommandInterface::DistanceUnits LanguageCommandInterface::get_commanded_distance_units() const + { + return distanceUnitSystem; + } + + LanguageCommandInterface::AreaUnits LanguageCommandInterface::get_commanded_area_units() const + { + return areaUnitSystem; + } + + LanguageCommandInterface::VolumeUnits LanguageCommandInterface::get_commanded_volume_units() const + { + return volumeUnitSystem; + } + + LanguageCommandInterface::MassUnits LanguageCommandInterface::get_commanded_mass_units() const + { + return massUnitSystem; + } + + LanguageCommandInterface::TemperatureUnits LanguageCommandInterface::get_commanded_temperature_units() const + { + return temperatureUnitSystem; + } + + LanguageCommandInterface::PressureUnits LanguageCommandInterface::get_commanded_pressure_units() const + { + return pressureUnitSystem; + } + + LanguageCommandInterface::ForceUnits LanguageCommandInterface::get_commanded_force_units() const + { + return forceUnitSystem; + } + + LanguageCommandInterface::UnitSystem LanguageCommandInterface::get_commanded_generic_units() const + { + return genericUnitSystem; + } + + const std::array LanguageCommandInterface::get_localization_raw_data() const + { + std::array retVal = { 0 }; + + if (languageCode.size() >= 2) + { + retVal[0] = languageCode[0]; + retVal[1] = languageCode[1]; + } + else + { + retVal[0] = ' '; + retVal[1] = ' '; + } + retVal[2] = ((static_cast(timeFormat) << 4) | + (static_cast(decimalSymbol) << 6)); + retVal[3] = static_cast(dateFormat); + retVal[4] = (static_cast(massUnitSystem) | + (static_cast(volumeUnitSystem) << 2) | + (static_cast(areaUnitSystem) << 4) | + (static_cast(distanceUnitSystem) << 6)); + retVal[5] = (static_cast(genericUnitSystem) | + (static_cast(forceUnitSystem) << 2) | + (static_cast(pressureUnitSystem) << 4) | + (static_cast(temperatureUnitSystem) << 6)); + retVal[6] = 0xFF; + return retVal; + } + + void LanguageCommandInterface::process_rx_message(const CANMessage &message, void *parentPointer) + { + auto *parentInterface = reinterpret_cast(parentPointer); + + if ((nullptr != parentInterface) && + (CAN_DATA_LENGTH <= message.get_data_length()) && + (static_cast(CANLibParameterGroupNumber::LanguageCommand) == message.get_identifier().get_parameter_group_number()) && + ((nullptr == parentInterface->myPartner) || + (message.get_source_control_function()->get_NAME() == parentInterface->myPartner->get_NAME()))) + { + const auto &data = message.get_data(); + parentInterface->languageCommandTimestamp_ms = SystemTiming::get_timestamp_ms(); + parentInterface->languageCode.clear(); + parentInterface->languageCode.push_back(static_cast(data.at(0))); + parentInterface->languageCode.push_back(static_cast(data.at(1))); + parentInterface->timeFormat = static_cast((data.at(2) >> 4) & 0x03); + parentInterface->decimalSymbol = static_cast((data.at(2) >> 6) & 0x03); + parentInterface->dateFormat = static_cast(data.at(3)); + parentInterface->massUnitSystem = static_cast(data.at(4) & 0x03); + parentInterface->volumeUnitSystem = static_cast((data.at(4) >> 2) & 0x03); + parentInterface->areaUnitSystem = static_cast((data.at(4) >> 4) & 0x03); + parentInterface->distanceUnitSystem = static_cast((data.at(4) >> 6) & 0x03); + parentInterface->genericUnitSystem = static_cast(data.at(5) & 0x03); + parentInterface->forceUnitSystem = static_cast((data.at(5) >> 2) & 0x03); + parentInterface->pressureUnitSystem = static_cast((data.at(5) >> 4) & 0x03); + parentInterface->temperatureUnitSystem = static_cast((data.at(5) >> 6) & 0x03); + parentInterface->countryCode.clear(); + + if ((0xFF != data.at(6)) || (0xFF != data.at(7))) + { + parentInterface->countryCode.push_back(static_cast(data.at(6))); + parentInterface->countryCode.push_back(static_cast(data.at(7))); + } + + CANStackLogger::debug("[VT/TC]: Language and unit data received from control function " + + isobus::to_string(static_cast(message.get_identifier().get_source_address())) + + " language is: " + + parentInterface->languageCode.c_str(), + " and country code is ", + parentInterface->countryCode.empty() ? "unknown." : parentInterface->countryCode.c_str()); + } + } + +} diff --git a/src/isobus_language_command_interface.hpp b/src/isobus_language_command_interface.hpp new file mode 100644 index 0000000..4911b4c --- /dev/null +++ b/src/isobus_language_command_interface.hpp @@ -0,0 +1,264 @@ +//================================================================================================ +/// @file isobus_language_command_interface.hpp +/// +/// @brief Defines a set of values found in the isobus language command message from +/// ISO11783-7 commonly used in VT and TC communication +/// @author Adrian Del Grosso +/// +/// @copyright 2023 Adrian Del Grosso +//================================================================================================ +#ifndef ISOBUS_LANGUAGE_COMMAND_INTERFACE_HPP +#define ISOBUS_LANGUAGE_COMMAND_INTERFACE_HPP + +#include "can_message.hpp" + +#include +#include + +namespace isobus +{ + class InternalControlFunction; + class PartneredControlFunction; + class ControlFunction; + + /// @brief An interface for requesting and parsing the ISO11783 language + /// command PGN, 0xFE0F. + /// @details This is a class that's meant to provide an easy interface for + /// dealing with the ISOBUS language command message. + /// This is meant for use inside the VT Client and TC Client classes, however + /// you can also use it standalone if you want. + class LanguageCommandInterface + { + public: + /// @brief Command sent to all CFs that determines whether a point or a comma will be + /// displayed as the decimal symbol. (SPN 2411) + enum class DecimalSymbols : std::uint8_t + { + Comma = 0, ///< A comma ',' is used + Point = 1, ///< A decimal point '.' is used + Reserved = 2, ///< Reserved + NoAction = 3 ///< Take No Action + }; + + /// @brief Command sent to all CFs specifying the displayed format of the date. (SPN 2412) + enum class DateFormats : std::uint8_t + { + ddmmyyyy = 0, ///< 31/12/2023 + ddyyyymm = 1, ///< 31/2023/12 + mmyyyydd = 2, ///< 12/2023/31 + mmddyyyy = 3, ///< 12/31/2023 + yyyymmdd = 4, ///< 2023/12/31 + yyyyddmm = 5, ///< 2023/31/12 + + ReservedStart = 6, ///< Reserved range begins here + ReservedEnd = 250 ///< Reserved range ends here + }; + + /// @brief Command sent to all CFs specifying the displayed format of the time. (SPN 2413) + enum class TimeFormats : std::uint8_t + { + TwentyFourHour = 0, ///< 24 h + TwelveHourAmPm = 1, ///< 12 h (am/pm) + Reserved = 2, ///< Reserved + NoAction = 3 ///< Take No Action + }; + + /// @brief Command specifying a distance unit. (SPN 2414) + enum class DistanceUnits : std::uint8_t + { + Metric = 0, ///< Kilometers, meters + ImperialUS = 1, ///< Miles, feet + Reserved = 2, ///< Reserved + NoAction = 3 ///< Take No Action + }; + + /// @brief Command specifying an area unit. (SPN 2415) + enum class AreaUnits : std::uint8_t + { + Metric = 0, ///< Hectares or m^2 + ImperialUS = 1, ///< Acres or ft^2 + Reserved = 2, ///< Reserved + NoAction = 3 ///< Take No Action + }; + + /// @brief Command specifying a volume unit. (SPN 2416) + enum class VolumeUnits : std::uint8_t + { + Metric = 0, ///< Litre + Imperial = 1, ///< Imperial Gallon + US = 2, ///< US Gallon + NoAction = 3 ///< Take No Action + }; + + /// @brief Command specifying a mass unit. (SPN 2417) + enum class MassUnits : std::uint8_t + { + Metric = 0, ///< Tonnes, Kilograms + Imperial = 1, ///< Long Tons, Pounds + US = 2, ///< Short Tons, Pounds + NoAction = 3 ///< Take No Action + }; + + /// @brief Command specifying a temperature unit. (SPN 5194) + enum class TemperatureUnits : std::uint8_t + { + Metric = 0, ///< Degrees Celsius, Degrees Kelvin + ImperialUS = 1, ///< Degrees Fahrenheit + Reserved = 2, ///< Reserved + NoAction = 3 ///< Take No Action + }; + + /// @brief Command specifying a pressure unit (SPN 5195) + enum class PressureUnits : std::uint8_t + { + Metric = 0, ///< Kilopascals, pascals + ImperialUS = 1, ///< Pounds per square inch + Reserved = 2, ///< Reserved + NoAction = 3 ///< Take No Action + }; + + /// @brief Command specifying a force unit (SPN 5196) + enum class ForceUnits : std::uint8_t + { + Metric = 0, ///< Newtons + ImperialUS = 1, ///< Pounds force + Reserved = 2, ///< Reserved + NoAction = 3 ///< Take No Action + }; + + /// @brief May be used for the display of any unit, or a unit other than + /// those explicitly specified (SPN 5197) + enum class UnitSystem : std::uint8_t + { + Metric = 0, ///< Generic metric + Imperial = 1, ///< Generic imperial + US = 2, ///< Generis US + NoAction = 3 ///< Take No Action + }; + + /// @brief Constructor for a LanguageCommandInterface + /// @details This constructor will make a version of the class that will accept the message from any source + /// @param sourceControlFunction The internal control function that the interface should communicate from + explicit LanguageCommandInterface(std::shared_ptr sourceControlFunction); + + /// @brief Constructor for a LanguageCommandInterface + /// @details This constructor will make a version of the class that will filter the message to be + /// only from the specified control function. + /// @param sourceControlFunction The internal control function that the interface should communicate from + /// @param filteredControlFunction The control function you want to explicitly communicate with + LanguageCommandInterface(std::shared_ptr sourceControlFunction, std::shared_ptr filteredControlFunction); + + /// @brief Deleted copy constructor for LanguageCommandInterface + LanguageCommandInterface(LanguageCommandInterface &) = delete; + + /// @brief Destructor for the LanguageCommandInterface + ~LanguageCommandInterface(); + + /// @brief Initializes the interface. + /// @details This needs to be called before the interface is useable. + /// It registers its PGN callback and sets up the PGN request interface + /// if needed. + void initialize(); + + /// @brief Changes the partner being used by the interface to a new partner + /// @param[in] filteredControlFunction The new partner to communicate with + void set_partner(std::shared_ptr filteredControlFunction); + + /// @brief Returns if initialize has been called yet + /// @return `true` if initialize has been called, otherwise false + bool get_initialized() const; + + /// @brief Sends a PGN request for the language command PGN to the interface's partner, or the global address + /// depending on if you set a partner when constructing the object + /// @return `true` if the message was sent, otherwise `false` + bool send_request_language_command() const; + + /// @brief Returns the commanded country code parsed from the last language command specifying the operator's desired language dialect. + /// @note ISO 11783 networks shall use the alpha-2 country codes in accordance with ISO 3166-1. + /// @return The commanded country code, or an empty string if none specified. + std::string get_country_code() const; + + /// @brief Returns the commanded language code parsed from the last language command + /// @note If you do not support the returned language, your default shall be used + /// @return The commanded language code (usually 2 characters length) + std::string get_language_code() const; + + /// @brief Returns a timestamp (in ms) corresponding to when the interface last received a language command + /// @return timestamp in milliseconds corresponding to when the interface last received a language command + std::uint32_t get_language_command_timestamp() const; + + /// @brief Returns the commanded decimal symbol parsed from the last language command + /// @return The decimal symbol that was last commanded + DecimalSymbols get_commanded_decimal_symbol() const; + + /// @brief Returns the commanded time format parsed from the last language command + /// @return The time format that was last commanded + TimeFormats get_commanded_time_format() const; + + /// @brief Returns the commanded date format parsed from the last language command + /// @return The date format that was last commanded + DateFormats get_commanded_date_format() const; + + /// @brief Returns the commanded distance units parsed from the last language command + /// @return The distance units that were last commanded + DistanceUnits get_commanded_distance_units() const; + + /// @brief Returns the commanded area units parsed from the last received language command + /// @return The area units that were last commanded + AreaUnits get_commanded_area_units() const; + + /// @brief Returns the commanded volume units parsed from the last received language command + /// @return The volume units that were last commanded + VolumeUnits get_commanded_volume_units() const; + + /// @brief Returns the commanded mass units parsed from the last received language command + /// @return The mass units that were last commanded + MassUnits get_commanded_mass_units() const; + + /// @brief Returns the commanded temperature units parsed from the last received language command + /// @return The temperature units that were last commanded + TemperatureUnits get_commanded_temperature_units() const; + + /// @brief Returns the commanded pressure units parsed from the last received language command + /// @return The pressure units that were last commanded + PressureUnits get_commanded_pressure_units() const; + + /// @brief Returns the commanded force units parsed from the last received language command + /// @return The force units that were last commanded + ForceUnits get_commanded_force_units() const; + + /// @brief Returns the commanded "unit system" generic value that was parsed from the last received language command + /// @return The commanded unit system + UnitSystem get_commanded_generic_units() const; + + /// @brief Returns The raw bytes that comprise the current localization data as defined in ISO11783-7 + /// @returns The raw bytes that comprise the current localization data + const std::array get_localization_raw_data() const; + + /// @brief Parses incoming CAN messages into usable unit and language settings + /// @param message The CAN message to parse + /// @param parentPointer A generic context variable, usually the `this` pointer for this interface instance + static void process_rx_message(const CANMessage &message, void *parentPointer); + + private: + std::shared_ptr myControlFunction; ///< The control function to send messages as + std::shared_ptr myPartner; ///< The partner to talk to, or nullptr to listen to all CFs + std::string countryCode; ///< The last received alpha-2 country code as specified by ISO 3166-1, such as "NL, FR, GB, US, DE". + std::string languageCode; ///< The last received language code, such as "en", "es", "de", etc. + std::uint32_t languageCommandTimestamp_ms = 0; ///< A millisecond timestamp correlated to the last received language command message + DecimalSymbols decimalSymbol = DecimalSymbols::Point; ///< The decimal symbol that was commanded by the last language command message + TimeFormats timeFormat = TimeFormats::TwelveHourAmPm; ///< The time format that was commanded by the last language command message + DateFormats dateFormat = DateFormats::mmddyyyy; ///< The date format that was commanded by the last language command message + DistanceUnits distanceUnitSystem = DistanceUnits::Metric; ///< The distance units that were commanded by the last language command message + AreaUnits areaUnitSystem = AreaUnits::Metric; ///< The area units that were commanded by the last language command message + VolumeUnits volumeUnitSystem = VolumeUnits::Metric; ///< The volume units that were commanded by the last language command message + MassUnits massUnitSystem = MassUnits::Metric; ///< The mass units that were commanded by the last language command message + TemperatureUnits temperatureUnitSystem = TemperatureUnits::Metric; ///< The temperature units that were commanded by the last language command message + PressureUnits pressureUnitSystem = PressureUnits::Metric; ///< The pressure units that were commanded by the last language command message + ForceUnits forceUnitSystem = ForceUnits::Metric; ///< The force units that were commanded by the last language command message + UnitSystem genericUnitSystem = UnitSystem::Metric; ///< The "unit system" that was commanded by the last language command message + bool initialized = false; ///< Tracks if initialize has been called yet for this interface + }; +} // namespace isobus + +#endif // ISOBUS_LANGUAGE_COMMAND_INTERFACE_HPP diff --git a/src/isobus_maintain_power_interface.cpp b/src/isobus_maintain_power_interface.cpp new file mode 100644 index 0000000..7a08855 --- /dev/null +++ b/src/isobus_maintain_power_interface.cpp @@ -0,0 +1,350 @@ +#include "isobus_maintain_power_interface.hpp" +#include "can_general_parameter_group_numbers.hpp" +#include "can_network_manager.hpp" +#include "can_stack_logger.hpp" +#include "system_timing.hpp" + +#include +#include +#include + +namespace isobus +{ + MaintainPowerInterface::MaintainPowerInterface(std::shared_ptr sourceControlFunction) : + maintainPowerTransmitData(sourceControlFunction), + txFlags(static_cast(TransmitFlags::NumberOfFlags), process_flags, this) + { + } + + MaintainPowerInterface::~MaintainPowerInterface() + { + if (initialized) + { + CANNetworkManager::CANNetwork.remove_any_control_function_parameter_group_number_callback(static_cast(CANLibParameterGroupNumber::MaintainPower), process_rx_message, this); + CANNetworkManager::CANNetwork.remove_any_control_function_parameter_group_number_callback(static_cast(CANLibParameterGroupNumber::WheelBasedSpeedAndDistance), process_rx_message, this); + } + } + + EventDispatcher, bool> &MaintainPowerInterface::get_maintain_power_data_event_publisher() + { + return maintainPowerDataEventPublisher; + } + + EventDispatcher<> &MaintainPowerInterface::get_key_switch_transition_off_event_publisher() + { + return keySwitchOffEventPublisher; + } + + void MaintainPowerInterface::update() + { + if (initialized) + { + receivedMaintainPowerMessages.erase(std::remove_if(receivedMaintainPowerMessages.begin(), + receivedMaintainPowerMessages.end(), + [](std::shared_ptr messageInfo) { + return SystemTiming::time_expired_ms(messageInfo->get_timestamp_ms(), MAINTAIN_POWER_TIMEOUT_MS); + }), + receivedMaintainPowerMessages.end()); + if (SystemTiming::time_expired_ms(maintainPowerTransmitTimestamp_ms, MAINTAIN_POWER_TIMEOUT_MS / 2) && + (0 != maintainPowerTransmitTimestamp_ms) && + (SystemTiming::get_time_elapsed_ms(keyOffTimestamp) < maintainPowerTime_ms) && + (nullptr != maintainPowerTransmitData.get_sender_control_function())) + { + txFlags.set_flag(static_cast(TransmitFlags::SendMaintainPower)); + maintainPowerTransmitTimestamp_ms = SystemTiming::get_timestamp_ms(); + } + txFlags.process_all_flags(); + } + else + { + CANStackLogger::error("[Maintain Power]: Interface has not been initialized yet."); + } + } + + MaintainPowerInterface::MaintainPowerData::MaintainPowerData(std::shared_ptr sendingControlFunction) : + sendingControlFunction(sendingControlFunction) + { + } + + bool MaintainPowerInterface::MaintainPowerData::set_implement_in_work_state(ImplementInWorkState inWorkState) + { + bool retVal = (inWorkState != currentImplementInWorkState); + currentImplementInWorkState = inWorkState; + return retVal; + } + + MaintainPowerInterface::MaintainPowerData::ImplementInWorkState MaintainPowerInterface::MaintainPowerData::get_implement_in_work_state() const + { + return currentImplementInWorkState; + } + + bool MaintainPowerInterface::MaintainPowerData::set_implement_ready_to_work_state(ImplementReadyToWorkState readyToWorkState) + { + bool retVal = (readyToWorkState != currentImplementReadyToWorkState); + currentImplementReadyToWorkState = readyToWorkState; + return retVal; + } + + MaintainPowerInterface::MaintainPowerData::ImplementReadyToWorkState MaintainPowerInterface::MaintainPowerData::get_implement_ready_to_work_state() const + { + return currentImplementReadyToWorkState; + } + + bool MaintainPowerInterface::MaintainPowerData::set_implement_park_state(ImplementParkState parkState) + { + bool retVal = (parkState != currentImplementParkState); + currentImplementParkState = parkState; + return retVal; + } + + MaintainPowerInterface::MaintainPowerData::ImplementParkState MaintainPowerInterface::MaintainPowerData::get_implement_park_state() const + { + return currentImplementParkState; + } + + bool MaintainPowerInterface::MaintainPowerData::set_implement_transport_state(ImplementTransportState transportState) + { + bool retVal = (transportState != currentImplementTransportState); + currentImplementTransportState = transportState; + return retVal; + } + + MaintainPowerInterface::MaintainPowerData::ImplementTransportState MaintainPowerInterface::MaintainPowerData::get_implement_transport_state() const + { + return currentImplementTransportState; + } + + bool MaintainPowerInterface::MaintainPowerData::set_maintain_actuator_power(MaintainActuatorPower maintainState) + { + bool retVal = (currentMaintainActuatorPowerState != maintainState); + currentMaintainActuatorPowerState = maintainState; + return retVal; + } + + MaintainPowerInterface::MaintainPowerData::MaintainActuatorPower MaintainPowerInterface::MaintainPowerData::get_maintain_actuator_power() const + { + return currentMaintainActuatorPowerState; + } + + bool MaintainPowerInterface::MaintainPowerData::set_maintain_ecu_power(MaintainECUPower maintainState) + { + bool retVal = (currentMaintainECUPowerState != maintainState); + currentMaintainECUPowerState = maintainState; + return retVal; + } + + MaintainPowerInterface::MaintainPowerData::MaintainECUPower MaintainPowerInterface::MaintainPowerData::get_maintain_ecu_power() const + { + return currentMaintainECUPowerState; + } + + std::shared_ptr MaintainPowerInterface::MaintainPowerData::get_sender_control_function() const + { + return sendingControlFunction; + } + + void MaintainPowerInterface::MaintainPowerData::set_timestamp_ms(std::uint32_t timestamp) + { + timestamp_ms = timestamp; + } + + std::uint32_t MaintainPowerInterface::MaintainPowerData::get_timestamp_ms() const + { + return timestamp_ms; + } + + void MaintainPowerInterface::initialize() + { + if (!initialized) + { + CANNetworkManager::CANNetwork.add_any_control_function_parameter_group_number_callback(static_cast(CANLibParameterGroupNumber::MaintainPower), process_rx_message, this); + CANNetworkManager::CANNetwork.add_any_control_function_parameter_group_number_callback(static_cast(CANLibParameterGroupNumber::WheelBasedSpeedAndDistance), process_rx_message, this); + initialized = true; + } + } + + bool MaintainPowerInterface::get_initialized() const + { + return initialized; + } + + void MaintainPowerInterface::set_maintain_power_time(std::uint32_t timeToMaintainPower) + { + maintainPowerTime_ms = timeToMaintainPower; + } + + std::uint32_t MaintainPowerInterface::get_maintain_power_time() const + { + return maintainPowerTime_ms; + } + + std::size_t MaintainPowerInterface::get_number_received_maintain_power_sources() const + { + return receivedMaintainPowerMessages.size(); + } + + std::shared_ptr MaintainPowerInterface::get_received_maintain_power(std::size_t index) + { + std::shared_ptr retVal = nullptr; + + if (index < receivedMaintainPowerMessages.size()) + { + retVal = receivedMaintainPowerMessages.at(index); + } + return retVal; + } + + bool MaintainPowerInterface::send_maintain_power() const + { + const std::array buffer = { + static_cast(0x0F | (static_cast(maintainPowerTransmitData.get_maintain_actuator_power()) << 4) | (static_cast(maintainPowerTransmitData.get_maintain_ecu_power()) << 6)), + static_cast(static_cast(maintainPowerTransmitData.get_implement_in_work_state()) | + static_cast(static_cast(maintainPowerTransmitData.get_implement_ready_to_work_state()) << 2) | + static_cast(static_cast(maintainPowerTransmitData.get_implement_park_state()) << 4) | + static_cast(static_cast(maintainPowerTransmitData.get_implement_transport_state()) << 6)), + 0xFF, + 0xFF, + 0xFF, + 0xFF, + 0xFF, + 0xFF + }; + + return CANNetworkManager::CANNetwork.send_can_message(static_cast(CANLibParameterGroupNumber::MaintainPower), + buffer.data(), + buffer.size(), + std::static_pointer_cast(maintainPowerTransmitData.get_sender_control_function())); + } + + void MaintainPowerInterface::process_flags(std::uint32_t flag, void *parentPointer) + { + if (static_cast(TransmitFlags::SendMaintainPower) == flag) + { + assert(nullptr != parentPointer); + auto targetInterface = static_cast(parentPointer); + + if (!targetInterface->send_maintain_power()) + { + targetInterface->txFlags.set_flag(static_cast(TransmitFlags::SendMaintainPower)); + } + } + } + + void MaintainPowerInterface::process_rx_message(const CANMessage &message, void *parentPointer) + { + assert(nullptr != parentPointer); + + auto targetInterface = static_cast(parentPointer); + + if (static_cast(CANLibParameterGroupNumber::WheelBasedSpeedAndDistance) == message.get_identifier().get_parameter_group_number()) + { + // This is checked in the speed interface as well, but we need to know about it for the key switch state. + if (CAN_DATA_LENGTH == message.get_data_length()) + { + if (nullptr != message.get_source_control_function()) + { + // We don't care who's sending this really, we just need to detect a transition from not-off to off. + const auto decodedKeySwitchState = static_cast((message.get_uint8_at(7) >> 2) & 0x03); + + switch (decodedKeySwitchState) + { + case KeySwitchState::Off: + { + if (0 != targetInterface->keyNotOffTimestamp) + { + CANStackLogger::info("[Maintain Power]: The key switch state has transitioned from NOT OFF to OFF."); + targetInterface->keyNotOffTimestamp = 0; + + // Send the maintain power message based on the key state transition + targetInterface->keyOffTimestamp = SystemTiming::get_timestamp_ms(); + targetInterface->txFlags.set_flag(static_cast(TransmitFlags::SendMaintainPower)); + targetInterface->maintainPowerTransmitTimestamp_ms = SystemTiming::get_timestamp_ms(); + targetInterface->keySwitchOffEventPublisher.invoke(); + } + else if (0 == targetInterface->keyOffTimestamp) + { + CANStackLogger::info("[Maintain Power]: The key switch state is detected as OFF."); + targetInterface->keyOffTimestamp = SystemTiming::get_timestamp_ms(); + } + } + break; + + case KeySwitchState::NotOff: + { + if (0 != targetInterface->keyOffTimestamp) + { + CANStackLogger::info("[Maintain Power]: The key switch state has transitioned from OFF to NOT OFF."); + targetInterface->keyOffTimestamp = 0; + targetInterface->keyNotOffTimestamp = SystemTiming::get_timestamp_ms(); + } + else if (0 == targetInterface->keyNotOffTimestamp) + { + CANStackLogger::info("[Maintain Power]: The key switch state is detected as NOT OFF."); + targetInterface->keyNotOffTimestamp = SystemTiming::get_timestamp_ms(); + } + targetInterface->maintainPowerTransmitTimestamp_ms = 0; + } + break; + + case KeySwitchState::Error: + { + CANStackLogger::warn("[Maintain Power]: The key switch is in an error state."); + targetInterface->keyOffTimestamp = 0; + targetInterface->keyNotOffTimestamp = 0; + targetInterface->maintainPowerTransmitTimestamp_ms = 0; + } + break; + + default: + { + // The "take no action" state, so we'll ignore it. + } + break; + } + } + } + else + { + CANStackLogger::warn("[Maintain Power]: Received malformed wheel based speed PGN. DLC must be 8."); + } + } + else if (static_cast(CANLibParameterGroupNumber::MaintainPower) == message.get_identifier().get_parameter_group_number()) + { + if (CAN_DATA_LENGTH == message.get_data_length()) + { + if (nullptr != message.get_source_control_function()) + { + auto result = std::find_if(targetInterface->receivedMaintainPowerMessages.cbegin(), + targetInterface->receivedMaintainPowerMessages.cend(), + [&message](const std::shared_ptr &receivedInfo) { + return (nullptr != receivedInfo) && (receivedInfo->get_sender_control_function() == message.get_source_control_function()); + }); + + if (result == targetInterface->receivedMaintainPowerMessages.end()) + { + // There is no existing message object from this control function, so create a new one + targetInterface->receivedMaintainPowerMessages.push_back(std::make_shared(message.get_source_control_function())); + result = targetInterface->receivedMaintainPowerMessages.end() - 1; + } + + auto &mpMessage = *result; + bool changed = false; + + changed |= mpMessage->set_maintain_actuator_power(static_cast((message.get_uint8_at(0) >> 4) & 0x03)); + changed |= mpMessage->set_maintain_ecu_power(static_cast((message.get_uint8_at(0) >> 6) & 0x03)); + changed |= mpMessage->set_implement_in_work_state(static_cast(message.get_uint8_at(1) & 0x03)); + changed |= mpMessage->set_implement_ready_to_work_state(static_cast((message.get_uint8_at(1) >> 2) & 0x03)); + changed |= mpMessage->set_implement_park_state(static_cast((message.get_uint8_at(1) >> 4) & 0x03)); + changed |= mpMessage->set_implement_transport_state(static_cast((message.get_uint8_at(1) >> 6) & 0x03)); + mpMessage->set_timestamp_ms(SystemTiming::get_timestamp_ms()); + + targetInterface->maintainPowerDataEventPublisher.call(mpMessage, changed); + } + } + else + { + CANStackLogger::warn("[Maintain Power]: Received malformed maintain power PGN. DLC must be 8."); + } + } + } +} // namespace isobus diff --git a/src/isobus_maintain_power_interface.hpp b/src/isobus_maintain_power_interface.hpp new file mode 100644 index 0000000..7cec012 --- /dev/null +++ b/src/isobus_maintain_power_interface.hpp @@ -0,0 +1,287 @@ +//================================================================================================ +/// @file isobus_maintain_power_interface.hpp +/// +/// @brief Defines an interface for sending and receiving the maintain power message (PGN 65095). +/// @details This interface is for managing the maintain power message, which is a +/// message sent by any CF connected to the implement bus requesting that the Tractor +/// ECU (TECU) not switch off the power for 2s after it has received the wheel-based speed and +/// distance message indicating that the ignition has been switched off. +/// The message also includes the connected implement(s) operating state. +/// You can choose if the TECU maintains actuator power independently of ECU power as well. +/// You might want to maintain actuator power to ensure your section valves close when keyed off. +/// @author Adrian Del Grosso +/// +/// @copyright 2023 Adrian Del Grosso +//================================================================================================ +#ifndef ISOBUS_MAINTAIN_POWER_INTERFACE_HPP +#define ISOBUS_MAINTAIN_POWER_INTERFACE_HPP + +#include "can_internal_control_function.hpp" +#include "can_message.hpp" +#include "event_dispatcher.hpp" +#include "processing_flags.hpp" + +#include + +namespace isobus +{ + /// @brief Manages sending and receiving the maintain power message (PGN 65095) + class MaintainPowerInterface + { + public: + /// @brief Stores information sent/received in a maintain power message. + class MaintainPowerData + { + public: + /// @brief Signal that indicates that an implement is connected to a tractor or power unit and is in work state. + /// @details SPN 7447 + enum class ImplementInWorkState : std::uint8_t + { + ImplementNotInWorkState = 0, + ImplementInWorkState = 1, + ErrorIndication = 2, + NotAvailable = 3 + }; + + /// @brief Signal that indicates that an implement is connected to a tractor or power unit and is ready for work. + /// @details SPN 1871 + enum class ImplementReadyToWorkState + { + ImplementNotReadyForFieldWork = 0, + ImplementReadyForFieldWork = 1, + ErrorIndication = 2, + NotAvailable = 3 + }; + + /// @brief Indicates the state of an implement where it may be disconnected from a tractor or power unit. + /// @details SPN 1870 + enum class ImplementParkState + { + ImplementMayNotBeDisconnected = 0, + ImplementMayBeDisconnected = 1, + ErrorIndication = 2, + NotAvailable = 3 + }; + + /// @brief Indicates the transport state of an implement connected to a tractor or power unit. + /// @details SPN 1869 + enum class ImplementTransportState + { + ImplementMayNotBeTransported = 0, + ImplementMayBeTransported = 1, + ErrorIndication = 2, + NotAvailable = 3 + }; + + /// @brief Enumerates the different states that can be requested in the "Maintain Actuator Power" SPN + /// @details SPN 1868 + enum class MaintainActuatorPower + { + NoFurtherRequirementForPWR = 0, + RequirementFor2SecondsMoreForPWR = 1, + Reserved = 2, + DontCare = 3 + }; + + /// @brief Enumerates the different states that can be requested in the "Maintain ECU Power" SPN + /// @details SPN 1867 + enum class MaintainECUPower + { + NoFurtherRequirementForECU_PWR = 0, + RequirementFor2SecondsMoreForECU_PWR = 1, + Reserved = 2, + DontCare = 3 + }; + + /// @brief Constructor for a MaintainPowerData object, which stores information sent/received in a maintain power message. + explicit MaintainPowerData(std::shared_ptr sendingControlFunction); + + /// @brief Sets the reported implement in-work state + /// @param[in] inWorkState The reported implement in-work state to set + /// @returns True if the set value was different from the stored value, otherwise false. + bool set_implement_in_work_state(ImplementInWorkState inWorkState); + + /// @brief Returns the reported implement in-work state + /// @return The reported implement in-work state + ImplementInWorkState get_implement_in_work_state() const; + + /// @brief Sets the reported implement ready to work state + /// @param[in] readyToWorkState The reported implement ready to work state + /// @returns True if the set value was different from the stored value, otherwise false. + bool set_implement_ready_to_work_state(ImplementReadyToWorkState readyToWorkState); + + /// @brief Returns the reported implement ready to work state + /// @return The reported implement ready to work state + ImplementReadyToWorkState get_implement_ready_to_work_state() const; + + /// @brief Sets the reported implement park state + /// @param[in] parkState The reported implement park state to set + /// @returns True if the set value was different from the stored value, otherwise false. + bool set_implement_park_state(ImplementParkState parkState); + + /// @brief Returns the reported implement park state + /// @returns The reported implement park state + ImplementParkState get_implement_park_state() const; + + /// @brief Sets the reported implement transport state + /// @param[in] transportState The reported implement transport state to set + /// @returns True if the set value was different from the stored value, otherwise false. + bool set_implement_transport_state(ImplementTransportState transportState); + + /// @brief Returns the reported implement transport state + /// @returns The reported implement transport state + ImplementTransportState get_implement_transport_state() const; + + /// @brief Sets the reported maintain actuator power state + /// @param[in] maintainState The reported maintain actuator power state + /// @returns True if the set value was different from the stored value, otherwise false. + bool set_maintain_actuator_power(MaintainActuatorPower maintainState); + + /// @brief Returns the reported maintain actuator power state + /// @returns The reported maintain actuator power state + MaintainActuatorPower get_maintain_actuator_power() const; + + /// @brief Sets the reported maintain ECU power state + /// @param[in] maintainState The reported maintain ECU power state + /// @returns True if the set value was different from the stored value, otherwise false. + bool set_maintain_ecu_power(MaintainECUPower maintainState); + + /// @brief Returns the reported maintain ECU power state + /// @returns The reported maintain ECU power state + MaintainECUPower get_maintain_ecu_power() const; + + /// @brief Returns a pointer to the sender of the message. If an ICF is the sender, returns the ICF being used to transmit from. + /// @attention The only way you could get an invalid pointer here is if you register a partner, it sends this message, then you delete the partner and + /// call this function, as that is the only time the stack deletes a control function. That would be abnormal program flow, but at some point + /// the stack will be updated to return a shared or weak pointer instead, but for now please be aware of that limitation. + /// Eventually though the message will time-out normally and you can get a new pointer for + /// the external CF that replaces the deleted partner. + /// @returns The control function sending this instance of the guidance system command message + std::shared_ptr get_sender_control_function() const; + + /// @brief Sets the timestamp for when the message was received or sent + /// @param[in] timestamp The timestamp, in milliseconds, when the message was sent or received + void set_timestamp_ms(std::uint32_t timestamp); + + /// @brief Returns the timestamp for when the message was received, in milliseconds + /// @returns The timestamp for when the message was received, in milliseconds + std::uint32_t get_timestamp_ms() const; + + private: + MaintainPowerData() = delete; + std::shared_ptr sendingControlFunction = nullptr; ///< The control function that is sending the message. + std::uint32_t timestamp_ms = 0; ///< A timestamp for when the message was released in milliseconds + ImplementInWorkState currentImplementInWorkState = ImplementInWorkState::NotAvailable; ///< The reported implement in-work state + ImplementReadyToWorkState currentImplementReadyToWorkState = ImplementReadyToWorkState::NotAvailable; ///< The reported implement ready to work state + ImplementParkState currentImplementParkState = ImplementParkState::NotAvailable; ///< The reported implement park state + ImplementTransportState currentImplementTransportState = ImplementTransportState::NotAvailable; ///< The reported transport state of the implement + MaintainActuatorPower currentMaintainActuatorPowerState = MaintainActuatorPower::DontCare; ///< The reported state for maintaining actuator power for 2 more seconds + MaintainECUPower currentMaintainECUPowerState = MaintainECUPower::DontCare; ///< The reported state for maintaining ECU power for 2 more seconds + }; + + /// @brief Constructor for a MaintainPowerInterface + /// @param[in] sourceControlFunction The control function to send the message from, or nullptr to listen only + explicit MaintainPowerInterface(std::shared_ptr sourceControlFunction); + + /// @brief Destructor for a MaintainPowerInterface + ~MaintainPowerInterface(); + + /// @brief Sets up the class and registers it to receive callbacks from the network manager for processing + /// messages. The class will not receive messages if this is not called. + void initialize(); + + /// @brief Returns if the interface has been initialized + /// @returns true if initialize has been called for this interface, otherwise false + bool get_initialized() const; + + /// @brief Use this to tell the interface how long it should transmit the maintain power message + /// after it detects a key state transition to off. The interface will use whatever you have set in + /// maintainPowerTransmitData when performing automatic transmission of the message. + /// @attention The interface will always send the message at least once with what you have configured + /// in maintainPowerTransmitData if it was set up with an internal control function, but you should + /// take care to configure maintainPowerTransmitData with the parameters that will ensure you have enough time + /// to safely stop your section control and shutdown your application, because when we stop sending this message + /// the TECU may kill power to your device or the actuators without warning. + /// @param[in] timeToMaintainPower The amount of time in milliseconds that the interface will send the maintain + /// power message when the key transitions to off. + void set_maintain_power_time(std::uint32_t timeToMaintainPower); + + /// @brief Returns the amount of time that the interface will continue to send the maintain + /// power message after it detects a key transition to off. + /// @returns The amount of time in milliseconds that the interface will send the maintain power message for. + std::uint32_t get_maintain_power_time() const; + + /// @brief Returns the number of unique senders of the maintain power message + /// @returns The number of unique senders of the maintain power message + std::size_t get_number_received_maintain_power_sources() const; + + /// @brief Returns the content of a received maintain power message + /// based on the index of the sender. Use this to read the received messages' content. + /// @param[in] index An index of senders of the maintain power message + std::shared_ptr get_received_maintain_power(std::size_t index); + + /// @brief Returns an event dispatcher which you can use to get callbacks when new/updated maintain power messages are received. + /// @returns The event publisher for maintain power messages + EventDispatcher, bool> &get_maintain_power_data_event_publisher(); + + /// @brief Returns an event dispatcher which you can use to get callbacks when the key switch transitions from + /// the not-off state to the off state. When you get this callback, you can then shutdown your application safely. + /// @note You can get more comprehensive key switch events by using the wheel-selected speed events in the + /// SpeedMessagesInterface file. + /// @returns The event publisher for key switch off transitions + EventDispatcher<> &get_key_switch_transition_off_event_publisher(); + + /// @brief Use this to configure the transmission of the maintain power message. + MaintainPowerData maintainPowerTransmitData; + + /// @brief Call this cyclically to update the interface. Transmits messages if needed and processes + /// timeouts for received messages. + void update(); + + protected: + /// @brief Transmits the maintain power message + /// @returns True if the message was sent, otherwise false + bool send_maintain_power() const; + + /// @brief Processes one flag (which sends the associated message) + /// @param[in] flag The flag to process + /// @param[in] parentPointer A pointer to the interface instance + static void process_flags(std::uint32_t flag, void *parentPointer); + + /// @brief Processes a CAN message + /// @param[in] message The CAN message being received + /// @param[in] parentPointer A context variable to find the relevant instance of this class + static void process_rx_message(const CANMessage &message, void *parentPointer); + + ProcessingFlags txFlags; ///< Tx flag for sending the maintain power message. Handles retries automatically. + + private: + /// @brief Enumerates a set of flags to manage transmitting messages owned by this interface + enum class TransmitFlags : std::uint32_t + { + SendMaintainPower = 0, ///< A flag to manage sending the maintain power message + + NumberOfFlags ///< The number of flags in this enumeration + }; + + /// @brief Enumerates the key switch states of the tractor or power unit. + enum class KeySwitchState : std::uint8_t + { + Off = 0, ///< Key is off + NotOff = 1, ///< Key is not off (does not always mean that it's on!) + Error = 2, + NotAvailable = 3 + }; + + static constexpr std::uint32_t MAINTAIN_POWER_TIMEOUT_MS = 2000; ///< The amount of time that power can be maintained per message, used as the timeout as well + std::vector> receivedMaintainPowerMessages; ///< A list of all received maintain power messages + EventDispatcher, bool> maintainPowerDataEventPublisher; ///< An event publisher for notifying when new maintain power messages are received + EventDispatcher<> keySwitchOffEventPublisher; ///< An event publisher for notifying when the key switch transitions to the off state + std::uint32_t keyNotOffTimestamp = 0; ///< A timestamp to track when the key was detected as ON, used to detect transitions to "Not On". + std::uint32_t keyOffTimestamp = 0; ///< A timestamp to track when the key is off, used to calculate how many messages to send and when to send them. + std::uint32_t maintainPowerTransmitTimestamp_ms = 0; ///< Timestamp used to know when to transmit the maintain power message in milliseconds + std::uint32_t maintainPowerTime_ms = 0; ///< The amount of time to ask the TECU to maintain actuator/section power. Will be rounded up to the next 2s mark when sent. + bool initialized = false; ///< Stores if the interface has been initialized + }; +} // namespace isobus +#endif // ISOBUS_MAINTAIN_POWER_INTERFACE_HPP diff --git a/src/isobus_shortcut_button_interface.cpp b/src/isobus_shortcut_button_interface.cpp new file mode 100644 index 0000000..511b367 --- /dev/null +++ b/src/isobus_shortcut_button_interface.cpp @@ -0,0 +1,236 @@ +//================================================================================================ +/// @file isobus_shortcut_button_interface.cpp +/// +/// @brief Implements the interface for an ISOBUS shortcut button +/// @author Adrian Del Grosso +/// +/// @copyright 2023 Adrian Del Grosso +//================================================================================================ +#include "isobus_shortcut_button_interface.hpp" + +#include "can_constants.hpp" +#include "can_general_parameter_group_numbers.hpp" +#include "can_network_manager.hpp" +#include "can_stack_logger.hpp" +#include "system_timing.hpp" + +#include +#include + +namespace isobus +{ + ShortcutButtonInterface::ShortcutButtonInterface(std::shared_ptr internalControlFunction, bool serverEnabled) : + sourceControlFunction(internalControlFunction), + txFlags(static_cast(TransmitFlags::NumberOfFlags), process_flags, this), + actAsISBServer(serverEnabled) + { + assert(nullptr != sourceControlFunction); // You need an internal control function for the interface to function + } + + ShortcutButtonInterface::~ShortcutButtonInterface() + { + if (initialized) + { + CANNetworkManager::CANNetwork.remove_global_parameter_group_number_callback(static_cast(CANLibParameterGroupNumber::AllImplementsStopOperationsSwitchState), process_rx_message, this); + } + } + + void ShortcutButtonInterface::initialize() + { + if (!initialized) + { + CANNetworkManager::CANNetwork.add_global_parameter_group_number_callback(static_cast(CANLibParameterGroupNumber::AllImplementsStopOperationsSwitchState), + process_rx_message, + this); + initialized = true; + } + } + + bool ShortcutButtonInterface::get_is_initialized() const + { + return initialized; + } + + EventDispatcher &ShortcutButtonInterface::get_stop_all_implement_operations_state_event_dispatcher() + { + return ISBEventDispatcher; + } + + void ShortcutButtonInterface::set_stop_all_implement_operations_state(StopAllImplementOperationsState newState) + { + if (actAsISBServer) + { + if (newState != commandedState) + { + commandedState = newState; + + if (StopAllImplementOperationsState::StopImplementOperations == newState) + { + CANStackLogger::error("[ISB]: All implement operations must stop. (Triggered internally)"); + } + else + { + CANStackLogger::info("[ISB]: Internal ISB state is now permitted."); + } + txFlags.set_flag(static_cast(TransmitFlags::SendStopAllImplementOperationsSwitchState)); + } + } + else + { + CANStackLogger::error("[ISB]: You are attempting to set the internal ISB state but the ISB interface is not configured as a server!"); + } + } + + ShortcutButtonInterface::StopAllImplementOperationsState ShortcutButtonInterface::get_state() const + { + StopAllImplementOperationsState retVal = StopAllImplementOperationsState::PermitAllImplementsToOperationOn; + + if (StopAllImplementOperationsState::StopImplementOperations == commandedState) + { + retVal = commandedState; + } + else + { + for (auto ISB = isobusShorcutButtonList.cbegin(); ISB != isobusShorcutButtonList.end(); ISB++) + { + if (StopAllImplementOperationsState::StopImplementOperations == ISB->commandedState) + { + retVal = ISB->commandedState; // Any stop condition will be returned. + break; + } + } + } + return retVal; + } + + void ShortcutButtonInterface::update() + { + if (SystemTiming::time_expired_ms(allImplementsStopOperationsSwitchStateTimestamp_ms, TRANSMISSION_RATE_MS)) + { + // Prune old ISBs + isobusShorcutButtonList.erase(std::remove_if(isobusShorcutButtonList.begin(), isobusShorcutButtonList.end(), [](ISBServerData &isb) { + return SystemTiming::time_expired_ms(isb.messageReceivedTimestamp_ms, TRANSMISSION_TIMEOUT_MS); + }), + isobusShorcutButtonList.end()); + + txFlags.set_flag(static_cast(TransmitFlags::SendStopAllImplementOperationsSwitchState)); + } + txFlags.process_all_flags(); + } + + void ShortcutButtonInterface::process_rx_message(const CANMessage &message, void *parentPointer) + { + assert(nullptr != parentPointer); + + static_cast(parentPointer)->process_message(message); + } + + void ShortcutButtonInterface::process_flags(std::uint32_t flag, void *parent) + { + auto myInterface = static_cast(parent); + bool transmitSuccessful = true; + assert(nullptr != parent); + + if (flag == static_cast(TransmitFlags::SendStopAllImplementOperationsSwitchState)) + { + transmitSuccessful = myInterface->send_stop_all_implement_operations_switch_state(); + + if (transmitSuccessful) + { + myInterface->stopAllImplementOperationsTransitionNumber++; + } + } + + if (!transmitSuccessful) + { + myInterface->txFlags.set_flag(flag); + } + } + + void ShortcutButtonInterface::process_message(const CANMessage &message) + { + if ((message.get_can_port_index() == sourceControlFunction->get_can_port()) && + (static_cast(CANLibParameterGroupNumber::AllImplementsStopOperationsSwitchState) == message.get_identifier().get_parameter_group_number())) + { + if (CAN_DATA_LENGTH == message.get_data_length()) + { + auto messageNAME = message.get_source_control_function()->get_NAME(); + auto matches_isoname = [messageNAME](ISBServerData &isb) { return isb.ISONAME == messageNAME; }; + auto ISB = std::find_if(isobusShorcutButtonList.begin(), isobusShorcutButtonList.end(), matches_isoname); + auto &messageData = message.get_data(); + StopAllImplementOperationsState previousState = get_state(); + + if (isobusShorcutButtonList.end() == ISB) + { + ISBServerData newISB; + + CANStackLogger::debug("[ISB]: New ISB detected at address %u", message.get_identifier().get_source_address()); + newISB.ISONAME = messageNAME; + isobusShorcutButtonList.emplace_back(newISB); + ISB = std::prev(isobusShorcutButtonList.end()); + } + + if (isobusShorcutButtonList.end() != ISB) + { + std::uint8_t newTransitionCount = messageData.at(6); + + if (((ISB->stopAllImplementOperationsTransitionNumber == 255) && + (0 != newTransitionCount)) || + ((ISB->stopAllImplementOperationsTransitionNumber < 255) && + (newTransitionCount > ISB->stopAllImplementOperationsTransitionNumber + 1))) + { + // A Working Set shall consider an increase in the transitions without detecting a corresponding + // transition of the Stop all implement operations state as an error and react accordingly. + ISB->commandedState = StopAllImplementOperationsState::StopImplementOperations; + CANStackLogger::error("[ISB]: Missed an ISB transition from ISB at address %u", message.get_identifier().get_source_address()); + } + else + { + ISB->commandedState = static_cast(messageData.at(7) & 0x03); + } + ISB->messageReceivedTimestamp_ms = SystemTiming::get_timestamp_ms(); + ISB->stopAllImplementOperationsTransitionNumber = messageData.at(6); + + auto newState = get_state(); + if (previousState != newState) + { + if (StopAllImplementOperationsState::StopImplementOperations == newState) + { + CANStackLogger::error("[ISB]: All implement operations must stop. (ISB at address %u has commanded it)", message.get_identifier().get_source_address()); + } + else + { + CANStackLogger::info("[ISB]: Implement operations now permitted."); + } + ISBEventDispatcher.call(newState); + } + } + } + else + { + CANStackLogger::warn("[ISB]: Received malformed All Implements Stop Operations Switch State. DLC must be 8."); + } + } + } + + bool ShortcutButtonInterface::send_stop_all_implement_operations_switch_state() const + { + std::array buffer = { + 0xFF, + 0xFF, + 0xFF, + 0xFF, + 0xFF, + 0xFF, + stopAllImplementOperationsTransitionNumber, + static_cast(0xFC | static_cast(commandedState)) + }; + + return CANNetworkManager::CANNetwork.send_can_message(static_cast(CANLibParameterGroupNumber::AllImplementsStopOperationsSwitchState), + buffer.data(), + buffer.size(), + sourceControlFunction, + nullptr, + CANIdentifier::Priority3); + } +} diff --git a/src/isobus_shortcut_button_interface.hpp b/src/isobus_shortcut_button_interface.hpp new file mode 100644 index 0000000..6f090ca --- /dev/null +++ b/src/isobus_shortcut_button_interface.hpp @@ -0,0 +1,164 @@ +//================================================================================================ +/// @file isobus_shortcut_button_interface.hpp +/// +/// @brief Defines an interface for communicating as or from an ISOBUS shortcut button (ISB). +/// Defined in AEF Guideline 004 - ISB and at https://www.isobus.net/isobus/pGNAndSPN/10936 +/// (ISO 11783-7) +/// +/// @details This interfaces manages the PGN used by isobus shortcut buttons (ISB). +/// You can choose to either receive this message, send it, or both. An ISB is essentially +/// a command to all implements to enter a safe state. See the description located at +/// https://www.isobus.net/isobus/pGNAndSPN/10936, ISO 11783-7, or +/// https://www.aef-online.org/fileadmin/user_upload/Content/pdfs/AEF_One_Pager.pdf +/// for more details. +/// +/// @attention If you consume this message, you MUST implement an associated alarm in your +/// VT/UT object pool, along with an icon or other indication on your home screen that your +/// working set master supports ISB, as required for AEF conformance. +/// +/// @author Adrian Del Grosso +/// +/// @copyright 2023 Adrian Del Grosso +//================================================================================================ +#ifndef ISOBUS_SHORTCUT_BUTTON_INTERFACE_HPP +#define ISOBUS_SHORTCUT_BUTTON_INTERFACE_HPP + +#include "can_NAME.hpp" +#include "can_internal_control_function.hpp" +#include "can_message.hpp" +#include "can_protocol.hpp" +#include "event_dispatcher.hpp" +#include "processing_flags.hpp" + +#include +#include +#include +#include + +namespace isobus +{ + /// @brief An interface for communicating as or interpreting the messages of ISOBUS Shortcut Buttons + /// @note This interface must be cyclically updated from your application since it's an application layer + /// message. Be sure to call "update" from time to time. Suggested rate is at least every 500ms, but ideally every 100ms or faster. + /// @details This interface parses the "All implements stop operations switch state" message + /// that is sent by ISOBUS shortcut buttons, and also allows you to optionally transmit the same message + /// as an ISOBUS shortcut button. + /// + /// This message may be sent by any control function connected to the implement bus on forestry or + /// agriculture implements providing to connected systems the current state of the all implement stop operations switch. + /// At least one of these switches shall be in each operator location of the connected system. + /// + /// All implements shall start a process to stop all operations when this broadcast message is received from any CF + /// with a value of "Stop implement operations" (SPN 5140). Before an implement turns off all implement operations, + /// it shall assume a failsafe condition. If an implement is operating in an automation mode, + /// it may enter a failsafe condition before requesting the tractor ECU to exit the automation mode, + /// e.g. PTO, Auxiliary valve, and/or tractor movement. + /// + /// The working set master for the implement shall then inform the operator that the implement has stopped + /// all operations due to the activation of the Stop All Implement Operations switch. + /// Implement working set masters shall include, on their home screen, an indication, + /// e.g. icon or a function name, if it supports Stop All Implement Operations. + /// The Working Set shall monitor the number of transitions for each ISB server upon receiving first + /// the message from a given ISB server. A Working Set shall consider an increase in the transitions + /// without detecting a corresponding transition of the Stop all implement operations state as an error and react accordingly. + class ShortcutButtonInterface + { + public: + /// @brief Enumerates the states that can be sent in the main ISB message (pgn 64770, 0xFD02) + enum class StopAllImplementOperationsState : std::uint8_t + { + StopImplementOperations = 0, ///< Stop implement operations + PermitAllImplementsToOperationOn = 1, ///< Permit all implements to operation ON + Error = 2, ///< Error indication + NotAvailable = 3 ///< Not available + }; + + /// @brief Constructor for a ShortcutButtonInterface + /// @param[in] internalControlFunction The InternalControlFunction that the interface will use to send messages (not nullptr) + /// @param[in] serverEnabled Enables the interface's transmission of the "Stop all implement operations" message. + ShortcutButtonInterface(std::shared_ptr internalControlFunction, bool serverEnabled = false); + + /// @brief Destructor for a ShortcutButtonInterface + virtual ~ShortcutButtonInterface(); + + /// @brief Used to initialize the interface. Registers for PGNs with the network manager. + void initialize(); + + /// @brief Returns if the interface has been initialized + /// @returns true if the interface has been initialized + bool get_is_initialized() const; + + /// @brief Gets the event dispatcher for when the assigned bus' ISB state changes. + /// The assigned bus is determined by which internal control function you pass into the constructor. + /// @returns The event dispatcher which can be used to register callbacks/listeners to + EventDispatcher &get_stop_all_implement_operations_state_event_dispatcher(); + + /// @brief Sets the state that the interface will broadcast on the bus. + /// @note This is only used when the interface was created as a server. + /// @param[in] newState The state to broadcast on the bus if the interface is a server + void set_stop_all_implement_operations_state(StopAllImplementOperationsState newState); + + /// @brief Returns the current ISB state for the bus, which is a combination of the internal commanded + /// state and the states reported by all other CFs. + /// @returns The current ISB state for the bus associated with the interface's internal control function + StopAllImplementOperationsState get_state() const; + + /// @brief This must be called cyclically to update the interface. + /// This processes transmits and timeouts. + void update(); + + private: + /// @brief Stores data about a sender of the stop all implement operations switch state + class ISBServerData + { + public: + /// @brief Constructor for ISBServerData, sets default values + ISBServerData() = default; + + NAME ISONAME; ///< The ISONAME of the sender, used as a lookup key + StopAllImplementOperationsState commandedState; ///< The last state we received from this ISB + std::uint32_t messageReceivedTimestamp_ms = 0; ///< Tracks the last time we received a message from this ISB so we can time them out if needed + std::uint8_t stopAllImplementOperationsTransitionNumber = 0; ///< Number of transitions from Permit (01) to Stop (00) since power up of the stop all implement operations parameter + }; + + /// @brief Enumerates a set of flags that the interface uses to know if it should transmit a message + enum class TransmitFlags : std::uint32_t + { + SendStopAllImplementOperationsSwitchState = 0, ///< A flag to send the main ISB message + + NumberOfFlags ///< The number of flags defined in this enumeration + }; + + /// @brief Parses incoming CAN messages for the interface + /// @param message The CAN message to parse + /// @param parentPointer A generic context variable, usually the `this` pointer for this interface instance + static void process_rx_message(const CANMessage &message, void *parentPointer); + + /// @brief Processes the internal Tx flags + /// @param[in] flag The flag to process + /// @param[in] parent A context variable to find the relevant interface class + static void process_flags(std::uint32_t flag, void *parent); + + /// @brief A generic way for a protocol to process a received message + /// @param[in] message A received CAN message + void process_message(const CANMessage &message); + + /// @brief Sends the Stop all implement operations switch state message + /// @returns true if the message was sent, otherwise false + bool send_stop_all_implement_operations_switch_state() const; + + static constexpr std::uint32_t TRANSMISSION_RATE_MS = 1000; ///< The cyclic transmission time for PGN 0xFD02 + static constexpr std::uint32_t TRANSMISSION_TIMEOUT_MS = 3000; ///< Amount of time between messages until we consider an ISB stale (arbitrary, but similar to VT timeout) + + std::list isobusShorcutButtonList; ///< A list of all senders of the ISB messages used to track transition counts + std::shared_ptr sourceControlFunction = nullptr; ///< The internal control function that the interface is assigned to and will use to transmit + EventDispatcher ISBEventDispatcher; ///< Manages callbacks about ISB states + ProcessingFlags txFlags; ///< A set of flags to manage retries while sending messages + std::uint32_t allImplementsStopOperationsSwitchStateTimestamp_ms = 0; ///< A timestamp to track the need for cyclic transmission of PGN 0xFD02 + std::uint8_t stopAllImplementOperationsTransitionNumber = 0; ///< A counter used to track our transitions from "stop" to "permit" when acting as a server + StopAllImplementOperationsState commandedState = StopAllImplementOperationsState::NotAvailable; ///< The state set by the user to transmit if we're acting as a server + bool actAsISBServer = false; ///< A setting that enables sending the ISB messages rather than just receiving them + bool initialized = false; ///< Stores if the interface has been initialized + }; +} // namespace isobus +#endif // ISOBUS_SHORTCUT_BUTTON_INTERFACE_HPP diff --git a/src/isobus_speed_distance_messages.cpp b/src/isobus_speed_distance_messages.cpp new file mode 100644 index 0000000..2c32bc5 --- /dev/null +++ b/src/isobus_speed_distance_messages.cpp @@ -0,0 +1,896 @@ +//================================================================================================ +/// @file isobus_speed_distance_messages.cpp +/// +/// @brief Implements an interface for sending and receiving ISOBUS speed/distance messages. +/// These messages are used to receive or transmit data about how fast the machine is going. +/// You can also use the machine selected speed command to command a machine to drive at a +/// desired speed. +/// +/// @attention Please use extreme care if you try to control the speed of a machine +/// with this interface! Remember that this library is licensed under The MIT License, +/// and that by obtaining a copy of this library and of course by attempting to +/// control a machine with it, you are agreeing to our license. +/// +/// @note Generally you will want to use the machine selected speed rather than the other +/// speeds, as the TECU chooses its favorite speed and reports it in that message. +/// +/// @author Adrian Del Grosso +/// +/// @copyright 2023 Adrian Del Grosso +//================================================================================================ +#include "isobus_speed_distance_messages.hpp" +#include "can_general_parameter_group_numbers.hpp" +#include "can_network_manager.hpp" +#include "can_stack_logger.hpp" +#include "system_timing.hpp" + +#include + +namespace isobus +{ + SpeedMessagesInterface::SpeedMessagesInterface(std::shared_ptr source, + bool enableSendingGroundBasedSpeedPeriodically, + bool enableSendingWheelBasedSpeedPeriodically, + bool enableSendingMachineSelectedSpeedPeriodically, + bool enableSendingMachineSelectedSpeedCommandPeriodically) : + machineSelectedSpeedTransmitData(MachineSelectedSpeedData(enableSendingMachineSelectedSpeedPeriodically ? source : nullptr)), + wheelBasedSpeedTransmitData(WheelBasedMachineSpeedData(enableSendingWheelBasedSpeedPeriodically ? source : nullptr)), + groundBasedSpeedTransmitData(GroundBasedSpeedData(enableSendingGroundBasedSpeedPeriodically ? source : nullptr)), + machineSelectedSpeedCommandTransmitData(MachineSelectedSpeedCommandData(enableSendingMachineSelectedSpeedCommandPeriodically ? source : nullptr)), + txFlags(static_cast(TransmitFlags::NumberOfFlags), process_flags, this) + { + } + + SpeedMessagesInterface::~SpeedMessagesInterface() + { + if (initialized) + { + CANNetworkManager::CANNetwork.remove_any_control_function_parameter_group_number_callback(static_cast(CANLibParameterGroupNumber::MachineSelectedSpeed), process_rx_message, this); + CANNetworkManager::CANNetwork.remove_any_control_function_parameter_group_number_callback(static_cast(CANLibParameterGroupNumber::WheelBasedSpeedAndDistance), process_rx_message, this); + CANNetworkManager::CANNetwork.remove_any_control_function_parameter_group_number_callback(static_cast(CANLibParameterGroupNumber::GroundBasedSpeedAndDistance), process_rx_message, this); + CANNetworkManager::CANNetwork.remove_any_control_function_parameter_group_number_callback(static_cast(CANLibParameterGroupNumber::MachineSelectedSpeedCommand), process_rx_message, this); + } + } + + SpeedMessagesInterface::WheelBasedMachineSpeedData::WheelBasedMachineSpeedData(std::shared_ptr sender) : + controlFunction(sender) + { + } + + std::uint32_t SpeedMessagesInterface::WheelBasedMachineSpeedData::get_machine_distance() const + { + std::uint32_t retVal = wheelBasedMachineDistance_mm; + + // Values above the max are sort of implicitly defined and should be ignored. + if (wheelBasedMachineDistance_mm > SAEds05_MAX_VALUE) + { + retVal = 0; + } + return retVal; + } + + bool SpeedMessagesInterface::WheelBasedMachineSpeedData::set_machine_distance(std::uint32_t distance) + { + bool retVal = (distance != wheelBasedMachineDistance_mm); + wheelBasedMachineDistance_mm = distance; + return retVal; + } + + std::uint16_t SpeedMessagesInterface::WheelBasedMachineSpeedData::get_machine_speed() const + { + std::uint16_t retVal = wheelBasedMachineSpeed_mm_per_sec; + + if (wheelBasedMachineSpeed_mm_per_sec > SAEvl01_MAX_VALUE) + { + retVal = 0; + } + return retVal; + } + + bool SpeedMessagesInterface::WheelBasedMachineSpeedData::set_machine_speed(std::uint16_t speed) + { + bool retVal = (speed != wheelBasedMachineSpeed_mm_per_sec); + wheelBasedMachineSpeed_mm_per_sec = speed; + return retVal; + } + + std::uint8_t SpeedMessagesInterface::WheelBasedMachineSpeedData::get_maximum_time_of_tractor_power() const + { + return maximumTimeOfTractorPower_min; + } + + bool SpeedMessagesInterface::WheelBasedMachineSpeedData::set_maximum_time_of_tractor_power(std::uint8_t maxTime) + { + bool retVal = (maximumTimeOfTractorPower_min != maxTime); + maximumTimeOfTractorPower_min = maxTime; + return retVal; + } + + SpeedMessagesInterface::MachineDirection SpeedMessagesInterface::WheelBasedMachineSpeedData::get_machine_direction_of_travel() const + { + return machineDirectionState; + } + + bool SpeedMessagesInterface::WheelBasedMachineSpeedData::set_machine_direction_of_travel(MachineDirection direction) + { + bool retVal = (machineDirectionState != direction); + machineDirectionState = direction; + return retVal; + } + + SpeedMessagesInterface::WheelBasedMachineSpeedData::KeySwitchState SpeedMessagesInterface::WheelBasedMachineSpeedData::get_key_switch_state() const + { + return keySwitchState; + } + + bool SpeedMessagesInterface::WheelBasedMachineSpeedData::set_key_switch_state(KeySwitchState state) + { + bool retVal = (keySwitchState != state); + keySwitchState = state; + return retVal; + } + + SpeedMessagesInterface::WheelBasedMachineSpeedData::ImplementStartStopOperations SpeedMessagesInterface::WheelBasedMachineSpeedData::get_implement_start_stop_operations_state() const + { + return implementStartStopOperationsState; + } + + bool SpeedMessagesInterface::WheelBasedMachineSpeedData::set_implement_start_stop_operations_state(ImplementStartStopOperations state) + { + bool retVal = (implementStartStopOperationsState != state); + implementStartStopOperationsState = state; + return retVal; + } + + SpeedMessagesInterface::WheelBasedMachineSpeedData::OperatorDirectionReversed SpeedMessagesInterface::WheelBasedMachineSpeedData::get_operator_direction_reversed_state() const + { + return operatorDirectionReversedState; + } + + bool SpeedMessagesInterface::WheelBasedMachineSpeedData::set_operator_direction_reversed_state(OperatorDirectionReversed reverseState) + { + bool retVal = (operatorDirectionReversedState != reverseState); + operatorDirectionReversedState = reverseState; + return retVal; + } + + std::shared_ptr SpeedMessagesInterface::WheelBasedMachineSpeedData::get_sender_control_function() const + { + return controlFunction; + } + + void SpeedMessagesInterface::WheelBasedMachineSpeedData::set_timestamp_ms(std::uint32_t timestamp) + { + timestamp_ms = timestamp; + } + + std::uint32_t SpeedMessagesInterface::WheelBasedMachineSpeedData::get_timestamp_ms() const + { + return timestamp_ms; + } + + SpeedMessagesInterface::MachineSelectedSpeedData::MachineSelectedSpeedData(std::shared_ptr sender) : + controlFunction(sender) + { + } + + std::uint32_t SpeedMessagesInterface::MachineSelectedSpeedData::get_machine_distance() const + { + std::uint32_t retVal = machineSelectedSpeedDistance_mm; + + // Values above the max are sort of implicitly defined and should be ignored. + if (machineSelectedSpeedDistance_mm > SAEds05_MAX_VALUE) + { + retVal = 0; + } + return retVal; + } + + bool SpeedMessagesInterface::MachineSelectedSpeedData::set_machine_distance(std::uint32_t distance) + { + bool retVal = (machineSelectedSpeedDistance_mm != distance); + machineSelectedSpeedDistance_mm = distance; + return retVal; + } + + std::uint16_t SpeedMessagesInterface::MachineSelectedSpeedData::get_machine_speed() const + { + std::uint16_t retVal = machineSelectedSpeed_mm_per_sec; + + if (machineSelectedSpeed_mm_per_sec > SAEvl01_MAX_VALUE) + { + retVal = 0; + } + return retVal; + } + + bool SpeedMessagesInterface::MachineSelectedSpeedData::set_machine_speed(std::uint16_t speed) + { + bool retVal = (speed != machineSelectedSpeed_mm_per_sec); + machineSelectedSpeed_mm_per_sec = speed; + return retVal; + } + + std::uint8_t SpeedMessagesInterface::MachineSelectedSpeedData::get_exit_reason_code() const + { + return exitReasonCode; + } + + bool SpeedMessagesInterface::MachineSelectedSpeedData::set_exit_reason_code(std::uint8_t exitCode) + { + bool retVal = (exitCode != exitReasonCode); + exitReasonCode = exitCode; + return retVal; + } + + SpeedMessagesInterface::MachineSelectedSpeedData::SpeedSource SpeedMessagesInterface::MachineSelectedSpeedData::get_speed_source() const + { + return source; + } + + bool SpeedMessagesInterface::MachineSelectedSpeedData::set_speed_source(SpeedSource selectedSource) + { + bool retVal = (source != selectedSource); + source = selectedSource; + return retVal; + } + + SpeedMessagesInterface::MachineSelectedSpeedData::LimitStatus SpeedMessagesInterface::MachineSelectedSpeedData::get_limit_status() const + { + return limitStatus; + } + + bool SpeedMessagesInterface::MachineSelectedSpeedData::set_limit_status(LimitStatus statusToSet) + { + bool retVal = (limitStatus != statusToSet); + limitStatus = statusToSet; + return retVal; + } + + SpeedMessagesInterface::MachineDirection SpeedMessagesInterface::MachineSelectedSpeedData::get_machine_direction_of_travel() const + { + return machineDirectionState; + } + + bool SpeedMessagesInterface::MachineSelectedSpeedData::set_machine_direction_of_travel(MachineDirection directionOfTravel) + { + bool retVal = (directionOfTravel != machineDirectionState); + machineDirectionState = directionOfTravel; + return retVal; + } + + std::shared_ptr SpeedMessagesInterface::MachineSelectedSpeedData::get_sender_control_function() const + { + return controlFunction; + } + + void SpeedMessagesInterface::MachineSelectedSpeedData::set_timestamp_ms(std::uint32_t timestamp) + { + timestamp_ms = timestamp; + } + + std::uint32_t SpeedMessagesInterface::MachineSelectedSpeedData::get_timestamp_ms() const + { + return timestamp_ms; + } + + SpeedMessagesInterface::GroundBasedSpeedData::GroundBasedSpeedData(std::shared_ptr sender) : + controlFunction(sender) + { + } + + std::uint32_t SpeedMessagesInterface::GroundBasedSpeedData::get_machine_distance() const + { + std::uint32_t retVal = groundBasedMachineDistance_mm; + + // Values above the max are sort of implicitly defined and should be ignored. + if (groundBasedMachineDistance_mm > SAEds05_MAX_VALUE) + { + retVal = 0; + } + return retVal; + } + + bool SpeedMessagesInterface::GroundBasedSpeedData::set_machine_distance(std::uint32_t distance) + { + bool retVal = (distance != groundBasedMachineDistance_mm); + groundBasedMachineDistance_mm = distance; + return retVal; + } + + std::uint16_t SpeedMessagesInterface::GroundBasedSpeedData::get_machine_speed() const + { + std::uint16_t retVal = groundBasedMachineSpeed_mm_per_sec; + + if (groundBasedMachineSpeed_mm_per_sec > SAEvl01_MAX_VALUE) + { + retVal = 0; + } + return retVal; + } + + bool SpeedMessagesInterface::GroundBasedSpeedData::set_machine_speed(std::uint16_t speed) + { + bool retVal = (speed != groundBasedMachineSpeed_mm_per_sec); + groundBasedMachineSpeed_mm_per_sec = speed; + return retVal; + } + + SpeedMessagesInterface::MachineDirection SpeedMessagesInterface::GroundBasedSpeedData::get_machine_direction_of_travel() const + { + return machineDirectionState; + } + + bool SpeedMessagesInterface::GroundBasedSpeedData::set_machine_direction_of_travel(MachineDirection directionOfTravel) + { + bool retVal = (directionOfTravel != machineDirectionState); + machineDirectionState = directionOfTravel; + return retVal; + } + + std::shared_ptr SpeedMessagesInterface::GroundBasedSpeedData::get_sender_control_function() const + { + return controlFunction; + } + + void SpeedMessagesInterface::GroundBasedSpeedData::set_timestamp_ms(std::uint32_t timestamp) + { + timestamp_ms = timestamp; + } + + std::uint32_t SpeedMessagesInterface::GroundBasedSpeedData::get_timestamp_ms() const + { + return timestamp_ms; + } + + SpeedMessagesInterface::MachineSelectedSpeedCommandData::MachineSelectedSpeedCommandData(std::shared_ptr sender) : + controlFunction(sender) + { + } + + std::uint16_t SpeedMessagesInterface::MachineSelectedSpeedCommandData::get_machine_speed_setpoint_command() const + { + std::uint16_t retVal = speedCommandedSetpoint; + + // Values over the max are implicitly defined, best to treat as zeros. + if (speedCommandedSetpoint > SAEvl01_MAX_VALUE) + { + retVal = 0; + } + return retVal; + } + + bool SpeedMessagesInterface::MachineSelectedSpeedCommandData::set_machine_speed_setpoint_command(std::uint16_t speed) + { + bool retVal = (speed != speedCommandedSetpoint); + speedCommandedSetpoint = speed; + return retVal; + } + + std::uint16_t SpeedMessagesInterface::MachineSelectedSpeedCommandData::get_machine_selected_speed_setpoint_limit() const + { + std::uint16_t retVal = speedSetpointLimit; + + // Values over the max are implicitly defined, best to treat as zeros. + if (speedSetpointLimit > SAEvl01_MAX_VALUE) + { + retVal = 0; + } + return retVal; + } + + bool SpeedMessagesInterface::MachineSelectedSpeedCommandData::set_machine_selected_speed_setpoint_limit(std::uint16_t speedLimit) + { + bool retVal = (speedSetpointLimit != speedLimit); + speedSetpointLimit = speedLimit; + return retVal; + } + + SpeedMessagesInterface::MachineDirection SpeedMessagesInterface::MachineSelectedSpeedCommandData::get_machine_direction_command() const + { + return machineDirectionCommand; + } + + bool SpeedMessagesInterface::MachineSelectedSpeedCommandData::set_machine_direction_of_travel(MachineDirection commandedDirection) + { + bool retVal = (commandedDirection != machineDirectionCommand); + machineDirectionCommand = commandedDirection; + return retVal; + } + + std::shared_ptr SpeedMessagesInterface::MachineSelectedSpeedCommandData::get_sender_control_function() const + { + return controlFunction; + } + + void SpeedMessagesInterface::MachineSelectedSpeedCommandData::set_timestamp_ms(std::uint32_t timestamp) + { + timestamp_ms = timestamp; + } + + std::uint32_t SpeedMessagesInterface::MachineSelectedSpeedCommandData::get_timestamp_ms() const + { + return timestamp_ms; + } + + void SpeedMessagesInterface::initialize() + { + if (!initialized) + { + if (nullptr != machineSelectedSpeedCommandTransmitData.get_sender_control_function()) + { + CANStackLogger::warn("[Speed/Distance]: Use extreme cation! You have configured an interface to command the speed of the machine. The machine may move without warning!"); + } + CANNetworkManager::CANNetwork.add_any_control_function_parameter_group_number_callback(static_cast(CANLibParameterGroupNumber::MachineSelectedSpeed), process_rx_message, this); + CANNetworkManager::CANNetwork.add_any_control_function_parameter_group_number_callback(static_cast(CANLibParameterGroupNumber::WheelBasedSpeedAndDistance), process_rx_message, this); + CANNetworkManager::CANNetwork.add_any_control_function_parameter_group_number_callback(static_cast(CANLibParameterGroupNumber::GroundBasedSpeedAndDistance), process_rx_message, this); + CANNetworkManager::CANNetwork.add_any_control_function_parameter_group_number_callback(static_cast(CANLibParameterGroupNumber::MachineSelectedSpeedCommand), process_rx_message, this); + initialized = true; + } + } + + bool SpeedMessagesInterface::get_initialized() const + { + return initialized; + } + + std::size_t SpeedMessagesInterface::get_number_received_wheel_based_speed_sources() const + { + return receivedWheelBasedSpeedMessages.size(); + } + + std::size_t SpeedMessagesInterface::get_number_received_ground_based_speed_sources() const + { + return receivedGroundBasedSpeedMessages.size(); + } + + std::size_t SpeedMessagesInterface::get_number_received_machine_selected_speed_sources() const + { + return receivedMachineSelectedSpeedMessages.size(); + } + + std::size_t SpeedMessagesInterface::get_number_received_machine_selected_speed_command_sources() const + { + return receivedMachineSelectedSpeedCommandMessages.size(); + } + + std::shared_ptr SpeedMessagesInterface::get_received_machine_selected_speed(std::size_t index) + { + std::shared_ptr retVal = nullptr; + + if (index < receivedMachineSelectedSpeedMessages.size()) + { + retVal = receivedMachineSelectedSpeedMessages.at(index); + } + return retVal; + } + + std::shared_ptr SpeedMessagesInterface::get_received_wheel_based_speed(std::size_t index) + { + std::shared_ptr retVal = nullptr; + + if (index < receivedWheelBasedSpeedMessages.size()) + { + retVal = receivedWheelBasedSpeedMessages.at(index); + } + return retVal; + } + + std::shared_ptr SpeedMessagesInterface::get_received_ground_based_speed(std::size_t index) + { + std::shared_ptr retVal = nullptr; + + if (index < receivedGroundBasedSpeedMessages.size()) + { + retVal = receivedGroundBasedSpeedMessages.at(index); + } + return retVal; + } + + std::shared_ptr SpeedMessagesInterface::get_received_machine_selected_speed_command(std::size_t index) + { + std::shared_ptr retVal = nullptr; + + if (index < receivedMachineSelectedSpeedCommandMessages.size()) + { + retVal = receivedMachineSelectedSpeedCommandMessages.at(index); + } + return retVal; + } + + EventDispatcher, bool> &SpeedMessagesInterface::get_wheel_based_machine_speed_data_event_publisher() + { + return wheelBasedMachineSpeedDataEventPublisher; + } + + EventDispatcher, bool> &SpeedMessagesInterface::get_machine_selected_speed_data_event_publisher() + { + return machineSelectedSpeedDataEventPublisher; + } + + EventDispatcher, bool> &SpeedMessagesInterface::get_ground_based_machine_speed_data_event_publisher() + { + return groundBasedSpeedDataEventPublisher; + } + + EventDispatcher, bool> &SpeedMessagesInterface::get_machine_selected_speed_command_data_event_publisher() + { + return machineSelectedSpeedCommandDataEventPublisher; + } + + void SpeedMessagesInterface::update() + { + if (initialized) + { + receivedMachineSelectedSpeedMessages.erase(std::remove_if(receivedMachineSelectedSpeedMessages.begin(), + receivedMachineSelectedSpeedMessages.end(), + [](std::shared_ptr messageInfo) { + return SystemTiming::time_expired_ms(messageInfo->get_timestamp_ms(), SPEED_DISTANCE_MESSAGE_RX_TIMEOUT_MS); + }), + receivedMachineSelectedSpeedMessages.end()); + receivedWheelBasedSpeedMessages.erase(std::remove_if(receivedWheelBasedSpeedMessages.begin(), + receivedWheelBasedSpeedMessages.end(), + [](std::shared_ptr messageInfo) { + return SystemTiming::time_expired_ms(messageInfo->get_timestamp_ms(), SPEED_DISTANCE_MESSAGE_RX_TIMEOUT_MS); + }), + receivedWheelBasedSpeedMessages.end()); + receivedGroundBasedSpeedMessages.erase(std::remove_if(receivedGroundBasedSpeedMessages.begin(), + receivedGroundBasedSpeedMessages.end(), + [](std::shared_ptr messageInfo) { + return SystemTiming::time_expired_ms(messageInfo->get_timestamp_ms(), SPEED_DISTANCE_MESSAGE_RX_TIMEOUT_MS); + }), + receivedGroundBasedSpeedMessages.end()); + receivedMachineSelectedSpeedCommandMessages.erase(std::remove_if(receivedMachineSelectedSpeedCommandMessages.begin(), + receivedMachineSelectedSpeedCommandMessages.end(), + [](std::shared_ptr messageInfo) { + return SystemTiming::time_expired_ms(messageInfo->get_timestamp_ms(), SPEED_DISTANCE_MESSAGE_RX_TIMEOUT_MS); + }), + receivedMachineSelectedSpeedCommandMessages.end()); + + if (SystemTiming::time_expired_ms(machineSelectedSpeedTransmitTimestamp_ms, SPEED_DISTANCE_MESSAGE_TX_INTERVAL_MS) && + (nullptr != machineSelectedSpeedTransmitData.get_sender_control_function())) + { + txFlags.set_flag(static_cast(TransmitFlags::SendMachineSelectedSpeed)); + machineSelectedSpeedTransmitTimestamp_ms = SystemTiming::get_timestamp_ms(); + } + if (SystemTiming::time_expired_ms(wheelBasedSpeedTransmitTimestamp_ms, SPEED_DISTANCE_MESSAGE_TX_INTERVAL_MS) && + (nullptr != wheelBasedSpeedTransmitData.get_sender_control_function())) + { + txFlags.set_flag(static_cast(TransmitFlags::SendWheelBasedSpeed)); + wheelBasedSpeedTransmitTimestamp_ms = SystemTiming::get_timestamp_ms(); + } + if (SystemTiming::time_expired_ms(groundBasedSpeedTransmitTimestamp_ms, SPEED_DISTANCE_MESSAGE_TX_INTERVAL_MS) && + (nullptr != groundBasedSpeedTransmitData.get_sender_control_function())) + { + txFlags.set_flag(static_cast(TransmitFlags::SendGroundBasedSpeed)); + groundBasedSpeedTransmitTimestamp_ms = SystemTiming::get_timestamp_ms(); + } + if (SystemTiming::time_expired_ms(machineSelectedSpeedCommandTransmitTimestamp_ms, SPEED_DISTANCE_MESSAGE_TX_INTERVAL_MS) && + (nullptr != machineSelectedSpeedCommandTransmitData.get_sender_control_function())) + { + txFlags.set_flag(static_cast(TransmitFlags::SendMachineSelectedSpeedCommand)); + machineSelectedSpeedCommandTransmitTimestamp_ms = SystemTiming::get_timestamp_ms(); + } + txFlags.process_all_flags(); + } + else + { + CANStackLogger::error("[Speed/Distance]: ISOBUS speed messages interface has not been initialized yet."); + } + } + + void SpeedMessagesInterface::process_flags(std::uint32_t flag, void *parentPointer) + { + if (nullptr != parentPointer) + { + auto targetInterface = static_cast(parentPointer); + bool transmitSuccessful = false; + + switch (flag) + { + case static_cast(TransmitFlags::SendMachineSelectedSpeed): + { + transmitSuccessful = targetInterface->send_machine_selected_speed(); + } + break; + + case static_cast(TransmitFlags::SendWheelBasedSpeed): + { + transmitSuccessful = targetInterface->send_wheel_based_speed(); + } + break; + + case static_cast(TransmitFlags::SendGroundBasedSpeed): + { + transmitSuccessful = targetInterface->send_ground_based_speed(); + } + break; + + case static_cast(TransmitFlags::SendMachineSelectedSpeedCommand): + { + transmitSuccessful = targetInterface->send_machine_selected_speed_command(); + } + break; + + default: + break; + } + + if (false == transmitSuccessful) + { + targetInterface->txFlags.set_flag(flag); + } + } + } + + void SpeedMessagesInterface::process_rx_message(const CANMessage &message, void *parentPointer) + { + assert(nullptr != parentPointer); + auto targetInterface = static_cast(parentPointer); + + switch (message.get_identifier().get_parameter_group_number()) + { + case static_cast(CANLibParameterGroupNumber::MachineSelectedSpeed): + { + if (CAN_DATA_LENGTH == message.get_data_length()) + { + if (nullptr != message.get_source_control_function()) + { + auto result = std::find_if(targetInterface->receivedMachineSelectedSpeedMessages.cbegin(), + targetInterface->receivedMachineSelectedSpeedMessages.cend(), + [&message](const std::shared_ptr &receivedInfo) { + return (nullptr != receivedInfo) && (receivedInfo->get_sender_control_function() == message.get_source_control_function()); + }); + + if (result == targetInterface->receivedMachineSelectedSpeedMessages.end()) + { + // There is no existing message object from this control function, so create a new one + targetInterface->receivedMachineSelectedSpeedMessages.push_back(std::make_shared(message.get_source_control_function())); + result = targetInterface->receivedMachineSelectedSpeedMessages.end() - 1; + } + + auto &mssMessage = *result; + bool changed = false; + + changed |= mssMessage->set_machine_speed(message.get_uint16_at(0)); + changed |= mssMessage->set_machine_distance(message.get_uint32_at(2)); + changed |= mssMessage->set_exit_reason_code(message.get_uint8_at(6) & 0x3F); + changed |= mssMessage->set_machine_direction_of_travel(static_cast(message.get_uint8_at(7) & 0x03)); + changed |= mssMessage->set_speed_source(static_cast((message.get_uint8_at(7) >> 2) & 0x07)); + changed |= mssMessage->set_limit_status(static_cast((message.get_uint8_at(7) >> 5) & 0x03)); + mssMessage->set_timestamp_ms(SystemTiming::get_timestamp_ms()); + + targetInterface->machineSelectedSpeedDataEventPublisher.call(mssMessage, changed); + } + } + else + { + CANStackLogger::error("[Speed/Distance]: Received a malformed machine selected speed. DLC must be 8."); + } + } + break; + + case static_cast(CANLibParameterGroupNumber::WheelBasedSpeedAndDistance): + { + if (CAN_DATA_LENGTH == message.get_data_length()) + { + if (nullptr != message.get_source_control_function()) + { + auto result = std::find_if(targetInterface->receivedWheelBasedSpeedMessages.cbegin(), + targetInterface->receivedWheelBasedSpeedMessages.cend(), + [&message](const std::shared_ptr &receivedInfo) { + return (nullptr != receivedInfo) && (receivedInfo->get_sender_control_function() == message.get_source_control_function()); + }); + + if (result == targetInterface->receivedWheelBasedSpeedMessages.end()) + { + // There is no existing message object from this control function, so create a new one + targetInterface->receivedWheelBasedSpeedMessages.push_back(std::make_shared(message.get_source_control_function())); + result = targetInterface->receivedWheelBasedSpeedMessages.end() - 1; + } + + auto &wheelSpeedMessage = *result; + bool changed = false; + + changed |= wheelSpeedMessage->set_machine_speed(message.get_uint16_at(0)); + changed |= wheelSpeedMessage->set_machine_distance(message.get_uint32_at(2)); + changed |= wheelSpeedMessage->set_maximum_time_of_tractor_power(message.get_uint8_at(6)); + changed |= wheelSpeedMessage->set_machine_direction_of_travel(static_cast(message.get_uint8_at(7) & 0x03)); + changed |= wheelSpeedMessage->set_key_switch_state(static_cast((message.get_uint8_at(7) >> 2) & 0x03)); + changed |= wheelSpeedMessage->set_implement_start_stop_operations_state(static_cast((message.get_uint8_at(7) >> 4) & 0x03)); + changed |= wheelSpeedMessage->set_operator_direction_reversed_state(static_cast((message.get_uint8_at(7) >> 6) & 0x03)); + wheelSpeedMessage->set_timestamp_ms(SystemTiming::get_timestamp_ms()); + + targetInterface->wheelBasedMachineSpeedDataEventPublisher.call(wheelSpeedMessage, changed); + } + } + else + { + CANStackLogger::error("[Speed/Distance]: Received a malformed wheel-based speed and distance message. DLC must be 8."); + } + } + break; + + case static_cast(CANLibParameterGroupNumber::GroundBasedSpeedAndDistance): + { + if (CAN_DATA_LENGTH == message.get_data_length()) + { + if (nullptr != message.get_source_control_function()) + { + auto result = std::find_if(targetInterface->receivedGroundBasedSpeedMessages.cbegin(), + targetInterface->receivedGroundBasedSpeedMessages.cend(), + [&message](const std::shared_ptr &receivedInfo) { + return (nullptr != receivedInfo) && (receivedInfo->get_sender_control_function() == message.get_source_control_function()); + }); + + if (result == targetInterface->receivedGroundBasedSpeedMessages.end()) + { + // There is no existing message object from this control function, so create a new one + targetInterface->receivedGroundBasedSpeedMessages.push_back(std::make_shared(message.get_source_control_function())); + result = targetInterface->receivedGroundBasedSpeedMessages.end() - 1; + } + + auto &groundSpeedMessage = *result; + bool changed = false; + + changed |= groundSpeedMessage->set_machine_speed(message.get_uint16_at(0)); + changed |= groundSpeedMessage->set_machine_distance(message.get_uint32_at(2)); + changed |= groundSpeedMessage->set_machine_direction_of_travel(static_cast(message.get_uint8_at(7) & 0x03)); + groundSpeedMessage->set_timestamp_ms(SystemTiming::get_timestamp_ms()); + + targetInterface->groundBasedSpeedDataEventPublisher.call(groundSpeedMessage, changed); + } + } + else + { + CANStackLogger::error("[Speed/Distance]: Received a malformed ground-based speed and distance message. DLC must be 8."); + } + } + break; + + case static_cast(CANLibParameterGroupNumber::MachineSelectedSpeedCommand): + { + if (CAN_DATA_LENGTH == message.get_data_length()) + { + if (nullptr != message.get_source_control_function()) + { + auto result = std::find_if(targetInterface->receivedMachineSelectedSpeedCommandMessages.cbegin(), + targetInterface->receivedMachineSelectedSpeedCommandMessages.cend(), + [&message](const std::shared_ptr &receivedInfo) { + return (nullptr != receivedInfo) && (receivedInfo->get_sender_control_function() == message.get_source_control_function()); + }); + + if (result == targetInterface->receivedMachineSelectedSpeedCommandMessages.end()) + { + // There is no existing message object from this control function, so create a new one + targetInterface->receivedMachineSelectedSpeedCommandMessages.push_back(std::make_shared(message.get_source_control_function())); + result = targetInterface->receivedMachineSelectedSpeedCommandMessages.end() - 1; + } + + auto &commandMessage = *result; + bool changed = false; + + commandMessage->set_machine_speed_setpoint_command(message.get_uint16_at(0)); + commandMessage->set_machine_selected_speed_setpoint_limit(message.get_uint16_at(2)); + commandMessage->set_machine_direction_of_travel(static_cast(message.get_uint8_at(7) & 0x03)); + commandMessage->set_timestamp_ms(SystemTiming::get_timestamp_ms()); + + targetInterface->machineSelectedSpeedCommandDataEventPublisher.call(commandMessage, changed); + } + } + else + { + CANStackLogger::error("[Speed/Distance]: Received a malformed machine selected speed command message. DLC must be 8."); + } + } + break; + + default: + break; + } + } + + bool SpeedMessagesInterface::send_machine_selected_speed() const + { + bool retVal = false; + + if (nullptr != machineSelectedSpeedTransmitData.get_sender_control_function()) + { + std::array buffer = { static_cast(machineSelectedSpeedTransmitData.get_machine_speed() & 0xFF), + static_cast((machineSelectedSpeedTransmitData.get_machine_speed() >> 8) & 0xFF), + static_cast(machineSelectedSpeedTransmitData.get_machine_distance() & 0xFF), + static_cast((machineSelectedSpeedTransmitData.get_machine_distance() >> 8) & 0xFF), + static_cast((machineSelectedSpeedTransmitData.get_machine_distance() >> 16) & 0xFF), + static_cast((machineSelectedSpeedTransmitData.get_machine_distance() >> 24) & 0xFF), + static_cast(0xC0 | static_cast(machineSelectedSpeedTransmitData.get_exit_reason_code() & 0x7F)), // 0xC0 sets reserved bits to 1s + static_cast(static_cast(machineSelectedSpeedTransmitData.get_machine_direction_of_travel()) | + (static_cast(machineSelectedSpeedTransmitData.get_speed_source()) << 2) | + (static_cast(machineSelectedSpeedTransmitData.get_limit_status()) << 5)) + + }; + retVal = CANNetworkManager::CANNetwork.send_can_message(static_cast(CANLibParameterGroupNumber::MachineSelectedSpeed), + buffer.data(), + buffer.size(), + std::static_pointer_cast(machineSelectedSpeedTransmitData.get_sender_control_function()), + nullptr, + CANIdentifier::Priority3); + } + return retVal; + } + + bool SpeedMessagesInterface::send_wheel_based_speed() const + { + bool retVal = false; + + if (nullptr != wheelBasedSpeedTransmitData.get_sender_control_function()) + { + std::array buffer = { static_cast(wheelBasedSpeedTransmitData.get_machine_speed() & 0xFF), + static_cast((wheelBasedSpeedTransmitData.get_machine_speed() >> 8) & 0xFF), + static_cast(wheelBasedSpeedTransmitData.get_machine_distance() & 0xFF), + static_cast((wheelBasedSpeedTransmitData.get_machine_distance() >> 8) & 0xFF), + static_cast((wheelBasedSpeedTransmitData.get_machine_distance() >> 16) & 0xFF), + static_cast((wheelBasedSpeedTransmitData.get_machine_distance() >> 24) & 0xFF), + wheelBasedSpeedTransmitData.get_maximum_time_of_tractor_power(), + static_cast(static_cast(wheelBasedSpeedTransmitData.get_machine_direction_of_travel()) | + (static_cast(wheelBasedSpeedTransmitData.get_key_switch_state()) << 2) | + (static_cast(wheelBasedSpeedTransmitData.get_implement_start_stop_operations_state()) << 4) | + (static_cast(wheelBasedSpeedTransmitData.get_operator_direction_reversed_state()) << 6)) }; + retVal = CANNetworkManager::CANNetwork.send_can_message(static_cast(CANLibParameterGroupNumber::WheelBasedSpeedAndDistance), + buffer.data(), + buffer.size(), + std::static_pointer_cast(wheelBasedSpeedTransmitData.get_sender_control_function()), + nullptr, + CANIdentifier::Priority3); + } + return retVal; + } + + bool SpeedMessagesInterface::send_ground_based_speed() const + { + bool retVal = false; + + if (nullptr != groundBasedSpeedTransmitData.get_sender_control_function()) + { + std::array buffer = { static_cast(groundBasedSpeedTransmitData.get_machine_speed() & 0xFF), + static_cast((groundBasedSpeedTransmitData.get_machine_speed() >> 8) & 0xFF), + static_cast(groundBasedSpeedTransmitData.get_machine_distance() & 0xFF), + static_cast((groundBasedSpeedTransmitData.get_machine_distance() >> 8) & 0xFF), + static_cast((groundBasedSpeedTransmitData.get_machine_distance() >> 16) & 0xFF), + static_cast((groundBasedSpeedTransmitData.get_machine_distance() >> 24) & 0xFF), + 0xFF, // Reserved + static_cast(0xFC | static_cast(groundBasedSpeedTransmitData.get_machine_direction_of_travel())) }; // 0xFC sets reserved bits to 1s + retVal = CANNetworkManager::CANNetwork.send_can_message(static_cast(CANLibParameterGroupNumber::GroundBasedSpeedAndDistance), + buffer.data(), + buffer.size(), + std::static_pointer_cast(groundBasedSpeedTransmitData.get_sender_control_function()), + nullptr, + CANIdentifier::Priority3); + } + return retVal; + } + + bool SpeedMessagesInterface::send_machine_selected_speed_command() const + { + bool retVal = false; + + if (nullptr != machineSelectedSpeedCommandTransmitData.get_sender_control_function()) + { + std::array buffer = { static_cast(machineSelectedSpeedCommandTransmitData.get_machine_speed_setpoint_command() & 0xFF), + static_cast((machineSelectedSpeedCommandTransmitData.get_machine_speed_setpoint_command() >> 8) & 0xFF), + static_cast(machineSelectedSpeedCommandTransmitData.get_machine_selected_speed_setpoint_limit() & 0xFF), + static_cast((machineSelectedSpeedCommandTransmitData.get_machine_selected_speed_setpoint_limit() >> 8) & 0xFF), + 0xFF, + 0xFF, + 0xFF, + static_cast(0xFC | static_cast(machineSelectedSpeedCommandTransmitData.get_machine_direction_command())) }; + retVal = CANNetworkManager::CANNetwork.send_can_message(static_cast(CANLibParameterGroupNumber::MachineSelectedSpeedCommand), + buffer.data(), + buffer.size(), + std::static_pointer_cast(machineSelectedSpeedCommandTransmitData.get_sender_control_function()), + nullptr, + CANIdentifier::Priority3); + } + return retVal; + } + +} // namespace isobus diff --git a/src/isobus_speed_distance_messages.hpp b/src/isobus_speed_distance_messages.hpp new file mode 100644 index 0000000..414dd18 --- /dev/null +++ b/src/isobus_speed_distance_messages.hpp @@ -0,0 +1,632 @@ +//================================================================================================ +/// @file isobus_speed_distance_messages.hpp +/// +/// @brief Defines classes for processing/sending ISOBUS speed messages. +/// @attention These classes are meant to be used in the ISOBUS odometry interface, not used +/// directly by a consuming application. +/// @note The full list of standardized speeds can be found at "isobus.net" +/// @author Adrian Del Grosso +/// +/// @copyright 2023 Adrian Del Grosso +//================================================================================================ +#ifndef ISOBUS_SPEED_MESSAGES_HPP +#define ISOBUS_SPEED_MESSAGES_HPP + +#include "can_internal_control_function.hpp" +#include "event_dispatcher.hpp" +#include "processing_flags.hpp" + +#include +#include +#include + +namespace isobus +{ + /// @brief This interface manages and parses ISOBUS speed messages + class SpeedMessagesInterface + { + public: + /// @brief Constructor for a SpeedMessagesInterface + /// @note Normally you would only configure this interface to transmit if you are + /// serving as the tractor ECU (TECU). + /// @param[in] source The internal control function to use when sending messages, or nullptr for listen only + /// @param[in] enableSendingGroundBasedSpeedPeriodically If true, ground-based speed will be sent periodically. (Normally you will not want to send this unless you are sensing the speed yourself) + /// @param[in] enableSendingWheelBasedSpeedPeriodically If true, wheel-based speed will be sent periodically. (Normally you will not want to send this unless you are sensing the speed yourself) + /// @param[in] enableSendingMachineSelectedSpeedPeriodically If true, machine-selected speed will be sent periodically. (Normally you will not want to send this unless you are selecting the speed yourself) + /// @param[in] enableSendingMachineSelectedSpeedCommandPeriodically If true, machine-selected speed command will be sent periodically. (Normally you will not want to send this unless you are intending to cause machine motion) + SpeedMessagesInterface(std::shared_ptr source, + bool enableSendingGroundBasedSpeedPeriodically = false, + bool enableSendingWheelBasedSpeedPeriodically = false, + bool enableSendingMachineSelectedSpeedPeriodically = false, + bool enableSendingMachineSelectedSpeedCommandPeriodically = false); + + /// @brief Destructor for SpeedMessagesInterface. Cleans up PGN registrations if needed. + ~SpeedMessagesInterface(); + + /// @brief Enumerates the values of the direction of travel for the machine. + enum class MachineDirection : std::uint8_t + { + Forward = 0, + Reverse = 1, + Error = 2, + NotAvailable = 3 + }; + + /// @brief Groups the data encoded in an ISO "Wheel-based Speed and Distance" message + class WheelBasedMachineSpeedData + { + public: + /// @brief Enumerates the key switch states of the tractor or power unit. + enum class KeySwitchState : std::uint8_t + { + Off = 0, ///< Key is off + NotOff = 1, ///< Key is not off (does not always mean that it's on!) + Error = 2, + NotAvailable = 3 + }; + + /// @brief Enumerates the states of a switch or operator input to start or enable implement operations + enum class ImplementStartStopOperations : std::uint8_t + { + StopDisableImplementOperations = 0, + StartEnableImplementOperations = 1, + Error = 2, + NotAvailable = 3 + }; + + /// @brief This parameter indicates whether the reported direction is reversed from the perspective of the operator + enum class OperatorDirectionReversed : std::uint8_t + { + NotReversed = 0, + Reversed = 1, + Error = 2, + NotAvailable = 3 + }; + + /// @brief Constructor for a WheelBasedMachineSpeedData + /// @param[in] sender The control function that is sending this message + explicit WheelBasedMachineSpeedData(std::shared_ptr sender); + + /// @brief Returns The distance traveled by a machine as calculated from wheel or tail-shaft speed. + /// @note When the distance exceeds 4211081215m the value shall be reset to zero and incremented as additional distance accrues. + /// @returns The distance traveled by a machine as calculated from wheel or tail-shaft speed. (millimeters) + std::uint32_t get_machine_distance() const; + + /// @brief Sets the distance traveled by a machine as calculated from wheel or tail-shaft speed. + /// @param distance The distance traveled by a machine as calculated from wheel or tail-shaft speed. (millimeters) + /// @return True if the set value was different from the stored value otherwise false + bool set_machine_distance(std::uint32_t distance); + + /// @brief Returns the value of the speed of a machine as calculated from the measured wheel or tail-shaft speed. + /// @returns The value of the speed of a machine as calculated from the measured wheel or tail-shaft speed. + std::uint16_t get_machine_speed() const; + + /// @brief Sets the value of the speed of a machine as calculated from the measured wheel or tail-shaft speed. + /// @param speed The value of the speed of a machine as calculated from the measured wheel or tail-shaft speed. + /// @return True if the set value was different from the stored value otherwise false + bool set_machine_speed(std::uint16_t speed); + + /// @brief Returns the maximum time (in minutes) of remaining tractor or power-unit-supplied electrical power at the current load. + /// @return The maximum time of remaining tractor or power-unit-supplied electrical power at the current load (in minutes) + std::uint8_t get_maximum_time_of_tractor_power() const; + + /// @brief Sets the maximum time (in minutes) of remaining tractor or power-unit-supplied electrical power at the current load. + /// @param maxTime The maximum time of remaining tractor or power-unit-supplied electrical power at the current load (in minutes) + /// @return True if the set value was different from the stored value otherwise false + bool set_maximum_time_of_tractor_power(std::uint8_t maxTime); + + /// @brief Returns A measured signal indicating either forward or reverse as the direction of travel. + /// @note When the speed is zero, this indicates the last travel direction until a different + /// direction is detected or selected and engaged. + /// @return The measured direction of travel for the machine + MachineDirection get_machine_direction_of_travel() const; + + /// @brief Sets a measured signal indicating either forward or reverse as the direction of travel. + /// @note The "Not Off" key switch state does not always mean "On" so use care when using it. + /// @param direction The measured direction of travel for the machine + /// @return True if the set value was different from the stored value otherwise false + bool set_machine_direction_of_travel(MachineDirection direction); + + /// @brief Returns the key switch state of the tractor or power unit. + /// @return The key switch state of the tractor or power unit. + KeySwitchState get_key_switch_state() const; + + /// @brief Sets the reported key switch state of the tractor or power unit. + /// @note The "Not Off" key switch state does not always mean "On" so use care when using it. + /// @param state The key switch state of the tractor or power unit. + /// @return True if the set value was different from the stored value otherwise false + bool set_key_switch_state(KeySwitchState state); + + /// @brief Returns the state of a switch or other operator input to start or enable implement operations. + /// @details The start or enabled state can be the result of the implement being positioned in an operating position. + /// It can be generated by an operator placing a switch to an ON state. Also called "Master ON/OFF" switch. + /// @return The state of a switch or other operator input to start or enable implement operations. + ImplementStartStopOperations get_implement_start_stop_operations_state() const; + + /// @brief Sets the state of a switch or other operator input to start or enable implement operations. + /// @details The start or enabled state can be the result of the implement being positioned in an operating position. + /// It can be generated by an operator placing a switch to an ON state. Also called "Master ON/OFF" switch. + /// @param state The state of a switch or other operator input to start or enable implement operations. + /// @return True if the set value was different from the stored value otherwise false + bool set_implement_start_stop_operations_state(ImplementStartStopOperations state); + + /// @brief Returns whether the reported direction is reversed from the perspective of the operator + /// @return Whether the reported direction is reversed from the perspective of the operator or not + OperatorDirectionReversed get_operator_direction_reversed_state() const; + + /// @brief Sets whether the reported direction is reversed from the perspective of the operator. + /// @param reverseState The state of whether the reported direction is reversed from the perspective of the operator + /// @return True if the set value was different from the stored value otherwise false + bool set_operator_direction_reversed_state(OperatorDirectionReversed reverseState); + + /// @brief Returns a pointer to the sender of the message. If an ICF is the sender, returns the ICF being used to transmit from. + /// @attention The only way you could get an invalid pointer here is if you register a partner, it sends this message, then you delete the partner and + /// call this function, as that is the only time the stack deletes a control function. That would be abnormal program flow, but at some point + /// the stack will be updated to return a shared or weak pointer instead, but for now please be aware of that limitation. + /// Eventually though the message will time-out normally and you can get a new pointer for + /// the external CF that replaces the deleted partner. + /// @returns The control function sending this instance of the guidance system command message + std::shared_ptr get_sender_control_function() const; + + /// @brief Sets the timestamp for when the message was received or sent + /// @param[in] timestamp The timestamp, in milliseconds, when the message was sent or received + void set_timestamp_ms(std::uint32_t timestamp); + + /// @brief Returns the timestamp for when the message was received, in milliseconds + /// @returns The timestamp for when the message was received, in milliseconds + std::uint32_t get_timestamp_ms() const; + + private: + std::shared_ptr const controlFunction; ///< The CF that is sending the message + std::uint32_t timestamp_ms = 0; ///< A timestamp for when the message was released in milliseconds + std::uint32_t wheelBasedMachineDistance_mm = 0; ///< Stores the decoded machine wheel-based distance in millimeters + std::uint16_t wheelBasedMachineSpeed_mm_per_sec = 0; ///< Stores the decoded wheel-based machine speed in mm/s + std::uint8_t maximumTimeOfTractorPower_min = 0; ///< Stores the maximum time of remaining tractor or power-unit-supplied electrical power at the current load + MachineDirection machineDirectionState = MachineDirection::NotAvailable; ///< Stores direction of travel. + KeySwitchState keySwitchState = KeySwitchState::NotAvailable; ///< Stores the key switch state of the tractor or power unit. + ImplementStartStopOperations implementStartStopOperationsState = ImplementStartStopOperations::NotAvailable; ///< Stores the state of a switch or other operator input to start or enable implement operations. + OperatorDirectionReversed operatorDirectionReversedState = OperatorDirectionReversed::NotAvailable; ///< Stores whether the reported direction is reversed from the perspective of the operator + }; + + /// @brief Message that provides the current machine selected speed, direction and source parameters. + /// @details This is usually the best/authoritative source of speed information as chosen by the machine. + class MachineSelectedSpeedData + { + public: + /// @brief This parameter is used to indicate why the vehicle speed control unit cannot currently accept + /// remote commands or has most recently stopped accepting remote commands. + /// @note Some values are reserved or manufacturer specific. See the SPN definition. + enum class ExitReasonCode : std::uint8_t + { + NoReasonAllClear = 0, + RequiredLevelOfOperatorPresenceAwarenessNotDetected = 1, + ImplementReleasedControlOfFunction = 2, + OperatorOverrideOfFunction = 3, + OperatorControlNotInValidPosition = 4, + RemoteCommandTimeout = 5, + RemoteCommandOutOfRangeInvalid = 6, + FunctionNotCalibrated = 7, + OperatorControlFault = 8, + FunctionFault = 9, + VehicleTransmissionGearDoesNotAllowRemoteCommands = 22, + Error = 62, + NotAvailable = 63 + }; + + /// @brief An indication of the speed source that is currently being reported in the machine selected speed parameter. + /// @details This parameter is an indication of the speed source that is currently being reported in the machine selected speed parameter. + /// Simulated speed is a system-generated speed message to permit implement operations when the machine is not actually moving. + /// Blended speed is a speed message that uses a combination of the actual speed sources based on the operator's or the manufacturer's selected logic, + /// i.e. when a ground-based speed source is less than 0.5 m/s, the speed message will then send the wheel speed source. + enum class SpeedSource : std::uint8_t + { + WheelBasedSpeed = 0, ///< Wheel encoder usually + GroundBasedSpeed = 1, ///< Radar usually + NavigationBasedSpeed = 2, ///< GNSS Usually + Blended = 3, ///< Some combination of source fusion + Simulated = 4, ///< A test speed + Reserved_1 = 5, ///< Reserved + Reserved_2 = 6, ///< Reserved + NotAvailable = 7 ///< N/A + }; + + /// @brief This parameter is used to report the Tractor ECU's present limit + /// status associated with a parameter whose commands are persistent. + /// Similar to other SAEbs03 limit statuses. + enum class LimitStatus : std::uint8_t + { + NotLimited = 0, + OperatorLimitedControlled = 1, ///< Request cannot be implemented + LimitedHigh = 2, ///< Only lower command values result in a change + LimitedLow = 3, ///< Only higher command values result in a change + Reserved_1 = 4, ///< Reserved + Reserved_2 = 5, ///< Reserved + NonRecoverableFault = 6, + NotAvailable ///< Parameter not supported + }; + + /// @brief Constructor for a MachineSelectedSpeedData + /// @param[in] sender The control function that is sending this message + explicit MachineSelectedSpeedData(std::shared_ptr sender); + + /// @brief Returns the Actual distance travelled by the machine based on the value of selected machine speed (SPN 4305). + /// @note When the distance exceeds 4211081215 meters the value shall be reset to zero and incremented as additional distance accrues. + /// @return The Actual distance travelled by the machine based on the value of selected machine speed (millimeters) + std::uint32_t get_machine_distance() const; + + /// @brief Sets the Actual distance travelled by the machine based on the value of selected machine speed (SPN 4305). + /// @note When the distance exceeds 4211081215 meters the value shall be reset to zero and incremented as additional distance accrues. + /// @param[in] distance The Actual distance travelled by the machine based on the value of selected machine speed (millimeters) + /// @return True if the set value was different from the stored value otherwise false + bool set_machine_distance(std::uint32_t distance); + + /// @brief Returns the current machine selected speed. + /// @details The TECU sends this value as the authoritative speed for the machine. + /// @returns The current machine selected speed in mm/s + std::uint16_t get_machine_speed() const; + + /// @brief Sets the machine selected speed. + /// @param speed The machine selected speed in mm/s + /// @return True if the set value was different from the stored value otherwise false + bool set_machine_speed(std::uint16_t speed); + + /// @brief Returns the reason why the vehicle speed control unit cannot currently accept remote commands or + /// has most recently stopped accepting remote commands + /// @return why the vehicle speed control unit cannot currently accept remote commands or + /// has most recently stopped accepting remote commands + std::uint8_t get_exit_reason_code() const; + + /// @brief Sets the reason why the vehicle speed control unit cannot currently accept remote commands or + /// has most recently stopped accepting remote commands + /// @param exitCode The exit/reason code to set + /// @return True if the set value was different from the stored value otherwise false + bool set_exit_reason_code(std::uint8_t exitCode); + + /// @brief Returns the speed source that is currently being reported in the machine selected speed parameter (SPN-4305). + /// @return The speed source that is currently being reported in the machine selected speed parameter (SPN-4305). + SpeedSource get_speed_source() const; + + /// @brief Sets the speed source that is currently being reported in the machine selected speed parameter (SPN-4305). + /// @param selectedSource The speed source that is currently being reported in the machine selected speed parameter (SPN-4305). + /// @return True if the set value was different from the stored value otherwise false + bool set_speed_source(SpeedSource selectedSource); + + /// @brief Returns The Tractor ECU's present limit status associated with a parameter whose commands are persistent + /// @return The Tractor ECU's present limit status associated with a parameter whose commands are persistent + LimitStatus get_limit_status() const; + + /// @brief Sets the Tractor ECU's present limit status associated with a parameter whose commands are persistent + /// @param statusToSet The Tractor ECU's present limit status associated with a parameter whose commands are persistent + /// @return True if the set value was different from the stored value otherwise false + bool set_limit_status(LimitStatus statusToSet); + + /// @brief Returns A measured signal indicating either forward or reverse as the direction of travel. + /// @note When the speed is zero, this indicates the last travel direction until a different + /// direction is detected or selected and engaged. + /// @return The measured direction of travel for the machine + MachineDirection get_machine_direction_of_travel() const; + + /// @brief Sets a measured signal indicating either forward or reverse as the direction of travel. + /// @note The "Not Off" key switch state does not always mean "On" so use care when using it. + /// @param directionOfTravel The measured direction of travel for the machine + /// @return True if the set value was different from the stored value otherwise false + bool set_machine_direction_of_travel(MachineDirection directionOfTravel); + + /// @brief Returns a pointer to the sender of the message. If an ICF is the sender, returns the ICF being used to transmit from. + /// @attention The only way you could get an invalid pointer here is if you register a partner, it sends this message, then you delete the partner and + /// call this function, as that is the only time the stack deletes a control function. That would be abnormal program flow, but at some point + /// the stack will be updated to return a shared or weak pointer instead, but for now please be aware of that limitation. + /// Eventually though the message will time-out normally and you can get a new pointer for + /// the external CF that replaces the deleted partner. + /// @returns The control function sending this instance of the guidance system command message + std::shared_ptr get_sender_control_function() const; + + /// @brief Sets the timestamp for when the message was received or sent + /// @param[in] timestamp The timestamp, in milliseconds, when the message was sent or received + void set_timestamp_ms(std::uint32_t timestamp); + + /// @brief Returns the timestamp for when the message was received, in milliseconds + /// @returns The timestamp for when the message was received, in milliseconds + std::uint32_t get_timestamp_ms() const; + + private: + std::shared_ptr const controlFunction; ///< The CF that is sending the message + std::uint32_t timestamp_ms = 0; ///< A timestamp for when the message was released in milliseconds + std::uint32_t machineSelectedSpeedDistance_mm = 0; ///< Stores the machine selected speed distance in millimeters + std::uint16_t machineSelectedSpeed_mm_per_sec = 0; ///< Stores the machine selected speed in mm/s + std::uint8_t exitReasonCode = static_cast(ExitReasonCode::NotAvailable); ///< Stores why the machine has most recently stopped accepting remote commands. + SpeedSource source = SpeedSource::NotAvailable; ///< Stores the speed source that is currently being reported + LimitStatus limitStatus = LimitStatus::NotAvailable; ///< Stores the tractor ECU limit status + MachineDirection machineDirectionState = MachineDirection::NotAvailable; ///< Stores direction of travel. + }; + + /// @brief Message normally sent by the Tractor ECU on the implement bus on construction and + /// agricultural implements providing to connected systems the current measured ground speed + /// (also includes a free-running distance counter and an indication of the direction of travel). + /// + /// @note Accuracies of both wheel-based and ground-based sources can be speed-dependent and degrade at low speeds. + /// Wheel-based information might not be updated at the 100ms rate at low speeds. + class GroundBasedSpeedData + { + public: + /// @brief Constructor for a GroundBasedSpeedData + /// @param[in] sender The control function that is sending this message + explicit GroundBasedSpeedData(std::shared_ptr sender); + + /// @brief Actual distance traveled by a machine, based on measurements from a sensor such as that is not susceptible to wheel slip + /// (e.g. radar, GPS, LIDAR, or stationary object tracking) + /// @note This distance is usually provided by radar. + /// @return Actual distance traveled by a machine, based on measurements from a sensor such as that is not susceptible to wheel slip (millimeters) + std::uint32_t get_machine_distance() const; + + /// @brief Sets the actual distance traveled by a machine, based on measurements from a sensor such as that is not susceptible to wheel slip. + /// @note This distance is usually provided by radar. + /// @param distance The actual distance traveled by a machine (millimeters) + /// @return True if the set value was different from the stored value otherwise false + bool set_machine_distance(std::uint32_t distance); + + /// @brief Returns the actual ground speed of a machine, measured by a sensor such as that is not susceptible to wheel slip in mm/s + /// @note This speed is usually provided by radar. + /// @return The actual ground speed of a machine, measured by a sensor such as that is not susceptible to wheel slip in mm/s + std::uint16_t get_machine_speed() const; + + /// @brief Sets the actual ground speed of a machine, measured by a sensor such as that is not susceptible to wheel slip in mm/s + /// @note This speed is usually provided by radar. + /// @param speed The actual ground speed of a machine, measured by a sensor such as that is not susceptible to wheel slip in mm/s + /// @return True if the set value was different from the stored value otherwise false + bool set_machine_speed(std::uint16_t speed); + + /// @brief Returns A measured signal indicating either forward or reverse as the direction of travel. + /// @note When the speed is zero, this indicates the last travel direction until a different + /// direction is detected or selected and engaged. + /// @return The measured direction of travel for the machine + MachineDirection get_machine_direction_of_travel() const; + + /// @brief Sets a measured signal indicating either forward or reverse as the direction of travel. + /// @note The "Not Off" key switch state does not always mean "On" so use care when using it. + /// @param directionOfTravel The measured direction of travel for the machine + /// @return True if the set value was different from the stored value otherwise false + bool set_machine_direction_of_travel(MachineDirection directionOfTravel); + + /// @brief Returns a pointer to the sender of the message. If an ICF is the sender, returns the ICF being used to transmit from. + /// @attention The only way you could get an invalid pointer here is if you register a partner, it sends this message, then you delete the partner and + /// call this function, as that is the only time the stack deletes a control function. That would be abnormal program flow, but at some point + /// the stack will be updated to return a shared or weak pointer instead, but for now please be aware of that limitation. + /// Eventually though the message will time-out normally and you can get a new pointer for + /// the external CF that replaces the deleted partner. + /// @returns The control function sending this instance of the guidance system command message + std::shared_ptr get_sender_control_function() const; + + /// @brief Sets the timestamp for when the message was received or sent + /// @param[in] timestamp The timestamp, in milliseconds, when the message was sent or received + void set_timestamp_ms(std::uint32_t timestamp); + + /// @brief Returns the timestamp for when the message was received, in milliseconds + /// @returns The timestamp for when the message was received, in milliseconds + std::uint32_t get_timestamp_ms() const; + + private: + std::shared_ptr const controlFunction; ///< The CF that is sending the message + std::uint32_t timestamp_ms = 0; ///< A timestamp for when the message was released in milliseconds + std::uint32_t groundBasedMachineDistance_mm = 0; ///< Stores the ground-based speed's distance in millimeters + std::uint16_t groundBasedMachineSpeed_mm_per_sec = 0; ///< Stores the ground-based speed in mm/s + MachineDirection machineDirectionState = MachineDirection::NotAvailable; ///< Stores direction of travel. + }; + + /// @brief Message that provides the control of the machine speed and direction. + /// If you receive this message, you can sniff the speed commands being sent to the TECU or act + /// as the TECU or propulsion interface yourself. + /// @attention Use extreme caution if you choose to send this message, as you may cause machine motion! + class MachineSelectedSpeedCommandData + { + public: + /// @brief Constructor for a MachineSelectedSpeedCommandData + /// @param[in] sender The control function that is sending this message + explicit MachineSelectedSpeedCommandData(std::shared_ptr sender); + + /// @brief Returns the commanded setpoint value of the machine speed as measured by the selected source in mm/s + /// @return The commanded setpoint value of the machine speed as measured by the selected source in mm/s + std::uint16_t get_machine_speed_setpoint_command() const; + + /// @brief Sets The commanded setpoint value of the machine speed as measured by the selected source in mm/s + /// @attention This is used to set the speed of the machine! Use with caution! + /// @param speed The commanded setpoint value of the machine speed as measured by the selected source in mm/s + /// @return True if the set value was different from the stored value otherwise false + bool set_machine_speed_setpoint_command(std::uint16_t speed); + + /// @brief Returns the machine's maximum allowed speed in mm/s, which get's communicated to the tractor/machine + /// @returns The machine's maximum allowed speed in mm/s, which get's communicated to the tractor/machine + std::uint16_t get_machine_selected_speed_setpoint_limit() const; + + /// @brief Sets the maximum allowed machine speed in mm/s, which gets communicated to the tractor/machine + /// @param[in] speedLimit The maximum allowed machine speed in mm/s + /// @return True if the set value was different from the stored value otherwise false + bool set_machine_selected_speed_setpoint_limit(std::uint16_t speedLimit); + + /// @brief Returns The commanded direction of the machine. + /// @return The commanded direction of the machine. + MachineDirection get_machine_direction_command() const; + + /// @brief Sets the commanded direction of the machine. + /// @param commandedDirection The commanded direction of travel for the machine + /// @return True if the set value was different from the stored value otherwise false + bool set_machine_direction_of_travel(MachineDirection commandedDirection); + + /// @brief Returns a pointer to the sender of the message. If an ICF is the sender, returns the ICF being used to transmit from. + /// @attention The only way you could get an invalid pointer here is if you register a partner, it sends this message, then you delete the partner and + /// call this function, as that is the only time the stack deletes a control function. That would be abnormal program flow, but at some point + /// the stack will be updated to return a shared or weak pointer instead, but for now please be aware of that limitation. + /// Eventually though the message will time-out normally and you can get a new pointer for + /// the external CF that replaces the deleted partner. + /// @returns The control function sending this instance of the guidance system command message + std::shared_ptr get_sender_control_function() const; + + /// @brief Sets the timestamp for when the message was received or sent + /// @param[in] timestamp The timestamp, in milliseconds, when the message was sent or received + void set_timestamp_ms(std::uint32_t timestamp); + + /// @brief Returns the timestamp for when the message was received, in milliseconds + /// @returns The timestamp for when the message was received, in milliseconds + std::uint32_t get_timestamp_ms() const; + + private: + std::shared_ptr const controlFunction; ///< The CF that is sending the message + std::uint32_t timestamp_ms = 0; ///< A timestamp for when the message was released in milliseconds + std::uint16_t speedCommandedSetpoint = 0; ///< Stores the commanded speed setpoint in mm/s + std::uint16_t speedSetpointLimit = 0; ///< Stores the maximum allowed speed in mm/s + MachineDirection machineDirectionCommand = MachineDirection::NotAvailable; ///< Stores commanded direction of travel. + }; + + /// @brief Sets up the class and registers it to receive callbacks from the network manager for processing + /// guidance messages. The class will not receive messages if this is not called. + void initialize(); + + /// @brief Returns if the interface has been initialized + /// @returns true if initialize has been called for this interface, otherwise false + bool get_initialized() const; + + /// @brief Use this to configure transmission of the machine selected speed message. + /// If you pass in an internal control function to the constructor of this class, then this message is available to be sent. + MachineSelectedSpeedData machineSelectedSpeedTransmitData; + + /// @brief Use this to configure transmission of the wheel-based speed message. + /// If you pass in an internal control function to the constructor of this class, then this message is available to be sent. + WheelBasedMachineSpeedData wheelBasedSpeedTransmitData; + + /// @brief Use this to configure transmission of the ground-based speed message. + /// If you pass in an internal control function to the constructor of this class, then this message is available to be sent. + GroundBasedSpeedData groundBasedSpeedTransmitData; + + /// @brief Use this to configure transmission of the machine selected speed command message. + /// If you pass in an internal control function to the constructor of this class, then this message is available to be sent. + MachineSelectedSpeedCommandData machineSelectedSpeedCommandTransmitData; + + /// @brief Returns the number of received, unique wheel-based speed message sources + /// @returns The number of CFs sending the wheel-based speed message + std::size_t get_number_received_wheel_based_speed_sources() const; + + /// @brief Returns the number of received, unique ground-based speed message sources + /// @returns The number of CFs sending the ground-based speed message + std::size_t get_number_received_ground_based_speed_sources() const; + + /// @brief Returns the number of received, unique machine selected speed message sources + /// @returns The number of CFs sending the machine selected speed message + std::size_t get_number_received_machine_selected_speed_sources() const; + + /// @brief Returns the number of received, unique machine selected speed command message sources + /// @returns The number of CFs sending the machine selected speed command message + std::size_t get_number_received_machine_selected_speed_command_sources() const; + + /// @brief Returns the content of the machine selected speed message + /// based on the index of the sender. Use this to read the received messages' content. + /// @param[in] index An index of senders of the machine selected speed message + /// @note Only one device on the bus will send this normally, but we provide a generic way to get + /// an arbitrary number of these commands. So generally using only index 0 will be acceptable. + /// @note It is also possible that this message may not be present, depending on your machine. + std::shared_ptr get_received_machine_selected_speed(std::size_t index); + + /// @brief Returns the content of the wheel-based speed message + /// based on the index of the sender. Use this to read the received messages' content. + /// @param[in] index An index of senders of the wheel-based speed message + /// @note Only one device on the bus will send this normally, but we provide a generic way to get + /// an arbitrary number of these commands. So generally using only index 0 will be acceptable. + /// @note It is also possible that this message may not be present, depending on your machine. + std::shared_ptr get_received_wheel_based_speed(std::size_t index); + + /// @brief Returns the content of the ground-based speed message + /// based on the index of the sender. Use this to read the received messages' content. + /// @param[in] index An index of senders of the ground-based speed message + /// @note Only one device on the bus will send this normally, but we provide a generic way to get + /// an arbitrary number of these commands. So generally using only index 0 will be acceptable. + /// @note It is also possible that this message may not be present, depending on your machine. + std::shared_ptr get_received_ground_based_speed(std::size_t index); + + /// @brief Returns the content of the machine selected speed command message + /// based on the index of the sender. Use this to read the received messages' content. + /// @param[in] index An index of senders of the machine selected speed command message + /// @note Only one device on the bus will send this normally, but we provide a generic way to get + /// an arbitrary number of these commands. So generally using only index 0 will be acceptable. + /// @note It is also possible that this message may not be present, depending on your machine. + std::shared_ptr get_received_machine_selected_speed_command(std::size_t index); + + /// @brief Returns an event dispatcher which you can use to get callbacks when new/updated wheel-based speed messages are received. + /// @returns The event publisher for wheel-based speed messages + EventDispatcher, bool> &get_wheel_based_machine_speed_data_event_publisher(); + + /// @brief Returns an event dispatcher which you can use to get callbacks when new/updated machine selected speed messages are received. + /// @returns The event publisher for machine selected speed messages + EventDispatcher, bool> &get_machine_selected_speed_data_event_publisher(); + + /// @brief Returns an event dispatcher which you can use to get callbacks when new/updated ground-based speed messages are received. + /// @returns The event publisher for ground-based speed messages + EventDispatcher, bool> &get_ground_based_machine_speed_data_event_publisher(); + + /// @brief Returns an event dispatcher which you can use to get callbacks when new/updated machine selected speed command messages are received. + /// @returns The event publisher for machine selected speed command messages + EventDispatcher, bool> &get_machine_selected_speed_command_data_event_publisher(); + + /// @brief Call this cyclically to update the interface. Transmits messages if needed and processes + /// timeouts for received messages. + void update(); + + protected: + /// @brief Enumerates a set of flags to manage transmitting messages owned by this interface + enum class TransmitFlags : std::uint32_t + { + SendWheelBasedSpeed = 0, ///< A flag to manage sending wheel-based speed + SendMachineSelectedSpeed, ///< A flag to manage sending machine selected speed + SendGroundBasedSpeed, ///< A flag to manage sending ground-based speed + SendMachineSelectedSpeedCommand, ///< A flag to manage sending the machine selected speed command message + + NumberOfFlags ///< The number of flags in this enumeration + }; + + static constexpr std::uint32_t SPEED_DISTANCE_MESSAGE_TX_INTERVAL_MS = 100; ///< The interval (in milliseconds) defined in ISO11783-7 for sending this class's messages + static constexpr std::uint32_t SPEED_DISTANCE_MESSAGE_RX_TIMEOUT_MS = 3 * SPEED_DISTANCE_MESSAGE_TX_INTERVAL_MS; ///< A (somewhat arbitrary) timeout for detecting stale messages. + static constexpr std::uint32_t SAEds05_MAX_VALUE = 4211081215; ///< The maximum valid value for a SAEds05 slot (see J1939) + static constexpr std::uint16_t SAEvl01_MAX_VALUE = 64255; ///< The maximum valid value for a SAEvl01 slot (see J1939) + + /// @brief Processes one flag (which sends the associated message) + /// @param[in] flag The flag to process + /// @param[in] parentPointer A pointer to the interface instance + static void process_flags(std::uint32_t flag, void *parentPointer); + + /// @brief Processes a CAN message + /// @param[in] message The CAN message being received + /// @param[in] parentPointer A context variable to find the relevant instance of this class + static void process_rx_message(const CANMessage &message, void *parentPointer); + + /// @brief Sends the machine selected speed message + /// @returns true if the message was sent, otherwise false + bool send_machine_selected_speed() const; + + /// @brief Sends the wheel-based speed message + /// @returns true if the message was sent, otherwise false + bool send_wheel_based_speed() const; + + /// @brief Sends the ground-based speed message + /// @returns true if the message was sent, otherwise false + bool send_ground_based_speed() const; + + /// @brief Sends the machine selected speed command message + /// @returns true if the message was sent, otherwise false + bool send_machine_selected_speed_command() const; + + ProcessingFlags txFlags; ///< Tx flag for sending messages periodically + EventDispatcher, bool> wheelBasedMachineSpeedDataEventPublisher; ///< An event publisher for notifying when new wheel-based speed messages are received + EventDispatcher, bool> machineSelectedSpeedDataEventPublisher; ///< An event publisher for notifying when new machine selected speed messages are received + EventDispatcher, bool> groundBasedSpeedDataEventPublisher; ///< An event publisher for notifying when new ground-based speed messages are received + EventDispatcher, bool> machineSelectedSpeedCommandDataEventPublisher; ///< An event publisher for notifying when new machine selected speed command messages are received + std::vector> receivedWheelBasedSpeedMessages; ///< A list of all received wheel-based speed messages + std::vector> receivedMachineSelectedSpeedMessages; ///< A list of all received machine selected speed messages + std::vector> receivedGroundBasedSpeedMessages; ///< A list of all received ground-based speed messages + std::vector> receivedMachineSelectedSpeedCommandMessages; ///< A list of all received ground-based speed messages + std::uint32_t wheelBasedSpeedTransmitTimestamp_ms = 0; ///< Timestamp used to know when to transmit the wheel-based speed message in milliseconds + std::uint32_t machineSelectedSpeedTransmitTimestamp_ms = 0; ///< Timestamp used to know when to transmit the machine selected speed message in milliseconds + std::uint32_t groundBasedSpeedTransmitTimestamp_ms = 0; ///< Timestamp used to know when to transmit the ground-based speed message in milliseconds + std::uint32_t machineSelectedSpeedCommandTransmitTimestamp_ms = 0; ///< Timestamp used to know when to transmit the ground-based speed message in milliseconds + bool initialized = false; ///< Stores if the interface has been initialized + }; +} // namespace isobus + +#endif // ISOBUS_SPEED_MESSAGES_HPP diff --git a/src/isobus_standard_data_description_indices.hpp b/src/isobus_standard_data_description_indices.hpp new file mode 100644 index 0000000..3fae628 --- /dev/null +++ b/src/isobus_standard_data_description_indices.hpp @@ -0,0 +1,548 @@ +//================================================================================================ +/// @file isobus_standard_data_description_indices.hpp +/// +/// @brief Defines some standard DDIs. +/// Does not include proprietary DDIs. +/// @note The full list of standardized DDIs can be found at "isobus.net" +/// @author Adrian Del Grosso +/// +/// @copyright 2023 Adrian Del Grosso +//================================================================================================ +#ifndef ISOBUS_STANDARD_DATA_DESCRIPTION_INDICES_HPP +#define ISOBUS_STANDARD_DATA_DESCRIPTION_INDICES_HPP + +#include + +namespace isobus +{ + /// @brief Enumerates a subset of standard ISOBUS DDIs + enum class DataDescriptionIndex : std::uint16_t + { + DataDictionaryVersion = 0x0000, ///< This DDE is used to specify which version of the Data Dictionary is being used. + SetpointVolumePerAreaApplicationRate = 0x0001, ///< Setpoint Application Rate specified as volume per area + ActualVolumePerAreaApplicationRate = 0x0002, ///< Actual Application Rate specified as volume per area + DefaultVolumePerAreaApplicationRate = 0x0003, ///< Default Application Rate specified as volume per area + MinimumVolumePerAreaApplicationRate = 0x0004, ///< Minimum Application Rate specified as volume per area + MaximumVolumePerAreaApplicationRate = 0x0005, ///< Maximum Application Rate specified as volume per area + SetpointMassPerAreaApplicationRate = 0x0006, ///< Setpoint Application Rate specified as mass per area + ActualMassPerAreaApplicationRate = 0x0007, ///< Actual Application Rate specified as mass per area + DefaultMassPerAreaApplicationRate = 0x0008, ///< Default Application Rate specified as mass per area + MinimumMassPerAreaApplicationRate = 0x0009, ///< Minimum Application Rate specified as mass per area + MaximumMassPerAreaApplicationRate = 0x000A, ///< Maximum Application Rate specified as mass per area + SetpointCountPerAreaApplicationRate = 0x000B, ///< Setpoint Application Rate specified as count per area + ActualCountPerAreaApplicationRate = 0x000C, ///< Actual Application Rate specified as count per area + DefaultCountPerAreaApplicationRate = 0x000D, ///< Default Application Rate specified as count per area + MinimumCountPerAreaApplicationRate = 0x000E, ///< Minimum Application Rate specified as count per area + MaximumCountPerAreaApplicationRate = 0x000F, ///< Maximum Application Rate specified as count per area + SetpointSpacingApplicationRate = 0x0010, ///< Setpoint Application Rate specified as distance: e.g. seed spacing of a precision seeder + ActualSpacingApplicationRate = 0x0011, ///< Actual Application Rate specified as distance: e.g. seed spacing of a precision seeder + DefaultSpacingApplicationRate = 0x0012, ///< Default Application Rate specified as distance: e.g. seed spacing of a precision seeder + MinimumSpacingApplicationRate = 0x0013, ///< Minimum Application Rate specified as distance: e.g. seed spacing of a precision seeder + MaximumSpacingApplicationRate = 0x0014, ///< Maximum Application Rate specified as distance: e.g. seed spacing of a precision seeder + SetpointVolumePerVolumeApplicationRate = 0x0015, ///< Setpoint Application Rate specified as volume per volume + ActualVolumePerVolumeApplicationRate = 0x0016, ///< Actual Application Rate specified as volume per volume + DefaultVolumePerVolumeApplicationRate = 0x0017, ///< Default Application Rate specified as volume per volume + MinimumVolumePerVolumeApplicationRate = 0x0018, ///< Minimum Application Rate specified as volume per volume + MaximumVolumePerVolumeApplicationRate = 0x0019, ///< Maximum Application Rate specified as volume per volume + SetpointMassPerMassApplicationRate = 0x001A, ///< Setpoint Application Rate specified as mass per mass + ActualMassPerMassApplicationRate = 0x001B, ///< Actual Application Rate specified as mass per mass + DefaultMassPerMassApplicationRate = 0x001C, ///< Default Application Rate specified as mass per mass + MinimumMassPerMassApplicationRate = 0x001D, ///< Minimum Application Rate specified as mass per mass + MaximumMassPerMassApplicationRate = 0x001E, ///< Maximum Application Rate specified as mass per mass + SetpointVolumePerMassApplicationRate = 0x001F, ///< Setpoint Application Rate specified as volume per mass + ActualVolumePerMassApplicationRate = 0x0020, ///< Actual Application Rate specified as volume per mass + DefaultVolumePerMassApplicationRate = 0x0021, ///< Default Application Rate specified as volume per mass + MinimumVolumePerMassApplicationRate = 0x0022, ///< Minimum Application Rate specified as volume per mass + MaximumVolumePerMassApplicationRate = 0x0023, ///< Maximum Application Rate specified as volume per mass + SetpointVolumePerTimeApplicationRate = 0x0024, ///< Setpoint Application Rate specified as volume per time + ActualVolumePerTimeApplicationRate = 0x0025, ///< Actual Application Rate specified as volume per time + DefaultVolumePerTimeApplicationRate = 0x0026, ///< Default Application Rate specified as volume per time + MinimumVolumePerTimeApplicationRate = 0x0027, ///< Minimum Application Rate specified as volume per time + MaximumVolumePerTimeApplicationRate = 0x0028, ///< Maximum Application Rate specified as volume per time + SetpointMassPerTimeApplicationRate = 0x0029, ///< Setpoint Application Rate specified as mass per time + ActualMassPerTimeApplicationRate = 0x002A, ///< Actual Application Rate specified as mass per time + DefaultMassPerTimeApplicationRate = 0x002B, ///< Default Application Rate specified as mass per time + MinimumMassPerTimeApplicationRate = 0x002C, ///< Minimum Application Rate specified as mass per time + MaximumMassPerTimeApplicationRate = 0x002D, ///< Maximum Application Rate specified as mass per time + SetpointCountPerTimeApplicationRate = 0x002E, ///< Setpoint Application Rate specified as count per time + ActualCountPerTimeApplicationRate = 0x002F, ///< Actual Application Rate specified as count per time + DefaultCountPerTimeApplicationRate = 0x0030, ///< Default Application Rate specified as count per time + MinimumCountPerTimeApplicationRate = 0x0031, ///< Minimum Application Rate specified as count per time + MaximumCountPerTimeApplicationRate = 0x0032, ///< Maximum Application Rate specified as count per time + SetpointTillageDepth = 0x0033, ///< Setpoint Tillage Depth of Device Element below soil surface, value increases with depth. In case of a negative value the system will indicate the distance above the ground. + ActualTillageDepth = 0x0034, ///< Actual Tillage Depth of Device Element below soil surface, value increases with depth. In case of a negative value the system will indicate the distance above the ground. + DefaultTillageDepth = 0x0035, ///< Default Tillage Depth of Device Element below soil surface, value increases with depth. In case of a negative value the system will indicate the distance above the ground. + MinimumTillageDepth = 0x0036, ///< Minimum Tillage Depth of Device Element below soil surface, value increases with depth. In case of a negative value the system will indicate the distance above the ground. + MaximumTillageDepth = 0x0037, ///< Maximum Tillage Depth of Device Element below soil surface, value increases with depth. In case of a negative value the system will indicate the distance above the ground. + SetpointSeedingDepth = 0x0038, ///< Setpoint Seeding Depth of Device Element below soil surface, value increases with depth + ActualSeedingDepth = 0x0039, ///< Actual Seeding Depth of Device Element below soil surface, value increases with depth + DefaultSeedingDepth = 0x003A, ///< Default Seeding Depth of Device Element below soil surface, value increases with depth + MinimumSeedingDepth = 0x003B, ///< Minimum Seeding Depth of Device Element below soil surface, value increases with depth + MaximumSeedingDepth = 0x003C, ///< Maximum Seeding Depth of Device Element below soil surface, value increases with depth + SetpointWorkingHeight = 0x003D, ///< Setpoint Working Height of Device Element above crop or soil + ActualWorkingHeight = 0x003E, ///< Actual Working Height of Device Element above crop or soil + DefaultWorkingHeight = 0x003F, ///< Default Working Height of Device Element above crop or soil + MinimumWorkingHeight = 0x0040, ///< Minimum Working Height of Device Element above crop or soil + MaximumWorkingHeight = 0x0041, ///< Maximum Working Height of Device Element above crop or soil + SetpointWorkingWidth = 0x0042, ///< Setpoint Working Width of Device Element + ActualWorkingWidth = 0x0043, ///< Actual Working Width of Device Element + DefaultWorkingWidth = 0x0044, ///< Default Working Width of Device Element + MinimumWorkingWidth = 0x0045, ///< Minimum Working Width of Device Element + MaximumWorkingWidth = 0x0046, ///< Maximum Working Width of Device Element + SetpointVolumeContent = 0x0047, ///< Setpoint Device Element Content specified as volume + ActualVolumeContent = 0x0048, ///< Actual Device Element Content specified as volume + MaximumVolumeContent = 0x0049, ///< Maximum Device Element Content specified as volume + SetpointMassContent = 0x004A, ///< Setpoint Machine Element Content specified as mass + ActualMassContent = 0x004B, ///< Actual Device Element Content specified as mass + MaximumMassContent = 0x004C, ///< Maximum Device Element Content specified as mass + SetpointCountContent = 0x004D, ///< Setpoint Device Element Content specified as count + ActualCountContent = 0x004E, ///< Actual Device Element Content specified as count + MaximumCountContent = 0x004F, ///< Maximum Device Element Content specified as count + ApplicationTotalVolume_L = 0x0050, ///< Accumulated Application specified as volume in liter [L] + ApplicationTotalMass_kg = 0x0051, ///< Accumulated Application specified as mass in kilogram [kg] + ApplicationTotalCount = 0x0052, ///< Accumulated Application specified as count + VolumePerAreaYield = 0x0053, ///< Yield as volume per area + MassPerAreaYield = 0x0054, ///< Yield as mass per area, not corrected for the reference moisture percentage DDI 184. + CountPerAreaYield = 0x0055, ///< Yield as count per area + VolumePerTimeYield = 0x0056, ///< Yield as volume per time + MassPerTimeYield = 0x0057, ///< Yield as mass per time, not corrected for the reference moisture percentage DDI 184. + CountPerTimeYield = 0x0058, ///< Yield as count per time + YieldTotalVolume = 0x0059, ///< Accumulated Yield specified as volume + YieldTotalMass = 0x005A, ///< Accumulated Yield specified as mass, not corrected for the reference moisture percentage DDI 184. + YieldTotalCount = 0x005B, ///< Accumulated Yield specified as count + VolumePerAreaCropLoss = 0x005C, ///< Crop yield loss as volume per area + MassPerAreaCropLoss = 0x005D, ///< Crop yield loss as mass per area + CountPerAreaCropLoss = 0x005E, ///< Crop yield loss as count per area + VolumePerTimeCropLoss = 0x005F, ///< Crop yield loss as volume per time + MassPerTimeCropLoss = 0x0060, ///< Crop yield loss as mass per time + CountPerTimeCropLoss = 0x0061, ///< Crop yield loss as count per time + PercentageCropLoss = 0x0062, ///< Crop yield loss + CropMoisture = 0x0063, ///< Moisture in crop yield + CropContamination = 0x0064, ///< Dirt or foreign material in crop yield + SetpointBaleWidth = 0x0065, ///< Setpoint Bale Width for square baler or round baler + ActualBaleWidth = 0x0066, ///< Actual Bale Width for square baler or round baler + DefaultBaleWidth = 0x0067, ///< Default Bale Width for square baler or round baler + MinimumBaleWidth = 0x0068, ///< Minimum Bale Width for square baler or round baler + MaximumBaleWidth = 0x0069, ///< Maximum Bale Width for square baler or round baler + SetpointBaleHeight = 0x006A, ///< Setpoint Bale Height is only applicable to square baler + ActualBaleHeight = 0x006B, ///< Actual Bale Height is only applicable to square baler + DefaultBaleHeight = 0x006C, ///< Default Bale Height is only applicable to square baler + MinimumBaleHeight = 0x006D, ///< Minimum Bale Height is only applicable to square baler + MaximumBaleHeight = 0x006E, ///< Maximum Bale Height is only applicable to square baler + SetpointBaleSize = 0x006F, ///< Setpoint Bale Size as length for a square baler or diameter for a round baler + ActualBaleSize = 0x0070, ///< Actual Bale Size as length for a square baler or diameter for a round baler + DefaultBaleSize = 0x0071, ///< Default Bale Size as length for a square baler or diameter for a round baler + MinimumBaleSize = 0x0072, ///< Minimum Bale Size as length for a square baler or diameter for a round baler + MaximumBaleSize = 0x0073, ///< Maximum Bale Size as length for a square baler or diameter for a round baler + TotalArea = 0x0074, ///< Accumulated Area + EffectiveTotalDistance = 0x0075, ///< Accumulated Distance in working position + IneffectiveTotalDistance = 0x0076, ///< Accumulated Distance out of working position + EffectiveTotalTime = 0x0077, ///< Accumulated Time in working position + IneffectiveTotalTime = 0x0078, ///< Accumulated Time out of working position + ProductDensityMassPerVolume = 0x0079, ///< Product Density as mass per volume + ProductDensityMassPerCount = 0x007A, ///< Product Density as mass per count + ProductDensityVolumePerCount = 0x007B, ///< Product Density as volume per count + AuxiliaryValveScalingExtend = 0x007C, ///< Factor to apply to AuxValveCommand PortFlowCommand. The scaling of the port flow relates to flow, not to spool position, although the position of the spool is of course indirectly affected. + AuxiliaryValveScalingRetract = 0x007D, ///< Factor to apply to AuxValveCommand PortFlowCommand. The scaling of the port flow relates to flow, not to spool position, although the position of the spool is of course indirectly affected. + AuxiliaryValveRampExtendUp = 0x007E, ///< The valve will apply a ramp to the Auxiliary ValveCommand PortFlowCommand, to limit the acceleration or deceleration of flow. The valve must apply the ramp to create a linear increase/decrease of flow over time. + AuxiliaryValveRampExtendDown = 0x007F, ///< The valve will apply a ramp to the Auxiliary ValveCommand PortFlowCommand, to limit the acceleration or deceleration of flow. The valve must apply the ramp to create a linear increase/decrease of flow over time. + AuxiliaryValveRampRetractUp = 0x0080, ///< The valve will apply a ramp to theAuxiliary ValveCommand PortFlowCommand, to limit the acceleration or deceleration of flow. The valve must apply the ramp to create a linear increase/decrease of flow over time. + AuxiliaryValveRampRetractDown = 0x0081, ///< The valve will apply a ramp to the Auxiliary ValveCommand PortFlowCommand, to limit the acceleration or deceleration of flow. The valve must apply the ramp to create a linear increase/decrease of flow over time. + AuxiliaryValveFloatThreshold = 0x0082, ///< Safety function. Current output of valve must be above threshold before float command is allowed. + AuxiliaryValveProgressivityExtend = 0x0083, ///< Define non-linear relationship between command and flow by 2nd degree polynomial + AuxiliaryValveProgressivityRetract = 0x0084, ///< Define non-linear relationship between command and flow by 2nd degree polynomial + AuxiliaryValveInvertPorts = 0x0085, ///< Tell valve to swap extend and retract ports, easier than redoing plumbing on valve + DeviceElementOffsetX = 0x0086, ///< X direction offset of a DeviceElement relative to a Device. + DeviceElementOffsetY = 0x0087, ///< Y direction offset of a DeviceElement relative to a Device. + DeviceElementOffsetZ = 0x0088, ///< Z direction offset of a DeviceElement relative to a Device. + DeviceVolumeCapacity = 0x0089, ///< DeviceElement Volume Capacity, dimension of a DeviceElement + DeviceMassCapacity = 0x008A, ///< DeviceElement Mass Capacity, dimension of a DeviceElement + DeviceCountCapacity = 0x008B, ///< DeviceElement Count Capacity, dimension of a DeviceElement + SetpointPercentageApplicationRate = 0x008C, ///< Application Rate expressed as percentage + ActualWorkState = 0x008D, ///< Actual Work State, 2 bits defined as 00=disabled/off, 01=enabled/on, 10=error, 11=undefined/not installed + PhysicalSetpointTimeLatency = 0x008E, ///< The Setpoint Value Latency Time is the time lapse between the moment of reception of a setpoint value command by the working set and the moment this setpoint value is physically applied on the device. + PhysicalActualValueTimeLatency = 0x008F, ///< The Actual Value Latency Time is the time lapse between the moment this actual value is communicated to the Task Controller, and the moment that this actual value is physically applied on the device. + YawAngle = 0x0090, ///< Pivot / Yaw Angle of a DeviceElement + RollAngle = 0x0091, ///< Roll Angle of a DeviceElement + PitchAngle = 0x0092, ///< Pitch Angle of a DeviceElement + LogCount = 0x0093, ///< Log Counter, may be used to control data log record generation on a Task Controller + TotalFuelConsumption = 0x0094, ///< Accumulated Fuel Consumption as Counter + InstantaneousFuelConsumptionPerTime = 0x0095, ///< Fuel consumption per time + InstantaneousFuelConsumptionPerArea = 0x0096, ///< Fuel consumption per area + InstantaneousAreaPerTimeCapacity = 0x0097, ///< Area per time capacity + ActualNormalizedDifferenceVegetativeIndexNDVI = 0x0099, ///< The Normalized Difference Vegetative Index (NDVI) computed from crop reflectances + PhysicalObjectLength = 0x009A, ///< Length of device element (dimension along the X-axis) + PhysicalObjectWidth = 0x009B, ///< Width of device element (dimension along the Y-axis) + PhysicalObjectHeight = 0x009C, ///< Height of device element (dimension along the Z-axis) + ConnectorType = 0x009D, ///< Specification of the type of coupler. See isobus.net for the enumerated values + PrescriptionControlState = 0x009E, ///< Defines and synchronizes the actual state of the prescription system + NumberOfSubUnitsPerSection = 0x009F, ///< Specifies the number of sub-units for a section (e.g. number of nozzles per sprayer section or number of planter row units per metering device). + SectionControlState = 0x00A0, ///< Specifies the actual state of section control + ActualCondensedWorkState1_16 = 0x00A1, ///< Combination of the actual work states of individual sections or units (e.g. nozzles) number 1 to 16 into a single actual work state of their parent device element. + ActualCondensedWorkState17_32 = 0x00A2, ///< Combination of the actual work states of individual sections or units (e.g. nozzles) number 17 to 32 into a single actual work state of their parent device element + ActualCondensedWorkState33_48 = 0x00A3, ///< Combination of the actual work states of individual sections or units (e.g. nozzles) number 33 to 48 into a single actual work state of their parent device element + ActualCondensedWorkState49_64 = 0x00A4, ///< Combination of the actual work states of individual sections or units (e.g. nozzles) number 49 to 64 into a single actual work state of their parent device element + ActualCondensedWorkState65_80 = 0x00A5, ///< Combination of the actual work states of individual sections or units (e.g. nozzles) number 65 to 80 into a single actual work state of their parent device element. + ActualCondensedWorkState81_96 = 0x00A6, ///< Combination of the actual work states of individual sections or units (e.g. nozzles) number 81 to 96 into a single actual work state of their parent device element. + ActualCondensedWorkState97_112 = 0x00A7, ///< Combination of the actual work states of individual sections or units (e.g. nozzles) number 97 to 112 into a single actual work state of their parent device element. + ActualCondensedWorkState113_128 = 0x00A8, ///< Combination of the actual work states of individual sections or units (e.g. nozzles) number 113 to 128 into a single actual work state of their parent device element. + ActualCondensedWorkState129_144 = 0x00A9, ///< Combination of the actual work states of individual sections or units (e.g. nozzles) number 129 to 144 into a single actual work state of their parent device element. + ActualCondensedWorkState145_160 = 0x00AA, ///< Combination of the actual work states of individual sections or units (e.g. nozzles) number 145 to 160 into a single actual work state of their parent device element. + ActualCondensedWorkState161_176 = 0x00AB, ///< Combination of the actual work states of individual sections or units (e.g. nozzles) number 161 to 176 into a single actual work state of their parent device element. + ActualCondensedWorkState177_192 = 0x00AC, ///< Combination of the actual work states of individual sections or units (e.g. nozzles) number 177 to 192 into a single actual work state of their parent device element. + ActualCondensedWorkState193_208 = 0x00AD, ///< Combination of the actual work states of individual sections or units (e.g. nozzles) number 193 to 208 into a single actual work state of their parent device element. + ActualCondensedWorkState209_224 = 0x00AE, ///< Combination of the actual work states of individual sections or units (e.g. nozzles) number 209 to 224 into a single actual work state of their parent device element. + ActualCondensedWorkState225_240 = 0x00AF, ///< Combination of the actual work states of individual sections or units (e.g. nozzles) number 225 to 240 into a single actual work state of their parent device element. + ActualCondensedWorkState241_256 = 0x00B0, ///< Combination of the actual work states of individual sections or units (e.g. nozzles) number 241 to 256 into a single actual work state of their parent device element. + ActualLengthOfCut = 0x00B1, ///< Actual length of cut for harvested material, e.g. Forage Harvester or Tree Harvester. + ElementTypeInstance = 0x00B2, ///< This DDI is used to enumerate and identify multiple device elements (DET) of the same type within one Device Description object pool. The value of this DDI is independent of the DET number. + ActualCulturalPractice = 0x00B3, ///< This DDI is used to define the current cultural practice which is performed by an individual device operation. For instance a planter/seeder could provide a sowing and a fertilizing operation at the same time. + DeviceReferencePointDRPtoGroundDistance = 0x00B4, ///< This DDI is used to specify the distance from the Device Reference Point (DRP) down to the ground surface + DryMassPerAreaYield = 0x00B5, ///< Actual Dry Mass Per Area Yield. The definition of dry mass is the mass with a reference moisture specified by DDI 184. + DryMassPerTimeYield = 0x00B6, ///< Actual Dry Mass Per Time Yield. The definition of dry mass is the mass with a reference moisture specified by DDI 184. + YieldTotalDryMass = 0x00B7, ///< Accumulated Yield specified as dry mass. The definition of dry mass is the mass with a reference moisture specified by DDI 184. + ReferenceMoistureForDryMass = 0x00B8, ///< Moisture percentage used for the dry mass DDIs 181, 182 and 183. + SeedCottonMassPerAreaYield = 0x00B9, ///< Seed cotton yield as mass per area, not corrected for a possibly included lint percentage. + LintCottonMassPerAreaYield = 0x00BA, ///< Lint cotton yield as mass per area. + SeedCottonMassPerTimeYield = 0x00BB, ///< Seed cotton yield as mass per time, not corrected for a possibly included lint percentage. + LintCottonMassPerTimeYield = 0x00BC, ///< Lint cotton yield as mass per time. + YieldTotalSeedCottonMass = 0x00BD, ///< Accumulated yield specified as seed cotton mass, not corrected for a possibly included lint percentage. + YieldTotalLintCottonMass = 0x00BE, ///< Accumulated yield specified as lint cotton mass. + LintTurnoutPercentage = 0x00BF, ///< Percent of lint in the seed cotton. + AmbientTemperature = 0x00C0, ///< Ambient temperature measured by a machine. Unit is milli-Kelvin (mK). + SetpointProductPressure = 0x00C1, ///< Setpoint Product Pressure to adjust the pressure of the product flow system at the point of dispensing. + ActualProductPressure = 0x00C2, ///< Actual Product Pressure is the measured pressure in the product flow system at the point of dispensing. + MinimumProductPressure = 0x00C3, ///< Minimum Product Pressure in the product flow system at the point of dispensing. + MaximumProductPressure = 0x00C4, ///< Maximum Product Pressure in the product flow system at the point of dispensing. + SetpointPumpOutputPressure = 0x00C5, ///< Setpoint Pump Output Pressure to adjust the pressure at the output of the solution pump. + ActualPumpOutputPressure = 0x00C6, ///< Actual Pump Output Pressure measured at the output of the solution pump. + MinimumPumpOutputPressure = 0x00C7, ///< Minimum Pump Output Pressure for the output pressure of the solution pump. + MaximumPumpOutputPressure = 0x00C8, ///< Maximum Pump Output Pressure for the output pressure of the solution pump. + SetpointTankAgitationPressure = 0x00C9, ///< Setpoint Tank Agitation Pressure to adjust the pressure for a stir system in a tank. + ActualTankAgitationPressure = 0x00CA, ///< Actual Tank Agitation Pressure measured by the tank stir system. + MinimumTankAgitationPressure = 0x00CB, ///< Minimum Tank Agitation Pressure for a stir system in a tank. + MaximumTankAgitationPressure = 0x00CC, ///< Maximum Tank Agitation Pressure for a stir system in a tank. + SCTurnOnTime = 0x00CD, ///< The Section Control Turn On Time defines the overall time lapse between the moment the TC sends a turn on section command to the working set and the moment this section is physically turned on and the product is applied. + SCTurnOffTime = 0x00CE, ///< The Section Control Turn Off Time defines the overall time lapse between the moment the TC sends a turn off section command to the working set and the moment this section is physically turned off. + WindSpeed = 0x00CF, ///< Wind speed measured in the treated field at the beginning of operations or on the application implement during operations + WindDirection = 0x00D0, ///< Wind direction measured in the treated field at the beginning of operations or on the application implement during operations. + AirHumidity = 0x00D1, ///< Ambient humidty measured by a weather station in a treated field or on the application implement. + SkyConditions = 0x00D2, ///< This DDE is used to define the current sky conditions during operation. The METAR format and its abbreviations is used + LastBaleFlakesPerBale = 0x00D3, ///< The number of flakes in the most recently produced bale. + LastBaleAverageMoisture = 0x00D4, ///< The average moisture in the most recently produced bale. + LastBaleAverageStrokesPerFlake = 0x00D5, ///< The number of baler plunger compression strokes per flake that has entered the bale compression chamber. This value is the average valid for the most recently produced bale. + LifetimeBaleCount = 0x00D6, ///< The number of bales produced by a machine over its entire lifetime. This DDE value can not be set through the process data interface but can be requested and added to a data log. + LifetimeWorkingHours = 0x00D7, ///< The number of working hours of a device element over its entire lifetime. This DDE value can not be set through the process data interface but can be requested and added to a data log. + ActualBaleHydraulicPressure = 0x00D8, ///< The actual value of the hydraulic pressure applied to the sides of the bale in the bale compression chamber. + LastBaleAverageHydraulicPressure = 0x00D9, ///< The average actual value of the hydraulic pressure applied to the sides of the bale in the bale compression chamber. This average is calculated over the most recently produced bale. + SetpointBaleCompressionPlungerLoad = 0x00DA, ///< The setpoint bale compression plunger load as a unitless number. + ActualBaleCompressionPlungerLoad = 0x00DB, ///< The actual bale compression plunger load as a unitless number. + LastBaleAverageBaleCompressionPlungerLoad = 0x00DC, ///< The average bale compression plunger load for the most recently produced bale. + LastBaleAppliedPreservative = 0x00DD, ///< The total preservative applied to the most recently produced bale. + LastBaleTagNumber = 0x00DE, ///< The Last Bale Tag Number as a decimal number in the range of 0 to 4294967295. Note that the value of this DDI has the limitation of being an unsigned 32 bit number. + LastBaleMass = 0x00DF, ///< The mass of the bale that has most recently been produced. + DeltaT = 0x00E0, ///< The difference between dry bulb temperature and wet bulb temperature measured by a weather station in a treated field or on the application equipment. + SetpointWorkingLength = 0x00E1, ///< Setpoint Working Length of Device Element. + ActualWorkingLength = 0x00E2, ///< Actual Working Length of a Device Element. + MinimumWorkingLength = 0x00E3, ///< Minimum Working Length of Device Element. + MaximumWorkingLength = 0x00E4, ///< Maximum Working Length of Device Element. + ActualNetWeight = 0x00E5, ///< Actual Net Weight value specified as mass + NetWeightState = 0x00E6, ///< Net Weight State + SetpointNetWeight = 0x00E7, ///< Setpoint Net Weight value. + ActualGrossWeight = 0x00E8, ///< Actual Gross Weight value specified as mass + GrossWeightState = 0x00E9, ///< Gross Weight State + MinimumGrossWeight = 0x00EA, ///< Minimum Gross Weight specified as mass. + MaximumGrossWeight = 0x00EB, ///< Maximum Gross Weight specified as mass. + ThresherEngagementTotalTime = 0x00EC, ///< Accumulated time while the threshing mechanism is engaged + ActualHeaderWorkingHeightStatus = 0x00ED, ///< Actual status of the header being above or below the threshold height for the in-work state. + ActualHeaderRotationalSpeedStatus = 0x00EE, ///< Actual status of the header rotational speed being above or below the threshold for in-work state. + YieldHoldStatus = 0x00EF, ///< Status indicator for the yield measurement system. When enabled/on, the measurements from the yield measurement system are ignored and the yield is held constant. + ActualUnLoadingSystemStatus = 0x00F0, ///< Actual status of the Unloading and/or Loading system. This DDE covers both Unloading and Loading of the device element wherein it is listed. + CropTemperature = 0x00F1, ///< Temperature of harvested crop + SetpointSieveClearance = 0x00F2, ///< Setpoint separation distance between Sieve elements + ActualSieveClearance = 0x00F3, ///< Actual separation distance between Sieve elements + MinimumSieveClearance = 0x00F4, ///< Minimal separation distance between Sieve elements + MaximumSieveClearance = 0x00F5, ///< Maximum separation distance between Sieve elements. + SetpointChafferClearance = 0x00F6, ///< Setpoint separation distance between Chaffer elements. + ActualChafferClearance = 0x00F7, ///< Actual separation distance between Chaffer elements. + MinimumChafferClearance = 0x00F8, ///< Minimum separation distance between Chaffer elements. + MaximumChafferClearance = 0x00F9, ///< Maximum separation distance between Chaffer elements. + SetpointConcaveClearance = 0x00FA, ///< Setpoint separation distance between Concave elements. + ActualConcaveClearance = 0x00FB, ///< Actual separation distance between Concave elements. + MinimumConcaveClearance = 0x00FC, ///< Minimum separation distance between Concave elements. + MaximumConcaveClearance = 0x00FD, ///< Maximum separation distance between Concave elements. + SetpointSeparationFanRotationalSpeed = 0x00FE, ///< Setpoint rotational speed of the fan used for separating product material from non product material. + ActualSeparationFanRotationalSpeed = 0x00FF, ///< Actual rotational speed of the fan used for separating product material from non product material. + MinimumSeparationFanRotationalSpeed = 0x0100, ///< Minimum rotational speed of the fan used for separating product material from non product material. + MaximumSeparationFanRotationalSpeed = 0x0101, ///< Maximum rotational speed of the fan used for separating product material from non product material. + HydraulicOilTemperature = 0x0102, ///< Temperature of fluid in the hydraulic system. + YieldLagIgnoreTime = 0x0103, ///< Amount of time to ignore yield data, starting at the transition from the in-work to the out-of-work state. During this time, the yield sensor provides inconsistent or unreliable crop flow data. + YieldLeadIgnoreTime = 0x0104, ///< Amount of time to ignore yield data, starting at the transition from the out-of-work to the in-work state. During this time, the yield sensor provides inconsistent or unreliable crop flow data. + AverageYieldMassPerTime = 0x0105, ///< Average Yield expressed as mass per unit time, not corrected for the reference moisture percentage DDI 184. This value is the average for a Task and may be reported as a total. + AverageCropMoisture = 0x0106, ///< Average Moisture of the harvested crop. This value is the average for a Task and may be reported as a total. + AverageYieldMassPerArea = 0x0107, ///< Average Yield expressed as mass per unit area, not corrected for the reference moisture percentage DDI 184. This value is the average for a Task and may be reported as a total. + ConnectorPivotXOffset = 0x0108, ///< X direction offset of a connector pivot point relative to DRP. + RemainingArea = 0x0109, ///< Remaining Area of a field, which is calculated from the total area and the processed area. + LifetimeApplicationTotalMass = 0x010A, ///< Entire Application Total Mass of the device lifetime. + LifetimeApplicationTotalCount = 0x010B, ///< Entire Application Total Count of the device lifetime. + LifetimeYieldTotalVolume = 0x010C, ///< Entire Yield Total Volume of the device lifetime. + LifetimeYieldTotalMass = 0x010D, ///< Entire Yield Total Mass of the device lifetime. + LifetimeYieldTotalCount = 0x010E, ///< Entire Yield Total Count of the device lifetime. + LifetimeTotalArea = 0x010F, ///< Entire Total Area of the device lifetime. + LifetimeEffectiveTotalDistance = 0x0110, ///< Entire Total Distance of the device lifetime. + LifetimeIneffectiveTotalDistance = 0x0111, ///< Entire Ineffective Total Distance of the device lifetime. + LifetimeEffectiveTotalTime = 0x0112, ///< Entire Effective Total Time of the device lifetime. + LifetimeIneffectiveTotalTime = 0x0113, ///< Entire Ineffective Total Time of the device lifetime. + LifetimeFuelConsumption = 0x0114, ///< Entire Fuel Consumption of the device lifetime. + LifetimeAverageFuelConsumptionPerTime = 0x0115, ///< Entire Average Fuel Consumption per Time of the device lifetime. + LifetimeAverageFuelConsumptionPerArea = 0x0116, ///< Entire Average Fuel Consumption per Area of the device lifetime. + LifetimeYieldTotalDryMass = 0x0117, ///< Entire Yield Total Dry Mass of the device lifetime. + LifetimeYieldTotalSeedCottonMass = 0x0118, ///< Entire Yield Total Seed Cotton Mass of the device lifetime. + LifetimeYieldTotalLintCottonMass = 0x0119, ///< Entire Yield Total Lint Cotton Mass of the device lifetime. + LifetimeThreshingEngagementTotalTime = 0x011A, ///< Entire Threshing Engagement Total Time of the device lifetime. + PrecutTotalCount = 0x011B, ///< The total number of pre-cut product units produced by a device during an operation. + UncutTotalCount = 0x011C, ///< The total number of un-cut product units produced by a device during an operation. + LifetimePrecutTotalCount = 0x011D, ///< Entire Precut Total Count of the device lifetime. + LifetimeUncutTotalCount = 0x011E, ///< Entire Uncut Total Count of the device lifetime. + SetpointPrescriptionMode = 0x011F, ///< This DDE defines the source of the Task Controller set point value sent to the Control Function. + ActualPrescriptionMode = 0x0120, ///< This DDE defines the actual source of the set point value used by the Control Function. + SetpointWorkState = 0x0121, ///< The Setpoint Work State DDI is the control command counterparts to the Work State DDI (141). + SetpointCondensedWorkState1_16 = 0x0122, ///< The value is a combination of the setpoint work states of individual sections or units (e.g. nozzles) number 1 to 16 into a single setpoint work state of their parent device element. + SetpointCondensedWorkState17_32 = 0x0123, ///< The value is a combination of the setpoint work states of individual sections or units (e.g. nozzles) number 17 to 32 into a single setpoint work state of their parent device element. + SetpointCondensedWorkState33_48 = 0x0124, ///< The value is a combination of the setpoint work states of individual sections or units (e.g. nozzles) number 33 to 48 into a single setpoint work state of their parent device element. + SetpointCondensedWorkState49_64 = 0x0125, ///< The value is a combination of the setpoint work states of individual sections or units (e.g. nozzles) number 49 to 64 into a single setpoint work state of their parent device element. + SetpointCondensedWorkState65_80 = 0x0126, ///< The value is a combination of the setpoint work states of individual sections or units (e.g. nozzles) number 65 to 80 into a single setpoint work state of their parent device element. + SetpointCondensedWorkState81_96 = 0x0127, ///< The value is a combination of the setpoint work states of individual sections or units (e.g. nozzles) number 81 to 96 into a single setpoint work state of their parent device element. + SetpointCondensedWorkState97_112 = 0x0128, ///< The value is a combination of the setpoint work states of individual sections or units (e.g. nozzles) number 97 to 112 into a single setpoint work state of their parent device element. + SetpointCondensedWorkState113_128 = 0x0129, ///< The value is a combination of the setpoint work states of individual sections or units (e.g. nozzles) number 113 to 128 into a single setpoint work state of their parent device element. + SetpointCondensedWorkState129_144 = 0x012A, ///< The value is a combination of the setpoint work states of individual sections or units (e.g. nozzles) number 129 to 144 into a single setpoint work state of their parent device element. + SetpointCondensedWorkState145_160 = 0x012B, ///< The value is a combination of the setpoint work states of individual sections or units (e.g. nozzles) number 145 to 160 into a single setpoint work state of their parent device element. + SetpointCondensedWorkState161_176 = 0x012C, ///< The value is a combination of the setpoint work states of individual sections or units (e.g. nozzles) number 161 to 176 into a single setpoint work state of their parent device element. + SetpointCondensedWorkState177_192 = 0x012D, ///< The value is a combination of the setpoint work states of individual sections or units (e.g. nozzles) number 177 to 192 into a single setpoint work state of their parent device element. + SetpointCondensedWorkState193_208 = 0x012E, ///< The value is a combination of the setpoint work states of individual sections or units (e.g. nozzles) number 193 to 208 into a single setpoint work state of their parent device element. + SetpointCondensedWorkState209_224 = 0x012F, ///< The value is a combination of the setpoint work states of individual sections or units (e.g. nozzles) number 209 to 224 into a single setpoint work state of their parent device element. + SetpointCondensedWorkState225_240 = 0x0130, ///< The value is a combination of the setpoint work states of individual sections or units (e.g. nozzles) number 225 to 240 into a single setpoint work state of their parent device element. + SetpointCondensedWorkState241_256 = 0x0131, ///< The value is a combination of the setpoint work states of individual sections or units (e.g. nozzles) number 241 to 256 into a single setpoint work state of their parent device element. + TrueRotationPointXOffset = 0x0132, ///< X direction offset of the device rotation point relative to the DRP. + TrueRotationPointYOffset = 0x0133, ///< Y direction offset of the device rotation point relative to the DRP. + ActualPercentageApplicationRate = 0x0134, ///< Actual Application Rate expressed as percentage + MinimumPercentageApplicationRate = 0x0135, ///< Minimum Application Rate expressed as percentage + MaximumPercentageApplicationRate = 0x0136, ///< Maximum Application Rate expressed as percentage + RelativeYieldPotential = 0x0137, ///< Relative yield potential provided by a FMIS or a sensor or entered by the operator for a certain task expressed as percentage. + MinimumRelativeYieldPotential = 0x0138, ///< Minimum potential yield expressed as percentage. + MaximumRelativeYieldPotential = 0x0139, ///< Maximum potential yield expressed as percentage. + ActualPercentageCropDryMatter = 0x013A, ///< Actual Percentage Crop Dry Matter expressed as parts per million. + AveragePercentageCropDryMatter = 0x013B, ///< Average Percentage Crop Dry Matter expressed as parts per million. + EffectiveTotalFuelConsumption = 0x013C, ///< Accumulated total fuel consumption in working position. + IneffectiveTotalFuelConsumption = 0x013D, ///< Accumulated total fuel consumption in non working position. + EffectiveTotalDieselExhaustFluidConsumption = 0x013E, ///< Accumulated total Diesel Exhaust Fluid consumption in working position. + IneffectiveTotalDieselExhaustFluidConsumption = 0x013F, ///< Accumulated total Diesel Exhaust Fluid consumption in non working position. + LastLoadedWeight = 0x0140, ///< Last loaded Weight value specified as mass + LastUnloadedWeight = 0x0141, ///< Last unloaded Weight value specified as mass + LoadIdentificationNumber = 0x0142, ///< The Load Identification Number as a decimal number in the range of 0 to 4294967295. Note that the value of this DDI has the limitation of being an unsigned 32 bit number. + UnloadIdentificationNumber = 0x0143, ///< The Unload Identification Number as a decimal number in the range of 0 to 2147483647. Note that the value of this DDI has the limitation of being an unsigned 32 bit number. + ChopperEngagementTotalTime = 0x0144, ///< Accumulated time while the chopping mechanism is engaged + LifetimeApplicationTotalVolume = 0x0145, ///< Entire Application Total Volume of the device lifetime. + SetpointHeaderSpeed = 0x0146, ///< The setpoint rotational speed of the header attachment of a chopper, mower or combine + ActualHeaderSpeed = 0x0147, ///< The actual rotational speed of the header attachment of a chopper, mower or combine + MinimumHeaderSpeed = 0x0148, ///< The minimum rotational speed of the header attachment of a chopper, mower or combine + MaximumHeaderSpeed = 0x0149, ///< The maximum rotational speed of the header attachment of a chopper, mower or combine + SetpointCuttingDrumSpeed = 0x014A, ///< The setpoint speed of the cutting drum of a chopper + ActualCuttingDrumSpeed = 0x014B, ///< The actual speed of the cutting drum of a chopper + MinimumCuttingDrumSpeed = 0x014C, ///< The minimum speed of the cutting drum of a chopper + MaximumCuttingDrumSpeed = 0x014D, ///< The maximum speed of the cutting drum of a chopper + OperatingHoursSinceLastSharpening = 0x014E, ///< This value describes the working hours since the last sharpening of the cutting device. + FrontPTOhours = 0x014F, ///< The hours the Front PTO of the machine was running for the current Task + RearPTOhours = 0x0150, ///< The hours the Rear PTO of the machine was running for the current Task + LifetimeFrontPTOhours = 0x0151, ///< The hours the Front PTO of the machine was running for the lifetime of the machine + LifetimeRearPTOHours = 0x0152, ///< The hours the Rear PTO of the machine was running for the lifetime of the machine + EffectiveTotalLoadingTime = 0x0153, ///< The total time needed in the current task to load a product such as crop. + EffectiveTotalUnloadingTime = 0x0154, ///< The total time needed in the current task to unload a product crop. + SetpointGrainKernelCrackerGap = 0x0155, ///< The setpoint gap (distance) of the grain kernel cracker drums in a chopper. + ActualGrainKernelCrackerGap = 0x0156, ///< The actual gap (distance) of the grain kernel cracker drums in a chopper + MinimumGrainKernelCrackerGap = 0x0157, ///< The minimum gap (distance) of the grain kernel cracker drums in a chopper + MaximumGrainKernelCrackerGap = 0x0158, ///< The maximum gap (distance) of the grain kernel cracker drums in a chopper + SetpointSwathingWidth = 0x0159, ///< This is the setpoint swathing width of the swath created by a raker. + ActualSwathingWidth = 0x015A, ///< This is the width of the swath currently created by a raker. + MinimumSwathingWidth = 0x015B, ///< This is the minimum swath width the raker can create. + MaximumSwathingWidth = 0x015C, ///< This is the maximum with of the swath the raker can create. + NozzleDriftReduction = 0x015D, ///< The Nozzle Drift Reduction classification value of the spraying equipment as percentage + FunctionOrOperationTechnique = 0x015E, ///< The Function or Operation Technique DDE can be used to define the operation technique or functionality performed by a device element defined within the DDOP + ApplicationTotalVolume_ml = 0x015F, ///< Accumulated Application specified as volume in milliliter [ml] + ApplicationTotalMassGram_g = 0x0160, ///< Accumulated Application specified as mass in gram [g] + TotalApplicationOfNitrogen = 0x0161, ///< Accumulated application of nitrogen [N2] specified as gram [g] + TotalApplicationOfAmmonium = 0x0162, ///< Accumulated application of ammonium [NH4] specified as gram [g] + TotalApplicationOfPhosphor = 0x0163, ///< Accumulated application of phosphor (P2O5) specified as gram [g] + TotalApplicationOfPotassium = 0x0164, ///< Accumulated application of potassium (K2) specified as gram [g] + TotalApplicationOfDryMatter = 0x0165, ///< Accumulated application of dry matter in kilogram [kg]. Dry matter measured at zero percent of moisture + AverageDryYieldMassPerTime = 0x0166, ///< Average Yield expressed as mass per unit time, corrected for the reference moisture percentage DDI 184. This value is the average for a Task and may be reported as a total. + AverageDryYieldMassPerArea = 0x0167, ///< Average Yield expressed as mass per unit area, corrected for the reference moisture percentage DDI 184. This value is the average for a Task and may be reported as a total + LastBaleSize = 0x0168, ///< The bale size of the most recently produced bale. Bale Size as length for a square baler or diameter for a round baler. + LastBaleDensity = 0x0169, ///< The bale density of the most recently produced bale. Unit : mg / l(mass per unit volume) + TotalBaleLength = 0x016A, ///< Gives the total baled meters during a task. This is calculated as the sum of the lengths of all knotted bales (square baler). + LastBaleDryMass = 0x016B, ///< The dry mass of the bale that has most recently been produced. This is the bale mass corrected for the average moisture of this bale (DDI 212). + ActualFlakeSize = 0x016C, ///< Actual size of the flake that is currently produced by the chamber. + SetpointDownForcePressure = 0x016D, ///< Setpoint downforce pressure for an operation + ActualDownForcePressure = 0x016E, ///< Actual downforce pressure for an operation + CondensedSectionOverrideState1_16 = 0x016F, ///< This DDE is used by the implement to communicate that a certain section is overridden and will not follow the section control commands. + CondensedSectionOverrideState17_32 = 0x0170, ///< This DDE is used by the implement to communicate that a certain section is overridden and will not follow the section control commands. + CondensedSectionOverrideState33_48 = 0x0171, ///< This DDE is used by the implement to communicate that a certain section is overridden and will not follow the section control commands + CondensedSectionOverrideState49_64 = 0x0172, ///< This DDE is used by the implement to communicate that a certain section is overridden and will not follow the section control commands + CondensedSectionOverrideState65_80 = 0x0173, ///< This DDE is used by the implement to communicate that a certain section is overridden and will not follow the section control commands + CondensedSectionOverrideState81_96 = 0x0174, ///< This DDE is used by the implement to communicate that a certain section is overridden and will not follow the section control commands + CondensedSectionOverrideState97_112 = 0x0175, ///< This DDE is used by the implement to communicate that a certain section is overridden and will not follow the section control commands + CondensedSectionOverrideState113_128 = 0x0176, ///< This DDE is used by the implement to communicate that a certain section is overridden and will not follow the section control commands + CondensedSectionOverrideState129_144 = 0x0177, ///< This DDE is used by the implement to communicate that a certain section is overridden and will not follow the section control commands + CondensedSectionOverrideState145_160 = 0x0178, ///< This DDE is used by the implement to communicate that a certain section is overridden and will not follow the section control commands + CondensedSectionOverrideState161_176 = 0x0179, ///< This DDE is used by the implement to communicate that a certain section is overridden and will not follow the section control commands + CondensedSectionOverrideState177_192 = 0x017A, ///< This DDE is used by the implement to communicate that a certain section is overridden and will not follow the section control commands + CondensedSectionOverrideState193_208 = 0x017B, ///< This DDE is used by the implement to communicate that a certain section is overridden and will not follow the section control commands + CondensedSectionOverrideState209_224 = 0x017C, ///< This DDE is used by the implement to communicate that a certain section is overridden and will not follow the section control commands + CondensedSectionOverrideState225_240 = 0x017D, ///< This DDE is used by the implement to communicate that a certain section is overridden and will not follow the section control commands + CondensedSectionOverrideState241_256 = 0x017E, ///< This DDE is used by the implement to communicate that a certain section is overridden and will not follow the section control commands + ApparentWindDirection = 0x017F, ///< The apparent wind is the wind which is measured on a moving vehicle. It is the result of two motions: the actual true wind and the motion of the vehicle. + ApparentWindSpeed = 0x0180, ///< The apparent wind is the wind which is measured on a moving vehicle. It is the result of two motions: the actual true wind and the motion of the vehicle. + MSLAtmosphericPressure = 0x0181, ///< The atmospheric pressure MSL (Mean Sea Level) is the air pressure related to mean sea level. + ActualAtmosphericPressure = 0x0182, ///< The Actual Atmospheric Pressure is the air pressure currently measured by the weather station. + TotalRevolutionsInFractionalRevolutions = 0x0183, ///< Accumulated Revolutions specified with fractional revolutions + TotalRevolutionsInCompleteRevolutions = 0x0184, ///< Accumulated Revolutions specified as completed integer revolutions + SetpointRevolutionsSpecifiedAsCountPerTime = 0x0185, ///< Setpoint Revolutions specified as count per time + ActualRevolutionsPerTime = 0x0186, ///< Actual Revolutions specified as count per time + DefaultRevolutionsPerTime = 0x0187, ///< Default Revolutions specified as count per time + MinimumRevolutionsPerTime = 0x0188, ///< Minimum Revolutions specified as count per time + MaximumRevolutionsPerTime = 0x0189, ///< Maximum Revolutions specified as count per time + ActualFuelTankContent = 0x018A, ///< The actual content of the fuel tank + ActualDieselExhaustFluidTankContent = 0x018B, ///< The actual content of the diesel exhaust fluid tank + SetpointSpeed = 0x018C, ///< The setpoint speed that can be specified in a process data variable for communication between farm management information systems and mobile implement control systems. + ActualSpeed = 0x018D, ///< The actual speed as measured on or used by a device for the execution of task based data, e.g. to convert a setpoint rate expressed per area to device specific control data that is expressed as a rate per time. + MinimumSpeed = 0x018E, ///< The minimum speed that can be specified in a process data variable for communication between farm management information systems and mobile implement control systems + MaximumSpeed = 0x018F, ///< The maximum speed that can be specified in a process data variable for communication between farm management information systems and mobile implement control systems. + SpeedSource = 0x0190, ///< The Speed Source that the device uses to report actual speed and to process the setpoint, minimum and maximum speeds. See isobus.net + ActualApplicationOfNitrogen = 0x0191, ///< Actual application of Nitrogen [N2] specified as milligram per liter [mg/l] + ActualApplicationOfAmmonium = 0x0192, ///< Actual application of Ammonium [NH4] specified as milligram per liter [mg/l] + ActualApplicationOfPhosphor = 0x0193, ///< Actual application of Phosphor [P2O5] specified as milligram per liter [mg/l] + ActualApplicationOfPotassium = 0x0194, ///< Actual application of Potassium [K2] specified as gram [g] + ActualApplicationOfDryMatter = 0x0195, ///< Actual application of Dry Matter in kilogram [kg]. Dry matter measured at Zero percent of moisture. + ActualProteinContent = 0x0196, ///< Actual Protein content of a harvested crops + AverageProteinContent = 0x0197, ///< Average protein content in a harvested crop + AverageCropContamination = 0x0198, ///< Average amount of dirt or foreign in a harvested crop + TotalDieselExhaustFluidConsumption = 0x0199, ///< Accumulated Diesel Exhaust Fluid Consumption as a Task Total. + InstantaneousDieselExhaustFluidConsumptionperTime = 0x019A, ///< Diesel Exhaust Fluid consumption per time + InstantaneousDieselExhaustFluidConsumptionperArea = 0x019B, ///< Diesel Exhaust Fluid consumption per area + LifetimeDieselExhaustFluidConsumption = 0x019C, ///< Accumulated Diesel Exhaust Fluid Consumption over the entire lifetime of the device. + LifetimeAverageDieselExhaustFluidConsumptionperTime = 0x019D, ///< Average Diesel Exhaust Fluid Consumption per Time over the entire lifetime of the device. + LifetimeAverageDieselExhaustFluidConsumptionperArea = 0x019E, ///< Average Diesel Exhaust Fluid Consumption per Area over the entire lifetime of the device. + ActualSeedSingulationPercentage = 0x019F, ///< Actual Seed Singulation Percentage calculated from measured seed spacing using ISO 7256-1 "Quality of Feed Index" algorithm + AverageSeedSingulationPercentage = 0x01A0, ///< Average Seed Singulation Percentage calculated from measured seed spacing using ISO 7256-1 "Quality of Feed Index" algorithm. The value is the average for a Task. + ActualSeedSkipPercentage = 0x01A1, ///< Actual Seed Skip Percentage calculated from measured seed spacing using ISO 7256-1 "Miss Index" algorithm + AverageSeedSkipPercentage = 0x01A2, ///< Average Seed Skip Percentage calculated from measured seed spacing using ISO 7256-1 "Miss Index" algorithm. The value is the average for a Task. + ActualSeedMultiplePercentage = 0x01A3, ///< Actual Seed Multiple Percentage calculated from measured seed spacing using ISO 7256-1 "Multiples Index" algorithm. + AverageSeedMultiplePercentage = 0x01A4, ///< Average Seed Multiple Percentage calculated from measured seed spacing using ISO 7256-1 "Multiples Index" algorithm. The value is the average for a Task. + ActualSeedSpacingDeviation = 0x01A5, ///< Actual Seed Spacing Deviation from setpoint seed spacing + AverageSeedSpacingDeviation = 0x01A6, ///< Average Seed Spacing Deviation from setpoint seed spacing. The value is the average for a Task. + ActualCoefficientofVariationofSeedSpacingPercentage = 0x01A7, ///< Actual Coefficient of Variation of Seed Spacing Percentage calculated from measured seed spacing using ISO 7256-1 algorithm + AverageCoefficientofVariationofSeedSpacingPercentage = 0x01A8, ///< Average Coefficient of Variation of Seed Spacing Percentage calculated from measured seed spacing using ISO 7256-1 algorithm. The value is the average for a Task. + SetpointMaximumAllowedSeedSpacingDeviation = 0x01A9, ///< Setpoint Maximum Allowed Seed Spacing Deviation + SetpointDownforceasForce = 0x01AA, ///< Setpoint Downforce as Force + ActualDownForceAsForce = 0x01AB, ///< Actual Downforce as Force + LoadedTotalMass = 0x01AC, ///< Accumulated Loads specified as mass, not corrected for the reference moisture percentage DDI 184. + UnloadedTotalMass = 0x01AD, ///< Accumulated Unloads specified as mass, not corrected for the reference moisture percentage DDI 184. + LifetimeLoadedTotalMass = 0x01AE, ///< Entire Yield Total Mass of the device lifetime. + LifetimeUnloadedTotalMass = 0x01AF, ///< Entire Unloaded Total Mass of the device lifetime. + SetpointApplicationRateOfNitrogen = 0x01B0, ///< Setpoint application rate of nitrogen specified as a mass per area + ActualApplicationRateOfNitrogen = 0x01B1, ///< Actual application rate of nitrogen specified as a mass per area + MinimumApplicationRateOfNitrogen = 0x01B2, ///< Minimum application rate of nitrogen specified as a mass per area + MaximumApplicationRateOfNitrogen = 0x01B3, ///< Maximum application rate of nitrogen specified as a mass per area + SetpointApplicationRateOfAmmonium = 0x01B4, ///< Setpoint application rate of Ammonium specified as a mass per area + ActualApplicationRateOfAmmonium = 0x01B5, ///< Actual application rate of Ammonium specified as a mass per area + MinimumApplicationRateOfAmmonium = 0x01B6, ///< Minimum application rate of Ammonium specified as a mass per area + MaximumApplicationRateOfAmmonium = 0x01B7, ///< Maximum application rate of Ammonium specified as a mass per area + SetpointApplicationRateOfPhosphor = 0x01B8, ///< Setpoint application rate of phosphor specified as a mass per area + ActualApplicationRateOfPhosphor = 0x01B9, ///< Actual application rate of phosphor specified as a mass per area + MinimumApplicationRateOfPhosphor = 0x01BA, ///< Minimum application rate of phosphor specified as a mass per area + MaximumApplicationRateOfPhosphor = 0x01BB, ///< Maximum application rate of phosphor specified as a mass per area + SetpointApplicationRateOfPotassium = 0x01BC, ///< Setpoint application rate of potassium specified as a mass per area + ActualApplicationRateOfPotassium = 0x01BD, ///< Actual application rate of potassium specified as a mass per area + MinimumApplicationRateOfPotassium = 0x01BE, ///< Minimum application rate of potassium specified as a mass per area + MaximumApplicationRateOfPotassium = 0x01BF, ///< Maximum application rate of potassium specified as a mass per area + SetpointApplicationRateOfDryMatter = 0x01C0, ///< Setpoint application rate of dry matter expressed as percentage + ActualApplicationRateOfDryMatter = 0x01C1, ///< Actual application rate of dry matter expressed as percentage + MinimumApplicationRateOfDryMatter = 0x01C2, ///< Minimum application rate of dry matter expressed as percentage + MaximumApplicationRateOfDryMatter = 0x01C3, ///< Maximum application rate of dry matter expressed as percentage + LoadedTotalVolume = 0x01C4, ///< Accumulated Loaded Volume specified as volume + UnloadedTotalVolume = 0x01C5, ///< Accumulated Unloaded Volume specified as volume + LifetimeLoadedTotalVolume = 0x01C6, ///< Entire loaded Volume of the device lifetime. + LifetimeUnloadedTotalVolume = 0x01C7, ///< Entire unloaded Volume of the device lifetime. + LastLoadedVolume = 0x01C8, ///< Last loaded Volume value specified as volume + LastUnloadedVolume = 0x01C9, ///< Last unloaded Volume value specified as volume + LoadedTotalCount = 0x01CA, ///< Accumulated Loads specified as count + UnloadedTotalCount = 0x01CB, ///< Accumulated Unloaded specified as count + LifetimeLoadedTotalCount = 0x01CC, ///< Entire Loaded Total Count of the device lifetime. + LifetimeUnloadedTotalCount = 0x01CD, ///< Entire Unloaded Total Count of the device lifetime. + LastLoadedCount = 0x01CE, ///< Last loaded Count value specified as count + LastUnloadedCount = 0x01CF, ///< Last unloaded Count value specified as count + HaulCounter = 0x01D0, ///< Each Time a Device Element is filled and emptied this is called a haul cycle. This counter counts the cycles + LifetimeHaulCounter = 0x01D1, ///< The number of haul cycles done by a machine over its entire lifetime + ActualRelativeConnectorAngle = 0x01D2, ///< The DDI Actual relative connector angle shall be placed in the device element of type connector in the DDOP of the TC-SC Client. + ActualPercentageContent = 0x01D3, ///< Actual Device Element Content specified as percent. + SetpointLengthOfCut = 0x01D8, ///< Setpoint length of cut for harvested material, e.g. Forage Harvester or Tree Harvester. + MinimumLengthOfCut = 0x01D9, ///< Minimum length of cut for harvested material, e.g. Forage Harvester or Tree Harvester. + MaximumLengthOfCut = 0x01DA, ///< Maximum length of cut for harvested material, e.g. Forage Harvester or Tree Harvester. + SetpointBaleHydraulicPressure = 0x01DB, ///< The setpoint value of the hydraulic pressure applied to the sides of the bale in the bale compression chamber. + MinimumBaleHydraulicPressure = 0x01DC, ///< The minimum value of the hydraulic pressure applied to the sides of the bale in the bale compression chamber. + MaximumBaleHydraulicPressure = 0x01DD, ///< The maximum value of the hydraulic pressure applied to the sides of the bale in the bale compression chamber. + SetpointFlakeSize = 0x01DE, ///< Setpoint size of the flake to be produced by the chamber. + MinimumFlakeSize = 0x01DF, ///< Minimum size of the flake that can be produced by the chamber. + MaximumFlakeSize = 0x01E0, ///< Maximum size of the flake that can be produced by the chamber. + SetpointNumberofSubbales = 0x01E1, ///< Number of smaller bales that shall be included in one bigger bale. + LastBaleNumberofSubbales = 0x01E2, ///< Number of smaller bales included in the latest produced bale. + SetpointEngineSpeed = 0x01E3, ///< The setpoint of the rotational speed of the engine. + ActualEngineSpeed = 0x01E4, ///< Actual rotational speed of the engine. + MinimumEngineSpeed = 0x01E5, ///< The minimum of the rotational speed of the engine. + MaximumEngineSpeed = 0x01E6, ///< The maximum of the rotational speed of the engine. + DieselExhaustFluidTankPercentageLevel = 0x01E8, ///< The actual level of the Diesel Exhaust Fluid Tank in percent. + MaximumDieselExhaustFluidTankContent = 0x01E9, ///< This value describes the maximum amount of Diesel Exhaust fluid, that can be filled into the tank of the machine + MaximumFuelTankContent = 0x01EA, ///< This value describes the maximum amount of fuel that can be filled into the machines Fuel tank. + FuelPercentageLevel = 0x01EB, ///< The actual level of the machine fuel tank in percent. + TotalEngineHours = 0x01EC, ///< The total time the engine was running when the task was active. + LifetimeEngineHours = 0x01ED, ///< The total time, when the engine was running over the whole lifetime of the machine. + LastEventPartnerIDByte1_4 = 0x01EE, ///< Last Event Partner ID as a decimal number of 128bit length. + LastEventPartnerIDByte5_8 = 0x01EF, ///< Last Event Partner ID as a decimal number of 128bit length. + LastEventPartnerIDByte9_12 = 0x01F0, ///< Last Event Partner ID as a decimal number of 128bit length. + LastEventPartnerIDByte13_16 = 0x01F1, ///< Last Event Partner ID as a decimal number of 128bit length. + LastEventPartnerIDType = 0x01F2, ///< Defines The Type of the Partner ID Device. See Attachment for Definition. + LastEventPartnerIDManufacturerIDCode = 0x01F3, ///< The Partner ID has to tell its Manufacturer, and the Manufacturer Numbers from SAE J1939 / ISO 11783 shall be used. + LastEventPartnerIDDeviceClass = 0x01F4, ///< This DDI should tell the Device Class of the Partner Device. + SetpointEngineTorque = 0x01F5, ///< The setpoint of the engine torque. + ActualEngineTorque = 0x01F6, ///< The current torque of the engine. + MinimumEngineTorque = 0x01F7, ///< The minimum value of the engine torque + MaximumEngineTorque = 0x01F8, ///< The maximum value of the engine torque + TramlineControlLevel = 0x01F9, ///< This DDI defines the Tramline Control capability of the Implement. + SetpointTramlineControlLevel = 0x01FA, ///< This DDI defines the Tramline Control capability of the Task Controller that is used with the appropriate Implement. + TramlineSequenceNumber = 0x01FB, ///< This DDI defines a group of DDIs which belong together. + UniqueABGuidanceReferenceLineID = 0x01FC, ///< This DDI defines a unique ID to identify which Guidance Reference Line is the base for the Tramline calculation. + ActualTrackNumber = 0x01FD, ///< This DDI defines a unique number of the Guidance Track the Implement is currently located on. + TrackNumberToTheRight = 0x01FE, ///< This DDI defines a unique number of the Guidance Track to right hand side in direction of Implement orientation. + TrackNumberToTheLeft = 0x01FF, ///< This DDI defines a unique number of the Guidance Track to left hand side in direction of Implement orientation. + GuidanceLineSwathWidth = 0x0200, ///< The Swath Width is the Distance between two adjacent Guidance Lines in a Guidance Pattern. + GuidanceLineDeviation = 0x0201, ///< Deviation of the Device-Reference-Point (DRP) to the actual guidance line, in driving direction. + GNSSQuality = 0x0202, ///< GNSS Quality Identifier to inform the implement about the used Position Status. + TramlineControlState = 0x0203, ///< Specifies the actual state of Tramline Control. See isobus.net + TramlineOverdosingRate = 0x0204, ///< Overdosing Rate for the rows adjacent to the Tramline Track. + SetpointTramlineCondensedWorkState1_16 = 0x0205, ///< The Setpoint Tramline Condensed Work State DDIs are the control command counterparts to the Actual Tramline Condensed Work States DDIs. + ActualTramlineCondensedWorkState1_16 = 0x0206, ///< Combination of the Actual States of individual Tramline Valves number 1 to 16 into a single Actual Tramline State of their parent DeviceElement. + LastBaleLifetimeCount = 0x0207, ///< The Lifetime Bale Count of the bale that leaves the machine. The value shall be equal to the Lifetime Bale Count that was reported when this bale is knotted. + ActualCanopyHeight = 0x0208, ///< Actual height of the canopy above ground. + GNSSInstallationType = 0x0209, ///< The GNSS Installation Type DDE is used by the device to provide additional information about the type and location of the GPS receiver with reference to the overall system. + TwineBaleTotalCount = 0x020A, ///< The total number of twine bound product units for which Twine binding method was used during operation. + MeshBaleTotalCount = 0x020B, ///< The total number of mesh product units for which Net binding method was used during operation. + LifetimeTwineBaleTotalCount = 0x020C, ///< Entire total number of twine bound product units for which Twine binding method was used during operation, of a device lifetime + LifetimeMeshBaleTotalCount = 0x020D, ///< Entire total number of mesh product units for which Net binding method was used during operation, of a device lifetime + ActualCoolingFluidTemperature = 0x020E, ///< The actual temperature of the cooling fluid for the machine. + LastBaleCapacity = 0x0210, ///< The capacity of the bale that leaves the machine. + PGNBasedData = 0xDFFE, ///< This DDI is used in the XML files to identify PGN based data. + RequestDefaultProcessData = 0xDFFF ///< Request Default Process Data. This DDE is the highest ISO assigned entity. The range above this number is reserved for manufacture specific DDE's. + }; +} + +#endif // ISOBUS_STANDARD_DATA_DESCRIPTION_INDICES_HPP diff --git a/src/isobus_task_controller_client.cpp b/src/isobus_task_controller_client.cpp new file mode 100644 index 0000000..7903c66 --- /dev/null +++ b/src/isobus_task_controller_client.cpp @@ -0,0 +1,2067 @@ +//================================================================================================ +/// @file isobus_task_controller_client.cpp +/// +/// @brief A class to manage a client connection to a ISOBUS field computer's task controller +/// @author Adrian Del Grosso +/// +/// @copyright 2023 Adrian Del Grosso +//================================================================================================ +#include "isobus_task_controller_client.hpp" + +#include "can_general_parameter_group_numbers.hpp" +#include "can_network_manager.hpp" +#include "can_stack_logger.hpp" +#include "isobus_virtual_terminal_client.hpp" +#include "system_timing.hpp" +#include "to_string.hpp" + +#include +#include +#include +#include +#include + +namespace isobus +{ + TaskControllerClient::TaskControllerClient(std::shared_ptr partner, std::shared_ptr clientSource, std::shared_ptr primaryVT) : + languageCommandInterface(clientSource, partner), + partnerControlFunction(partner), + myControlFunction(clientSource), + primaryVirtualTerminal(primaryVT) + { + } + + TaskControllerClient::~TaskControllerClient() + { + terminate(); + } + + void TaskControllerClient::initialize(bool spawnThread) + { + // You cannot use this interface without having valid control functions. + assert(nullptr != myControlFunction); + assert(nullptr != partnerControlFunction); + + partnerControlFunction->add_parameter_group_number_callback(static_cast(CANLibParameterGroupNumber::ProcessData), process_rx_message, this); + partnerControlFunction->add_parameter_group_number_callback(static_cast(CANLibParameterGroupNumber::Acknowledge), process_rx_message, this); + CANNetworkManager::CANNetwork.add_global_parameter_group_number_callback(static_cast(CANLibParameterGroupNumber::ProcessData), process_rx_message, this); + + if (!languageCommandInterface.get_initialized()) + { + languageCommandInterface.initialize(); + } + + if (shouldTerminate) + { + shouldTerminate = false; + initialized = false; + } + + if (!initialized) + { +#if !defined CAN_STACK_DISABLE_THREADS && !defined ARDUINO + if (spawnThread) + { + workerThread = new std::thread([this]() { worker_thread_function(); }); + } +#endif + initialized = true; + } + } + + void TaskControllerClient::add_request_value_callback(RequestValueCommandCallback callback, void *parentPointer) + { +#if !defined CAN_STACK_DISABLE_THREADS && !defined ARDUINO + const std::lock_guard lock(clientMutex); +#endif + + RequestValueCommandCallbackInfo callbackData = { callback, parentPointer }; + requestValueCallbacks.push_back(callbackData); + } + + void TaskControllerClient::add_value_command_callback(ValueCommandCallback callback, void *parentPointer) + { +#if !defined CAN_STACK_DISABLE_THREADS && !defined ARDUINO + const std::lock_guard lock(clientMutex); +#endif + + ValueCommandCallbackInfo callbackData = { callback, parentPointer }; + valueCommandsCallbacks.push_back(callbackData); + } + + void TaskControllerClient::remove_request_value_callback(RequestValueCommandCallback callback, void *parentPointer) + { +#if !defined CAN_STACK_DISABLE_THREADS && !defined ARDUINO + const std::lock_guard lock(clientMutex); +#endif + + RequestValueCommandCallbackInfo callbackData = { callback, parentPointer }; + auto callbackLocation = std::find(requestValueCallbacks.begin(), requestValueCallbacks.end(), callbackData); + + if (requestValueCallbacks.end() != callbackLocation) + { + requestValueCallbacks.erase(callbackLocation); + } + } + + void TaskControllerClient::remove_value_command_callback(ValueCommandCallback callback, void *parentPointer) + { +#if !defined CAN_STACK_DISABLE_THREADS && !defined ARDUINO + const std::lock_guard lock(clientMutex); +#endif + + ValueCommandCallbackInfo callbackData = { callback, parentPointer }; + auto callbackLocation = std::find(valueCommandsCallbacks.begin(), valueCommandsCallbacks.end(), callbackData); + + if (valueCommandsCallbacks.end() != callbackLocation) + { + valueCommandsCallbacks.erase(callbackLocation); + } + } + + void TaskControllerClient::configure(std::shared_ptr DDOP, + std::uint8_t maxNumberBoomsSupported, + std::uint8_t maxNumberSectionsSupported, + std::uint8_t maxNumberChannelsSupportedForPositionBasedControl, + bool reportToTCSupportsDocumentation, + bool reportToTCSupportsTCGEOWithoutPositionBasedControl, + bool reportToTCSupportsTCGEOWithPositionBasedControl, + bool reportToTCSupportsPeerControlAssignment, + bool reportToTCSupportsImplementSectionControl) + { + if (StateMachineState::Disconnected == get_state()) + { + assert(nullptr != DDOP); // Client will not work without a DDOP. + generatedBinaryDDOP.clear(); + ddopStructureLabel.clear(); + userSuppliedVectorDDOP = nullptr; + ddopLocalizationLabel.fill(0x00); + ddopUploadMode = DDOPUploadType::ProgramaticallyGenerated; + clientDDOP = DDOP; + userSuppliedBinaryDDOP = nullptr; + userSuppliedBinaryDDOPSize_bytes = 0; + set_common_config_items(maxNumberBoomsSupported, + maxNumberSectionsSupported, + maxNumberChannelsSupportedForPositionBasedControl, + reportToTCSupportsDocumentation, + reportToTCSupportsTCGEOWithoutPositionBasedControl, + reportToTCSupportsTCGEOWithPositionBasedControl, + reportToTCSupportsPeerControlAssignment, + reportToTCSupportsImplementSectionControl); + } + else + { + // We don't want someone to erase our object pool or something while it is being used. + CANStackLogger::error("[TC]: Cannot reconfigure TC client while it is running!"); + } + } + + void TaskControllerClient::configure(const std::uint8_t *binaryDDOP, + std::uint32_t DDOPSize, + std::uint8_t maxNumberBoomsSupported, + std::uint8_t maxNumberSectionsSupported, + std::uint8_t maxNumberChannelsSupportedForPositionBasedControl, + bool reportToTCSupportsDocumentation, + bool reportToTCSupportsTCGEOWithoutPositionBasedControl, + bool reportToTCSupportsTCGEOWithPositionBasedControl, + bool reportToTCSupportsPeerControlAssignment, + bool reportToTCSupportsImplementSectionControl) + { + if (StateMachineState::Disconnected == get_state()) + { + assert(nullptr != binaryDDOP); // Client will not work without a DDOP. + assert(0 != DDOPSize); + generatedBinaryDDOP.clear(); + ddopStructureLabel.clear(); + userSuppliedVectorDDOP = nullptr; + ddopLocalizationLabel.fill(0x00); + ddopUploadMode = DDOPUploadType::UserProvidedBinaryPointer; + userSuppliedBinaryDDOP = binaryDDOP; + userSuppliedBinaryDDOPSize_bytes = DDOPSize; + set_common_config_items(maxNumberBoomsSupported, + maxNumberSectionsSupported, + maxNumberChannelsSupportedForPositionBasedControl, + reportToTCSupportsDocumentation, + reportToTCSupportsTCGEOWithoutPositionBasedControl, + reportToTCSupportsTCGEOWithPositionBasedControl, + reportToTCSupportsPeerControlAssignment, + reportToTCSupportsImplementSectionControl); + } + else + { + // We don't want someone to erase our object pool or something while it is being used. + CANStackLogger::error("[TC]: Cannot reconfigure TC client while it is running!"); + } + } + + void TaskControllerClient::configure(std::shared_ptr> binaryDDOP, + std::uint8_t maxNumberBoomsSupported, + std::uint8_t maxNumberSectionsSupported, + std::uint8_t maxNumberChannelsSupportedForPositionBasedControl, + bool reportToTCSupportsDocumentation, + bool reportToTCSupportsTCGEOWithoutPositionBasedControl, + bool reportToTCSupportsTCGEOWithPositionBasedControl, + bool reportToTCSupportsPeerControlAssignment, + bool reportToTCSupportsImplementSectionControl) + { + if (StateMachineState::Disconnected == get_state()) + { + assert(nullptr != binaryDDOP); // Client will not work without a DDOP. + ddopStructureLabel.clear(); + generatedBinaryDDOP.clear(); + ddopLocalizationLabel.fill(0x00); + userSuppliedVectorDDOP = binaryDDOP; + ddopUploadMode = DDOPUploadType::UserProvidedVector; + userSuppliedBinaryDDOP = nullptr; + userSuppliedBinaryDDOPSize_bytes = 0; + set_common_config_items(maxNumberBoomsSupported, + maxNumberSectionsSupported, + maxNumberChannelsSupportedForPositionBasedControl, + reportToTCSupportsDocumentation, + reportToTCSupportsTCGEOWithoutPositionBasedControl, + reportToTCSupportsTCGEOWithPositionBasedControl, + reportToTCSupportsPeerControlAssignment, + reportToTCSupportsImplementSectionControl); + } + else + { + // We don't want someone to erase our object pool or something while it is being used. + CANStackLogger::error("[TC]: Cannot reconfigure TC client while it is running!"); + } + } + + void TaskControllerClient::terminate() + { + if (initialized) + { + if (nullptr != partnerControlFunction) + { + partnerControlFunction->remove_parameter_group_number_callback(static_cast(CANLibParameterGroupNumber::ProcessData), process_rx_message, this); + partnerControlFunction->remove_parameter_group_number_callback(static_cast(CANLibParameterGroupNumber::Acknowledge), process_rx_message, this); + CANNetworkManager::CANNetwork.remove_global_parameter_group_number_callback(static_cast(CANLibParameterGroupNumber::ProcessData), process_rx_message, this); + } + + shouldTerminate = true; + +#if !defined CAN_STACK_DISABLE_THREADS && !defined ARDUINO + if (nullptr != workerThread) + { + workerThread->join(); + delete workerThread; + workerThread = nullptr; + } +#endif + } + } + + std::shared_ptr TaskControllerClient::get_internal_control_function() const + { + return myControlFunction; + } + + std::shared_ptr TaskControllerClient::get_partner_control_function() const + { + return partnerControlFunction; + } + + std::uint8_t TaskControllerClient::get_number_booms_supported() const + { + return numberBoomsSupported; + } + + std::uint8_t TaskControllerClient::get_number_sections_supported() const + { + return numberSectionsSupported; + } + + std::uint8_t TaskControllerClient::get_number_channels_supported_for_position_based_control() const + { + return numberChannelsSupportedForPositionBasedControl; + } + + bool TaskControllerClient::get_supports_documentation() const + { + return supportsDocumentation; + } + + bool TaskControllerClient::get_supports_tcgeo_without_position_based_control() const + { + return supportsTCGEOWithoutPositionBasedControl; + } + + bool TaskControllerClient::get_supports_tcgeo_with_position_based_control() const + { + return supportsTCGEOWithPositionBasedControl; + } + + bool TaskControllerClient::get_supports_peer_control_assignment() const + { + return supportsPeerControlAssignment; + } + + bool TaskControllerClient::get_supports_implement_section_control() const + { + return supportsImplementSectionControl; + } + + bool TaskControllerClient::get_is_initialized() const + { + return initialized; + } + + bool TaskControllerClient::get_is_connected() const + { + return (StateMachineState::Connected == currentState); + } + + bool TaskControllerClient::get_is_task_active() const + { + return (get_is_connected() && (0 != (0x01 & tcStatusBitfield))); + } + + void TaskControllerClient::update() + { + switch (currentState) + { + case StateMachineState::Disconnected: + { + enableStatusMessage = false; + + if (get_was_ddop_supplied()) + { + set_state(StateMachineState::WaitForStartUpDelay); + } + } + break; + + case StateMachineState::WaitForStartUpDelay: + { + if (SystemTiming::time_expired_ms(stateMachineTimestamp_ms, SIX_SECOND_TIMEOUT_MS)) + { + CANStackLogger::debug("[TC]: Startup delay complete, waiting for TC server status message."); + set_state(StateMachineState::WaitForServerStatusMessage); + } + } + break; + + case StateMachineState::SendWorkingSetMaster: + { + if (send_working_set_master()) + { + set_state(StateMachineState::SendStatusMessage); + } + else if (SystemTiming::time_expired_ms(stateMachineTimestamp_ms, TWO_SECOND_TIMEOUT_MS)) + { + CANStackLogger::error("[TC]: Timeout sending working set master message. Resetting client connection."); + set_state(StateMachineState::Disconnected); + } + } + break; + + case StateMachineState::SendStatusMessage: + { + // Start sending the status message + if (send_status()) + { + enableStatusMessage = true; + statusMessageTimestamp_ms = SystemTiming::get_timestamp_ms(); + set_state(StateMachineState::RequestVersion); + } + else if (SystemTiming::time_expired_ms(stateMachineTimestamp_ms, TWO_SECOND_TIMEOUT_MS)) + { + CANStackLogger::error("[TC]: Timeout sending first status message. Resetting client connection."); + set_state(StateMachineState::Disconnected); + } + } + break; + + case StateMachineState::RequestVersion: + { + if (send_version_request()) + { + set_state(StateMachineState::WaitForRequestVersionResponse); + } + else if (SystemTiming::time_expired_ms(stateMachineTimestamp_ms, TWO_SECOND_TIMEOUT_MS)) + { + CANStackLogger::error("[TC]: Timeout sending version request message. Resetting client connection."); + set_state(StateMachineState::Disconnected); + } + } + break; + + case StateMachineState::WaitForRequestVersionResponse: + { + if (SystemTiming::time_expired_ms(stateMachineTimestamp_ms, TWO_SECOND_TIMEOUT_MS)) + { + CANStackLogger::error("[TC]: Timeout waiting for version request response. Resetting client connection."); + set_state(StateMachineState::Disconnected); + } + } + break; + + case StateMachineState::WaitForRequestVersionFromServer: + { + if (SystemTiming::time_expired_ms(stateMachineTimestamp_ms, SIX_SECOND_TIMEOUT_MS)) + { + CANStackLogger::warn("[TC]: Timeout waiting for version request from TC. This is not required, so proceeding anways."); + set_state(StateMachineState::RequestLanguage); + } + } + break; + + case StateMachineState::SendRequestVersionResponse: + { + if (send_request_version_response()) + { + set_state(StateMachineState::RequestLanguage); + } + else if (SystemTiming::time_expired_ms(stateMachineTimestamp_ms, TWO_SECOND_TIMEOUT_MS)) + { + CANStackLogger::error("[TC]: Timeout sending version request response. Resetting client connection."); + set_state(StateMachineState::Disconnected); + } + } + break; + + case StateMachineState::RequestLanguage: + { + if ((serverVersion < static_cast(Version::SecondPublishedEdition)) && + (nullptr == primaryVirtualTerminal)) + { + languageCommandInterface.set_partner(nullptr); // TC might not reply and no VT specified, so just see if anyone knows. + CANStackLogger::warn("[TC]: The TC is < version 4 but no VT was provided. Language data will be requested globally, which might not be ideal."); + } + + if ((serverVersion < static_cast(Version::SecondPublishedEdition)) && + (nullptr != primaryVirtualTerminal) && + (primaryVirtualTerminal->languageCommandInterface.send_request_language_command())) + { + set_state(StateMachineState::WaitForLanguageResponse); + } + else if (languageCommandInterface.send_request_language_command()) + { + set_state(StateMachineState::WaitForLanguageResponse); + } + else if (SystemTiming::time_expired_ms(stateMachineTimestamp_ms, SIX_SECOND_TIMEOUT_MS)) + { + CANStackLogger::error("[TC]: Timeout trying to send request for language command message. Resetting client connection."); + set_state(StateMachineState::Disconnected); + } + } + break; + + case StateMachineState::WaitForLanguageResponse: + { + if ((SystemTiming::get_time_elapsed_ms(languageCommandInterface.get_language_command_timestamp()) < SIX_SECOND_TIMEOUT_MS) && + ("" != languageCommandInterface.get_language_code())) + { + set_state(StateMachineState::ProcessDDOP); + } + } + break; + + case StateMachineState::ProcessDDOP: + { + if (DDOPUploadType::ProgramaticallyGenerated == ddopUploadMode) + { + assert(0 != clientDDOP->size()); // Need to have a valid object pool! + + if (serverVersion < clientDDOP->get_task_controller_compatibility_level()) + { + clientDDOP->set_task_controller_compatibility_level(serverVersion); // Manipulate the DDOP slightly if needed to upload a version compatible DDOP + CANStackLogger::info("[TC]: DDOP will be generated using the server's version instead of the specified version. New version: " + + isobus::to_string(static_cast(serverVersion))); + } + + if (generatedBinaryDDOP.empty()) + { + // Binary DDOP has not been generated before. + if (clientDDOP->generate_binary_object_pool(generatedBinaryDDOP)) + { + process_labels_from_ddop(); + CANStackLogger::debug("[TC]: DDOP Generated, size: " + isobus::to_string(static_cast(generatedBinaryDDOP.size()))); + set_state(StateMachineState::RequestStructureLabel); + } + else + { + CANStackLogger::error("[TC]: Cannot proceed with connection to TC due to invalid DDOP. Check log for [DDOP] events. TC client will now terminate."); + terminate(); + } + } + else + { + CANStackLogger::debug("[TC]: Using previously generated DDOP binary"); + set_state(StateMachineState::RequestStructureLabel); + } + } + else + { + if ((ddopLocalizationLabel.empty()) || + (ddopStructureLabel.empty())) + { + CANStackLogger::debug("[TC]: Beginning a search of pre-serialized DDOP for device structure and localization labels."); + process_labels_from_ddop(); + + if ((ddopLocalizationLabel.empty()) || + (ddopStructureLabel.empty())) + { + CANStackLogger::error("[TC]: Failed to parse the DDOP. Ensure you provided a valid device object. TC client will now terminate."); + terminate(); + } + } + else + { + CANStackLogger::debug("[TC]: Reusing previously located device labels."); + } + set_state(StateMachineState::RequestStructureLabel); + } + } + break; + + case StateMachineState::RequestStructureLabel: + { + if (send_request_structure_label()) + { + set_state(StateMachineState::WaitForStructureLabelResponse); + } + else if (SystemTiming::time_expired_ms(stateMachineTimestamp_ms, TWO_SECOND_TIMEOUT_MS)) + { + CANStackLogger::error("[TC]: Timeout trying to send request for TC structure label. Resetting client connection."); + set_state(StateMachineState::Disconnected); + } + } + break; + + case StateMachineState::WaitForStructureLabelResponse: + { + if (SystemTiming::time_expired_ms(stateMachineTimestamp_ms, TWO_SECOND_TIMEOUT_MS)) + { + CANStackLogger::error("[TC]: Timeout waiting for TC structure label. Resetting client connection."); + set_state(StateMachineState::Disconnected); + } + } + break; + + case StateMachineState::RequestLocalizationLabel: + { + if (send_request_localization_label()) + { + set_state(StateMachineState::WaitForLocalizationLabelResponse); + } + else if (SystemTiming::time_expired_ms(stateMachineTimestamp_ms, TWO_SECOND_TIMEOUT_MS)) + { + CANStackLogger::error("[TC]: Timeout trying to send request for TC localization label. Resetting client connection."); + set_state(StateMachineState::Disconnected); + } + } + break; + + case StateMachineState::WaitForLocalizationLabelResponse: + { + if (SystemTiming::time_expired_ms(stateMachineTimestamp_ms, TWO_SECOND_TIMEOUT_MS)) + { + CANStackLogger::error("[TC]: Timeout waiting for TC localization label. Resetting client connection."); + set_state(StateMachineState::Disconnected); + } + } + break; + + case StateMachineState::SendDeleteObjectPool: + { + if (send_delete_object_pool()) + { + set_state(StateMachineState::WaitForDeleteObjectPoolResponse); + } + else if (SystemTiming::time_expired_ms(stateMachineTimestamp_ms, TWO_SECOND_TIMEOUT_MS)) + { + CANStackLogger::error("[TC]: Timeout trying to send delete object pool message. Resetting client connection."); + set_state(StateMachineState::Disconnected); + } + } + break; + + case StateMachineState::WaitForDeleteObjectPoolResponse: + { + if (SystemTiming::time_expired_ms(stateMachineTimestamp_ms, TWO_SECOND_TIMEOUT_MS)) + { + CANStackLogger::error("[TC]: Timeout waiting for delete object pool response. Resetting client connection."); + set_state(StateMachineState::Disconnected); + } + } + break; + + case StateMachineState::SendRequestTransferObjectPool: + { + if (send_request_object_pool_transfer()) + { + set_state(StateMachineState::WaitForRequestTransferObjectPoolResponse); + } + else if (SystemTiming::time_expired_ms(stateMachineTimestamp_ms, TWO_SECOND_TIMEOUT_MS)) + { + CANStackLogger::error("[TC]: Timeout trying to send request to transfer object pool. Resetting client connection."); + set_state(StateMachineState::Disconnected); + } + } + break; + + case StateMachineState::WaitForRequestTransferObjectPoolResponse: + { + if (SystemTiming::time_expired_ms(stateMachineTimestamp_ms, TWO_SECOND_TIMEOUT_MS)) + { + CANStackLogger::error("[TC]: Timeout waiting for request transfer object pool response. Resetting client connection."); + set_state(StateMachineState::Disconnected); + } + } + break; + + case StateMachineState::BeginTransferDDOP: + { + bool transmitSuccessful = false; + std::uint32_t dataLength = 0; + + switch (ddopUploadMode) + { + case DDOPUploadType::ProgramaticallyGenerated: + { + dataLength = static_cast(generatedBinaryDDOP.size() + 1); // Account for Mux byte + } + break; + + case DDOPUploadType::UserProvidedBinaryPointer: + { + dataLength = static_cast(userSuppliedBinaryDDOPSize_bytes + 1); + } + break; + + case DDOPUploadType::UserProvidedVector: + { + dataLength = static_cast(userSuppliedVectorDDOP->size() + 1); + } + break; + + default: + break; + } + + assert(0 != dataLength); + transmitSuccessful = CANNetworkManager::CANNetwork.send_can_message(static_cast(CANLibParameterGroupNumber::ProcessData), + nullptr, + dataLength, + myControlFunction, + partnerControlFunction, + CANIdentifier::CANPriority::PriorityLowest7, + process_tx_callback, + this, + process_internal_object_pool_upload_callback); + if (transmitSuccessful) + { + set_state(StateMachineState::WaitForDDOPTransfer); + } + else if (SystemTiming::time_expired_ms(stateMachineTimestamp_ms, TWO_SECOND_TIMEOUT_MS)) + { + CANStackLogger::error("[TC]: Timeout trying to begin the object pool upload. Resetting client connection."); + set_state(StateMachineState::Disconnected); + } + } + break; + + case StateMachineState::WaitForDDOPTransfer: + case StateMachineState::WaitForServerStatusMessage: + { + // Waiting... + } + break; + + case StateMachineState::WaitForObjectPoolTransferResponse: + { + if (SystemTiming::time_expired_ms(stateMachineTimestamp_ms, TWO_SECOND_TIMEOUT_MS)) + { + CANStackLogger::error("[TC]: Timeout waiting for object pool transfer response. Resetting client connection."); + set_state(StateMachineState::Disconnected); + } + } + break; + + case StateMachineState::SendObjectPoolActivate: + { + if (send_object_pool_activate()) + { + set_state(StateMachineState::WaitForObjectPoolActivateResponse); + } + else if (SystemTiming::time_expired_ms(stateMachineTimestamp_ms, TWO_SECOND_TIMEOUT_MS)) + { + CANStackLogger::error("[TC]: Timeout trying to activate object pool. Resetting client connection."); + set_state(StateMachineState::Disconnected); + } + } + break; + + case StateMachineState::WaitForObjectPoolActivateResponse: + { + if (SystemTiming::time_expired_ms(stateMachineTimestamp_ms, TWO_SECOND_TIMEOUT_MS)) + { + CANStackLogger::error("[TC]: Timeout waiting for activate object pool response. Resetting client connection."); + set_state(StateMachineState::Disconnected); + } + } + break; + + case StateMachineState::Connected: + { + if (SystemTiming::time_expired_ms(serverStatusMessageTimestamp_ms, SIX_SECOND_TIMEOUT_MS)) + { + CANStackLogger::error("[TC]: Server Status Message Timeout. The TC may be offline."); + set_state(StateMachineState::Disconnected); + } + else + { + process_queued_commands(); + process_queued_threshold_commands(); + } + } + break; + + case StateMachineState::DeactivateObjectPool: + { + if (send_object_pool_deactivate()) + { + set_state(StateMachineState::WaitForObjectPoolDeactivateResponse); + } + else if (SystemTiming::time_expired_ms(stateMachineTimestamp_ms, TWO_SECOND_TIMEOUT_MS)) + { + CANStackLogger::error("[TC]: Timeout sending object pool deactivate. Client terminated."); + set_state(StateMachineState::Disconnected); + terminate(); + } + } + break; + + case StateMachineState::WaitForObjectPoolDeactivateResponse: + { + if (SystemTiming::time_expired_ms(stateMachineTimestamp_ms, TWO_SECOND_TIMEOUT_MS)) + { + CANStackLogger::error("[TC]: Timeout waiting for deactivate object pool response. Client terminated."); + set_state(StateMachineState::Disconnected); + terminate(); + } + } + break; + + default: + { + assert(false); // Unknown state? File a bug on GitHub if you see this happen. + } + break; + } + + if ((enableStatusMessage) && + (SystemTiming::time_expired_ms(statusMessageTimestamp_ms, TWO_SECOND_TIMEOUT_MS)) && + (send_status())) + { + statusMessageTimestamp_ms = SystemTiming::get_timestamp_ms(); + } + } + + bool TaskControllerClient::ProcessDataCallbackInfo::operator==(const ProcessDataCallbackInfo &obj) const + { + return ((obj.ddi == this->ddi) && (obj.elementNumber == this->elementNumber)); + } + + bool TaskControllerClient::RequestValueCommandCallbackInfo::operator==(const RequestValueCommandCallbackInfo &obj) const + { + return (obj.callback == this->callback) && (obj.parent == this->parent); + } + + bool TaskControllerClient::ValueCommandCallbackInfo::operator==(const ValueCommandCallbackInfo &obj) const + { + return (obj.callback == this->callback) && (obj.parent == this->parent); + } + + void TaskControllerClient::clear_queues() + { + queuedValueRequests.clear(); + queuedValueCommands.clear(); + measurementTimeIntervalCommands.clear(); + measurementMinimumThresholdCommands.clear(); + measurementMaximumThresholdCommands.clear(); + measurementOnChangeThresholdCommands.clear(); + } + + bool TaskControllerClient::get_was_ddop_supplied() const + { + bool retVal = false; + + switch (ddopUploadMode) + { + case DDOPUploadType::ProgramaticallyGenerated: + { + retVal = (nullptr != clientDDOP); + } + break; + + case DDOPUploadType::UserProvidedBinaryPointer: + { + retVal = (nullptr != userSuppliedBinaryDDOP) && + (0 != userSuppliedBinaryDDOPSize_bytes); + } + break; + + case DDOPUploadType::UserProvidedVector: + { + retVal = (nullptr != userSuppliedVectorDDOP) && + (!userSuppliedVectorDDOP->empty()); + } + break; + + default: + break; + } + return retVal; + } + + void TaskControllerClient::process_labels_from_ddop() + { + std::uint32_t currentByteIndex = 0; + const std::string DEVICE_TABLE_ID = "DVC"; + constexpr std::uint8_t DESIGNATOR_BYTE_OFFSET = 5; + constexpr std::uint8_t CLIENT_NAME_LENGTH = 8; + + switch (ddopUploadMode) + { + case DDOPUploadType::ProgramaticallyGenerated: + { + assert(nullptr != clientDDOP); // You need a DDOP + // Does your DDOP have a device object? Device object 0 is required by ISO11783-10 + auto deviceObject = clientDDOP->get_object_by_id(0); + assert(nullptr != deviceObject); + assert(task_controller_object::ObjectTypes::Device == deviceObject->get_object_type()); + + ddopStructureLabel = std::static_pointer_cast(deviceObject)->get_structure_label(); + + while (ddopStructureLabel.size() < task_controller_object::DeviceObject::MAX_STRUCTURE_AND_LOCALIZATION_LABEL_LENGTH) + { + ddopStructureLabel.push_back(' '); + } + + ddopLocalizationLabel = std::static_pointer_cast(deviceObject)->get_localization_label(); + } + break; + + case DDOPUploadType::UserProvidedBinaryPointer: + case DDOPUploadType::UserProvidedVector: + { + auto getDDOPSize = [this]() { + if (ddopUploadMode == DDOPUploadType::UserProvidedBinaryPointer) + { + return userSuppliedBinaryDDOPSize_bytes; + } + else + { + return static_cast(userSuppliedVectorDDOP->size()); + } + }; + + auto getDDOPByteAt = [this](std::size_t index) { + if (ddopUploadMode == DDOPUploadType::UserProvidedBinaryPointer) + { + return userSuppliedBinaryDDOP[index]; + } + else + { + return userSuppliedVectorDDOP->at(index); + } + }; + // Searching for "DVC" + while (currentByteIndex < (getDDOPSize() - DEVICE_TABLE_ID.size())) + { + if ((DEVICE_TABLE_ID[0] == getDDOPByteAt(currentByteIndex)) && + (DEVICE_TABLE_ID[1] == getDDOPByteAt(currentByteIndex + 1)) && + (DEVICE_TABLE_ID[2] == getDDOPByteAt(currentByteIndex + 2))) + { + // We have to do a lot of error checking on the DDOP length + // This is because we don't control the content of this DDOP, and have no + // assurances that the schema is even valid + + assert((currentByteIndex + DESIGNATOR_BYTE_OFFSET) < getDDOPSize()); // Not enough bytes to read the designator length + currentByteIndex += DESIGNATOR_BYTE_OFFSET; // Skip to the next variable length part of the object + + const std::uint32_t DESIGNATOR_LENGTH = getDDOPByteAt(currentByteIndex); // "N", See Table A.1 + assert(currentByteIndex + DESIGNATOR_LENGTH < getDDOPSize()); // Not enough bytes in your DDOP! + currentByteIndex += DESIGNATOR_LENGTH + 1; + + const std::uint32_t SOFTWARE_VERSION_LENGTH = getDDOPByteAt(currentByteIndex); // "M", See Table A.1 + assert(currentByteIndex + SOFTWARE_VERSION_LENGTH + CLIENT_NAME_LENGTH < getDDOPSize()); // Not enough bytes in your DDOP! + currentByteIndex += SOFTWARE_VERSION_LENGTH + CLIENT_NAME_LENGTH + 1; + + const std::uint32_t SERIAL_NUMBER_LENGTH = getDDOPByteAt(currentByteIndex); // "O", See Table A.1 + assert(currentByteIndex + SERIAL_NUMBER_LENGTH < getDDOPSize()); // Not enough bytes in your DDOP! + currentByteIndex += SERIAL_NUMBER_LENGTH + 1; + + assert(currentByteIndex + task_controller_object::DeviceObject::MAX_STRUCTURE_AND_LOCALIZATION_LABEL_LENGTH < getDDOPSize()); // // Not enough bytes in your DDOP! + for (std::uint_fast8_t i = 0; i < task_controller_object::DeviceObject::MAX_STRUCTURE_AND_LOCALIZATION_LABEL_LENGTH; i++) + { + ddopStructureLabel.push_back(getDDOPByteAt(currentByteIndex + i)); // Read the descriptor + } + + currentByteIndex += task_controller_object::DeviceObject::MAX_STRUCTURE_AND_LOCALIZATION_LABEL_LENGTH; + assert(currentByteIndex + task_controller_object::DeviceObject::MAX_STRUCTURE_AND_LOCALIZATION_LABEL_LENGTH < getDDOPSize()); // // Not enough bytes in your DDOP! + for (std::uint_fast8_t i = 0; i < task_controller_object::DeviceObject::MAX_STRUCTURE_AND_LOCALIZATION_LABEL_LENGTH; i++) + { + ddopLocalizationLabel[i] = (getDDOPByteAt(currentByteIndex + i)); // Read the localization label + } + break; + } + } + } + break; + + default: + break; + } + } + + void TaskControllerClient::process_queued_commands() + { +#if !defined CAN_STACK_DISABLE_THREADS && !defined ARDUINO + const std::lock_guard lock(clientMutex); +#endif + bool transmitSuccessful = true; + + while (!queuedValueRequests.empty() && transmitSuccessful) + { + const auto ¤tRequest = queuedValueRequests.front(); + + for (auto ¤tCallback : requestValueCallbacks) + { + std::uint32_t newValue = 0; + if (currentCallback.callback(currentRequest.elementNumber, currentRequest.ddi, newValue, currentCallback.parent)) + { + transmitSuccessful = send_value_command(currentRequest.elementNumber, currentRequest.ddi, newValue); + break; + } + } + queuedValueRequests.pop_front(); + } + while (!queuedValueCommands.empty() && transmitSuccessful) + { + const auto ¤tRequest = queuedValueCommands.front(); + + for (auto ¤tCallback : valueCommandsCallbacks) + { + if (currentCallback.callback(currentRequest.elementNumber, currentRequest.ddi, currentRequest.processDataValue, currentCallback.parent)) + { + break; + } + } + queuedValueCommands.pop_front(); + + //! @todo process PDACKs better + if (currentRequest.ackRequested) + { + transmitSuccessful = send_pdack(currentRequest.elementNumber, currentRequest.ddi); + } + } + } + + void TaskControllerClient::process_queued_threshold_commands() + { + bool transmitSuccessful = false; + + for (auto &measurementTimeCommand : measurementTimeIntervalCommands) + { + if (SystemTiming::time_expired_ms(measurementTimeCommand.lastValue, measurementTimeCommand.processDataValue)) + { + // Time to update this time interval variable + transmitSuccessful = false; + for (auto ¤tCallback : requestValueCallbacks) + { + std::uint32_t newValue = 0; + if (currentCallback.callback(measurementTimeCommand.elementNumber, measurementTimeCommand.ddi, newValue, currentCallback.parent)) + { + transmitSuccessful = send_value_command(measurementTimeCommand.elementNumber, measurementTimeCommand.ddi, newValue); + break; + } + } + + if (transmitSuccessful) + { + measurementTimeCommand.lastValue = SystemTiming::get_timestamp_ms(); + } + } + } + for (auto &measurementMaxCommand : measurementMaximumThresholdCommands) + { + // Get the current process data value + std::uint32_t newValue = 0; + for (auto ¤tCallback : requestValueCallbacks) + { + if (currentCallback.callback(measurementMaxCommand.elementNumber, measurementMaxCommand.ddi, newValue, currentCallback.parent)) + { + break; + } + } + + if (!measurementMaxCommand.thresholdPassed) + { + if ((newValue > measurementMaxCommand.processDataValue) && + (send_value_command(measurementMaxCommand.elementNumber, measurementMaxCommand.ddi, newValue))) + { + measurementMaxCommand.thresholdPassed = true; + } + } + else + { + if (newValue < measurementMaxCommand.processDataValue) + { + measurementMaxCommand.thresholdPassed = false; + } + } + } + for (auto &measurementMinCommand : measurementMinimumThresholdCommands) + { + // Get the current process data value + std::uint32_t newValue = 0; + for (auto ¤tCallback : requestValueCallbacks) + { + if (currentCallback.callback(measurementMinCommand.elementNumber, measurementMinCommand.ddi, newValue, currentCallback.parent)) + { + break; + } + } + + if (!measurementMinCommand.thresholdPassed) + { + if ((newValue < measurementMinCommand.processDataValue) && + (send_value_command(measurementMinCommand.elementNumber, measurementMinCommand.ddi, newValue))) + { + measurementMinCommand.thresholdPassed = true; + } + } + else + { + if (newValue > measurementMinCommand.processDataValue) + { + measurementMinCommand.thresholdPassed = false; + } + } + } + for (auto &measurementChangeCommand : measurementOnChangeThresholdCommands) + { + // Get the current process data value + std::uint32_t newValue = 0; + for (auto ¤tCallback : requestValueCallbacks) + { + if (currentCallback.callback(measurementChangeCommand.elementNumber, measurementChangeCommand.ddi, newValue, currentCallback.parent)) + { + break; + } + } + + std::int64_t lowerLimit = (static_cast(measurementChangeCommand.lastValue) - measurementChangeCommand.processDataValue); + if (lowerLimit < 0) + { + lowerLimit = 0; + } + + if ((newValue != measurementChangeCommand.lastValue) && + ((newValue >= (measurementChangeCommand.lastValue + measurementChangeCommand.processDataValue)) || + (newValue <= lowerLimit))) + { + if (send_value_command(measurementChangeCommand.elementNumber, measurementChangeCommand.ddi, newValue)) + { + measurementChangeCommand.lastValue = newValue; + } + } + } + } + + void TaskControllerClient::process_rx_message(const CANMessage &message, void *parentPointer) + { + if ((nullptr != parentPointer) && + (CAN_DATA_LENGTH <= message.get_data_length()) && + (nullptr != message.get_source_control_function())) + { + auto parentTC = static_cast(parentPointer); + const auto &messageData = message.get_data(); + + switch (message.get_identifier().get_parameter_group_number()) + { + case static_cast(CANLibParameterGroupNumber::Acknowledge): + { + if (AcknowledgementType::Negative == static_cast(message.get_uint8_at(0))) + { + std::uint32_t targetParameterGroupNumber = message.get_uint24_at(5); + if (static_cast(CANLibParameterGroupNumber::ProcessData) == targetParameterGroupNumber) + { + CANStackLogger::CAN_stack_log(CANStackLogger::LoggingLevel::Error, "[TC]: The TC Server is NACK-ing our messages. Disconnecting."); + parentTC->set_state(StateMachineState::Disconnected); + } + } + } + break; + + case static_cast(CANLibParameterGroupNumber::ProcessData): + { + switch (static_cast(messageData[0] & 0x0F)) + { + case ProcessDataCommands::TechnicalCapabilities: + { + switch (static_cast(messageData[0] >> 4)) + { + case TechnicalDataMessageCommands::ParameterRequestVersion: + { + if (StateMachineState::WaitForRequestVersionFromServer == parentTC->get_state()) + { + parentTC->set_state(StateMachineState::SendRequestVersionResponse); + } + else + { + CANStackLogger::warn("[TC]: Server requested version information at a strange time."); + } + } + break; + + case TechnicalDataMessageCommands::ParameterVersion: + { + parentTC->serverVersion = messageData[1]; + parentTC->maxServerBootTime_s = messageData[2]; + parentTC->serverOptionsByte1 = messageData[3]; + parentTC->serverOptionsByte2 = messageData[4]; + parentTC->serverNumberOfBoomsForSectionControl = messageData[5]; + parentTC->serverNumberOfSectionsForSectionControl = messageData[6]; + parentTC->serverNumberOfChannelsForPositionBasedControl = messageData[7]; + + if (messageData[1] > static_cast(Version::SecondPublishedEdition)) + { + CANStackLogger::warn("[TC]: Server version is newer than client's maximum supported version."); + } + CANStackLogger::debug("[TC]: TC Server supports version %u with %u booms, %u sections, and %u position based control channels.", + messageData[1], + messageData[5], + messageData[6], + messageData[7]); + + if (StateMachineState::WaitForRequestVersionResponse == parentTC->get_state()) + { + parentTC->set_state(StateMachineState::WaitForRequestVersionFromServer); + } + } + break; + + default: + { + CANStackLogger::warn("[TC]: Unsupported process data technical data message received. Message will be dropped."); + } + break; + } + } + break; + + case ProcessDataCommands::DeviceDescriptor: + { + switch (static_cast(messageData[0] >> 4)) + { + case DeviceDescriptorCommands::StructureLabel: + { + if (StateMachineState::WaitForStructureLabelResponse == parentTC->get_state()) + { + if ((0xFF == messageData[1]) && + (0xFF == messageData[2]) && + (0xFF == messageData[3]) && + (0xFF == messageData[4]) && + (0xFF == messageData[5]) && + (0xFF == messageData[6]) && + (0xFF == messageData[7]) && + (CAN_DATA_LENGTH == messageData.size())) + { + // TC has no structure label for us. Need to upload the DDOP. + parentTC->set_state(StateMachineState::SendRequestTransferObjectPool); + } + else + { + std::string tcStructure; + + for (std::size_t i = 1; i < messageData.size(); i++) + { + tcStructure.push_back(messageData[i]); + } + + if (tcStructure.size() > 40) + { + CANStackLogger::warn("[TC]: Structure Label from TC exceeds the max length allowed by ISO11783-10"); + } + + if (parentTC->ddopStructureLabel == tcStructure) + { + // Structure label matched. No upload needed yet. + CANStackLogger::debug("[TC]: Task controller structure labels match"); + parentTC->set_state(StateMachineState::RequestLocalizationLabel); + } + else + { + // Structure label did not match. Need to delete current DDOP and re-upload. + CANStackLogger::info("[TC]: Task controller structure labels do not match. DDOP will be deleted and reuploaded."); + parentTC->set_state(StateMachineState::SendDeleteObjectPool); + } + } + } + else + { + CANStackLogger::warn("[TC]: Structure label message received, but ignored due to current state machine state."); + } + } + break; + + case DeviceDescriptorCommands::LocalizationLabel: + { + // Right now, we'll just reload the pool if the localization doesn't match, but + // in the future we should permit modifications to the localization and DVP objects + //! @todo Localization label partial pool handling + if (StateMachineState::WaitForLocalizationLabelResponse == parentTC->get_state()) + { + if ((0xFF == messageData[1]) && + (0xFF == messageData[2]) && + (0xFF == messageData[3]) && + (0xFF == messageData[4]) && + (0xFF == messageData[5]) && + (0xFF == messageData[6]) && + (0xFF == messageData[7]) && + (CAN_DATA_LENGTH == messageData.size())) + { + // TC has no localization label for us. Need to upload the DDOP. + parentTC->set_state(StateMachineState::SendRequestTransferObjectPool); + } + else + { + assert(7 == parentTC->ddopLocalizationLabel.size()); // Make sure the DDOP is valid before we access the label. It must be 7 bytes + bool labelsMatch = true; + + for (std::uint_fast8_t i = 0; i < (CAN_DATA_LENGTH - 1); i++) + { + if (messageData[i + 1] != parentTC->ddopLocalizationLabel[i]) + { + labelsMatch = false; + break; + } + } + + if (labelsMatch) + { + // DDOP labels all matched + CANStackLogger::debug("[TC]: Task controller localization labels match"); + parentTC->set_state(StateMachineState::SendObjectPoolActivate); + } + else + { + // Labels didn't match. Reupload + CANStackLogger::info("[TC]: Task controller localization labels do not match. DDOP will be deleted and reuploaded."); + parentTC->set_state(StateMachineState::SendDeleteObjectPool); + } + } + } + else + { + CANStackLogger::warn("[TC]: Localization label message received, but ignored due to current state machine state."); + } + } + break; + + case DeviceDescriptorCommands::RequestObjectPoolTransferResponse: + { + if (StateMachineState::WaitForRequestTransferObjectPoolResponse == parentTC->get_state()) + { + if (0 == messageData[1]) + { + // Because there is overhead associated with object storage, it is impossible to predict whether there is enough memory available, technically. + CANStackLogger::debug("[TC]: Server indicates there may be enough memory available."); + parentTC->set_state(StateMachineState::BeginTransferDDOP); + } + else + { + CANStackLogger::error("[TC]: Server states that there is not enough memory available for our DDOP. Client will terminate."); + parentTC->terminate(); + } + } + else + { + CANStackLogger::warn("[TC]: Request Object-pool Transfer Response message received, but ignored due to current state machine state."); + } + } + break; + + case DeviceDescriptorCommands::ObjectPoolActivateDeactivateResponse: + { + if (StateMachineState::WaitForObjectPoolActivateResponse == parentTC->get_state()) + { + if (0 == messageData[1]) + { + CANStackLogger::info("[TC]: DDOP Activated without error."); + parentTC->set_state(StateMachineState::Connected); + } + else + { + CANStackLogger::error("[TC]: DDOP was not activated."); + if (0x01 & messageData[1]) + { + CANStackLogger::error("[TC]: There are errors in the DDOP. Faulting parent ID: " + + isobus::to_string(static_cast(static_cast(messageData[2]) | + static_cast(messageData[3] << 8))) + + " Faulting object: " + + isobus::to_string(static_cast(static_cast(messageData[4]) | + static_cast(messageData[5] << 8)))); + if (0x01 & messageData[6]) + { + CANStackLogger::error("[TC]: Method or attribute not supported by the TC"); + } + if (0x02 & messageData[6]) + { + CANStackLogger::error("[TC]: Unknown object reference (missing object)"); + } + if (0x04 & messageData[6]) + { + CANStackLogger::error("[TC]: Unknown error (Any other error)"); + } + if (0x08 & messageData[6]) + { + CANStackLogger::error("[TC]: Device descriptor object pool was deleted from volatile memory"); + } + if (0xF0 & messageData[6]) + { + CANStackLogger::warn("[TC]: The TC sent illegal errors in the reserved bits of the response."); + } + } + if (0x02 & messageData[1]) + { + CANStackLogger::error("[TC]: Task Controller ran out of memory during activation."); + } + if (0x04 & messageData[1]) + { + CANStackLogger::error("[TC]: Task Controller indicates an unknown error occurred."); + } + if (0x08 & messageData[1]) + { + CANStackLogger::error("[TC]: A different DDOP with the same structure label already exists in the TC."); + } + if (0xF0 & messageData[1]) + { + CANStackLogger::warn("[TC]: The TC sent illegal errors in the reserved bits of the response."); + } + parentTC->set_state(StateMachineState::Disconnected); + CANStackLogger::error("[TC]: Client terminated."); + parentTC->terminate(); + } + } + else if (StateMachineState::WaitForObjectPoolDeactivateResponse == parentTC->get_state()) + { + if (0 == messageData[1]) + { + CANStackLogger::info("[TC]: Object pool deactivated OK."); + } + else + { + CANStackLogger::error("[TC]: Object pool deactivation error."); + } + } + else + { + CANStackLogger::warn("[TC]: Object pool activate/deactivate response received at a strange time. Message dropped."); + } + } + break; + + case DeviceDescriptorCommands::ObjectPoolDeleteResponse: + { + // Message content of this is unreliable, the standard is ambigious on what to even check. + // Plus, if the delete failed, the recourse is the same, always proceed. + if (StateMachineState::WaitForDeleteObjectPoolResponse == parentTC->get_state()) + { + parentTC->set_state(StateMachineState::SendRequestTransferObjectPool); + } + } + break; + + case DeviceDescriptorCommands::ObjectPoolTransferResponse: + { + if (StateMachineState::WaitForObjectPoolTransferResponse == parentTC->get_state()) + { + if (0 == messageData[1]) + { + CANStackLogger::debug("[TC]: DDOP upload completed with no errors."); + parentTC->set_state(StateMachineState::SendObjectPoolActivate); + } + else + { + if (0x01 == messageData[1]) + { + CANStackLogger::error("[TC]: DDOP upload completed but TC ran out of memory during transfer."); + } + else + { + CANStackLogger::error("[TC]: DDOP upload completed but TC had some unknown error."); + } + CANStackLogger::error("[TC]: Client terminated."); + parentTC->terminate(); + } + } + else + { + CANStackLogger::warn("[TC]: Recieved unexpected object pool transfer response"); + } + } + break; + + default: + { + CANStackLogger::warn("[TC]: Unsupported device descriptor command message received. Message will be dropped."); + } + break; + } + } + break; + + case ProcessDataCommands::StatusMessage: + { + if (parentTC->partnerControlFunction->get_NAME() == message.get_source_control_function()->get_NAME()) + { + // Many values in the status message were undefined in version 2 and before, so the + // standard explicitly tells us to ignore those attributes. The only things that really + // matter are that we got the mesesage, and bytes 5, 6 and 7. + parentTC->tcStatusBitfield = messageData[4]; + parentTC->sourceAddressOfCommandBeingExecuted = messageData[5]; + parentTC->commandBeingExecuted = messageData[6]; + parentTC->serverStatusMessageTimestamp_ms = SystemTiming::get_timestamp_ms(); + if (StateMachineState::WaitForServerStatusMessage == parentTC->currentState) + { + parentTC->set_state(StateMachineState::SendWorkingSetMaster); + } + } + } + break; + + case ProcessDataCommands::ClientTask: + { + CANStackLogger::warn("[TC]: Server sent the client task message, which is not meant to be sent by servers."); + } + break; + + case ProcessDataCommands::RequestValue: + { + ProcessDataCallbackInfo requestData = { 0, 0, 0, 0, false, false }; +#if !defined CAN_STACK_DISABLE_THREADS && !defined ARDUINO + const std::lock_guard lock(parentTC->clientMutex); +#endif + + requestData.ackRequested = false; + requestData.elementNumber = (static_cast(messageData[0] >> 4) | (static_cast(messageData[1]) << 4)); + requestData.ddi = static_cast(messageData[2]) | + (static_cast(messageData[3]) << 8); + requestData.processDataValue = (static_cast(messageData[4]) | + (static_cast(messageData[5]) << 8) | + (static_cast(messageData[6]) << 16) | + (static_cast(messageData[7]) << 24)); + parentTC->queuedValueRequests.push_back(requestData); + } + break; + + case ProcessDataCommands::Value: + { + ProcessDataCallbackInfo requestData = { 0, 0, 0, 0, false, false }; +#if !defined CAN_STACK_DISABLE_THREADS && !defined ARDUINO + const std::lock_guard lock(parentTC->clientMutex); +#endif + + requestData.ackRequested = false; + requestData.elementNumber = (static_cast(messageData[0] >> 4) | (static_cast(messageData[1]) << 4)); + requestData.ddi = static_cast(messageData[2]) | + (static_cast(messageData[3]) << 8); + requestData.processDataValue = (static_cast(messageData[4]) | + (static_cast(messageData[5]) << 8) | + (static_cast(messageData[6]) << 16) | + (static_cast(messageData[7]) << 24)); + parentTC->queuedValueCommands.push_back(requestData); + } + break; + + case ProcessDataCommands::SetValueAndAcknowledge: + { + ProcessDataCallbackInfo requestData = { 0, 0, 0, 0, false, false }; +#if !defined CAN_STACK_DISABLE_THREADS && !defined ARDUINO + const std::lock_guard lock(parentTC->clientMutex); +#endif + + requestData.ackRequested = true; + requestData.elementNumber = (static_cast(messageData[0] >> 4) | (static_cast(messageData[1]) << 4)); + requestData.ddi = static_cast(messageData[2]) | + (static_cast(messageData[3]) << 8); + requestData.processDataValue = (static_cast(messageData[4]) | + (static_cast(messageData[5]) << 8) | + (static_cast(messageData[6]) << 16) | + (static_cast(messageData[7]) << 24)); + parentTC->queuedValueCommands.push_back(requestData); + } + break; + + case ProcessDataCommands::MeasurementTimeInterval: + { + ProcessDataCallbackInfo commandData = { 0, 0, 0, 0, false, false }; +#if !defined CAN_STACK_DISABLE_THREADS && !defined ARDUINO + const std::lock_guard lock(parentTC->clientMutex); +#endif + + commandData.elementNumber = (static_cast(messageData[0] >> 4) | (static_cast(messageData[1]) << 4)); + commandData.ddi = static_cast(messageData[2]) | + (static_cast(messageData[3]) << 8); + commandData.processDataValue = (static_cast(messageData[4]) | + (static_cast(messageData[5]) << 8) | + (static_cast(messageData[6]) << 16) | + (static_cast(messageData[7]) << 24)); + commandData.lastValue = SystemTiming::get_timestamp_ms(); + + auto previousCommand = std::find(parentTC->measurementTimeIntervalCommands.begin(), parentTC->measurementTimeIntervalCommands.end(), commandData); + if (parentTC->measurementTimeIntervalCommands.end() == previousCommand) + { + parentTC->measurementTimeIntervalCommands.push_back(commandData); + CANStackLogger::debug("[TC]: TC Requests element: " + + isobus::to_string(static_cast(commandData.elementNumber)) + + " DDI: " + + isobus::to_string(static_cast(commandData.ddi)) + + " every: " + + isobus::to_string(static_cast(commandData.processDataValue)) + + " milliseconds."); + } + else + { + // Use the existing one and update the value + previousCommand->processDataValue = commandData.processDataValue; + CANStackLogger::debug("[TC]: TC Altered time interval request for element: " + + isobus::to_string(static_cast(commandData.elementNumber)) + + " DDI: " + + isobus::to_string(static_cast(commandData.ddi)) + + " every: " + + isobus::to_string(static_cast(commandData.processDataValue)) + + " milliseconds."); + } + } + break; + + case ProcessDataCommands::MeasurementMaximumWithinThreshold: + { + ProcessDataCallbackInfo commandData = { 0, 0, 0, 0, false, false }; +#if !defined CAN_STACK_DISABLE_THREADS && !defined ARDUINO + const std::lock_guard lock(parentTC->clientMutex); +#endif + + commandData.elementNumber = (static_cast(messageData[0] >> 4) | (static_cast(messageData[1]) << 4)); + commandData.ddi = static_cast(messageData[2]) | + (static_cast(messageData[3]) << 8); + commandData.processDataValue = (static_cast(messageData[4]) | + (static_cast(messageData[5]) << 8) | + (static_cast(messageData[6]) << 16) | + (static_cast(messageData[7]) << 24)); + + auto previousCommand = std::find(parentTC->measurementMaximumThresholdCommands.begin(), parentTC->measurementMaximumThresholdCommands.end(), commandData); + if (parentTC->measurementMaximumThresholdCommands.end() == previousCommand) + { + parentTC->measurementMaximumThresholdCommands.push_back(commandData); + CANStackLogger::debug("[TC]: TC Requests element: " + + isobus::to_string(static_cast(commandData.elementNumber)) + + " DDI: " + + isobus::to_string(static_cast(commandData.ddi)) + + " when it is above the raw value: " + + isobus::to_string(static_cast(commandData.processDataValue))); + } + else + { + // Just update the existing one with the new value + previousCommand->processDataValue = commandData.processDataValue; + previousCommand->thresholdPassed = false; + } + } + break; + + case ProcessDataCommands::MeasurementMinimumWithinThreshold: + { + ProcessDataCallbackInfo commandData = { 0, 0, 0, 0, false, false }; +#if !defined CAN_STACK_DISABLE_THREADS && !defined ARDUINO + const std::lock_guard lock(parentTC->clientMutex); +#endif + + commandData.elementNumber = (static_cast(messageData[0] >> 4) | (static_cast(messageData[1]) << 4)); + commandData.ddi = static_cast(messageData[2]) | + (static_cast(messageData[3]) << 8); + commandData.processDataValue = (static_cast(messageData[4]) | + (static_cast(messageData[5]) << 8) | + (static_cast(messageData[6]) << 16) | + (static_cast(messageData[7]) << 24)); + + auto previousCommand = std::find(parentTC->measurementMinimumThresholdCommands.begin(), parentTC->measurementMinimumThresholdCommands.end(), commandData); + if (parentTC->measurementMinimumThresholdCommands.end() == previousCommand) + { + parentTC->measurementMinimumThresholdCommands.push_back(commandData); + CANStackLogger::debug("[TC]: TC Requests Element " + + isobus::to_string(static_cast(commandData.elementNumber)) + + " DDI: " + + isobus::to_string(static_cast(commandData.ddi)) + + " when it is below the raw value: " + + isobus::to_string(static_cast(commandData.processDataValue))); + } + else + { + // Just update the existing one with the new value + previousCommand->processDataValue = commandData.processDataValue; + previousCommand->thresholdPassed = false; + } + } + break; + + case ProcessDataCommands::MeasurementChangeThreshold: + { + ProcessDataCallbackInfo commandData = { 0, 0, 0, 0, false, false }; +#if !defined CAN_STACK_DISABLE_THREADS && !defined ARDUINO + const std::lock_guard lock(parentTC->clientMutex); +#endif + + commandData.elementNumber = (static_cast(messageData[0] >> 4) | (static_cast(messageData[1]) << 4)); + commandData.ddi = static_cast(messageData[2]) | + (static_cast(messageData[3]) << 8); + commandData.processDataValue = (static_cast(messageData[4]) | + (static_cast(messageData[5]) << 8) | + (static_cast(messageData[6]) << 16) | + (static_cast(messageData[7]) << 24)); + + auto previousCommand = std::find(parentTC->measurementOnChangeThresholdCommands.begin(), parentTC->measurementOnChangeThresholdCommands.end(), commandData); + if (parentTC->measurementOnChangeThresholdCommands.end() == previousCommand) + { + parentTC->measurementOnChangeThresholdCommands.push_back(commandData); + CANStackLogger::debug("[TC]: TC Requests element " + + isobus::to_string(static_cast(commandData.elementNumber)) + + " DDI: " + + isobus::to_string(static_cast(commandData.ddi)) + + " on change by at least: " + + isobus::to_string(static_cast(commandData.processDataValue))); + } + else + { + // Just update the existing one with the new value + previousCommand->processDataValue = commandData.processDataValue; + previousCommand->thresholdPassed = false; + } + } + break; + + case ProcessDataCommands::ProcessDataAcknowledge: + { + if (0 != messageData[4]) + { + CANStackLogger::warn("[TC]: TC sent us a PDNACK"); + } + } + break; + + default: + { + CANStackLogger::warn("[TC]: Unhandled process data message!"); + } + break; + } + } + break; + + default: + { + } + break; + } + } + } + + bool TaskControllerClient::process_internal_object_pool_upload_callback(std::uint32_t, + std::uint32_t bytesOffset, + std::uint32_t numberOfBytesNeeded, + std::uint8_t *chunkBuffer, + void *parentPointer) + { + auto parentTCClient = static_cast(parentPointer); + bool retVal = false; + + // These assertions should never fail, but if they do, please consider reporting it on our GitHub page + // along with a CAN trace and accompanying CANStackLogger output of the issue. + assert(nullptr != parentTCClient); + assert(nullptr != chunkBuffer); + assert(0 != numberOfBytesNeeded); + + if (((bytesOffset + numberOfBytesNeeded) <= parentTCClient->generatedBinaryDDOP.size() + 1) || + ((bytesOffset + numberOfBytesNeeded) <= parentTCClient->userSuppliedBinaryDDOPSize_bytes + 1)) + { + retVal = true; + if (0 == bytesOffset) + { + chunkBuffer[0] = static_cast(ProcessDataCommands::DeviceDescriptor) | + (static_cast(DeviceDescriptorCommands::ObjectPoolTransfer) << 4); + + if (DDOPUploadType::UserProvidedBinaryPointer == parentTCClient->ddopUploadMode) + { + memcpy(&chunkBuffer[1], &parentTCClient->userSuppliedBinaryDDOP[bytesOffset], numberOfBytesNeeded - 1); + } + else + { + memcpy(&chunkBuffer[1], &parentTCClient->generatedBinaryDDOP[bytesOffset], numberOfBytesNeeded - 1); + } + } + else + { + if (DDOPUploadType::UserProvidedBinaryPointer == parentTCClient->ddopUploadMode) + { + // Subtract off 1 to account for the mux in the first byte of the message + memcpy(chunkBuffer, &parentTCClient->userSuppliedBinaryDDOP[bytesOffset - 1], numberOfBytesNeeded); + } + else + { + // Subtract off 1 to account for the mux in the first byte of the message + memcpy(chunkBuffer, &parentTCClient->generatedBinaryDDOP[bytesOffset - 1], numberOfBytesNeeded); + } + } + } + else + { + CANStackLogger::error("[TC]: DDOP internal data callback received out of range request."); + } + return retVal; + } + + void TaskControllerClient::process_tx_callback(std::uint32_t parameterGroupNumber, + std::uint32_t, + std::shared_ptr, + std::shared_ptr destinationControlFunction, + bool successful, + void *parentPointer) + { + if ((nullptr != parentPointer) && + (static_cast(CANLibParameterGroupNumber::ProcessData) == parameterGroupNumber) && + (nullptr != destinationControlFunction)) + { + auto parent = reinterpret_cast(parentPointer); + + if (StateMachineState::WaitForDDOPTransfer == parent->get_state()) + { + if (successful) + { + parent->set_state(StateMachineState::WaitForObjectPoolTransferResponse); + } + else + { + CANStackLogger::error("[TC]: DDOP upload did not complete. Resetting."); + parent->set_state(StateMachineState::Disconnected); + } + } + } + } + + bool TaskControllerClient::send_delete_object_pool() const + { + return send_generic_process_data(static_cast(ProcessDataCommands::DeviceDescriptor) | + (static_cast(DeviceDescriptorCommands::ObjectPoolDelete) << 4)); + } + + bool TaskControllerClient::send_generic_process_data(std::uint8_t multiplexor) const + { + const std::array buffer = { multiplexor, + 0xFF, + 0xFF, + 0xFF, + 0xFF, + 0xFF, + 0xFF, + 0xFF }; + + return CANNetworkManager::CANNetwork.send_can_message(static_cast(CANLibParameterGroupNumber::ProcessData), + buffer.data(), + CAN_DATA_LENGTH, + myControlFunction, + partnerControlFunction); + } + + bool TaskControllerClient::send_object_pool_activate() const + { + return send_generic_process_data(static_cast(ProcessDataCommands::DeviceDescriptor) | + (static_cast(DeviceDescriptorCommands::ObjectPoolActivateDeactivate) << 4)); + } + + bool TaskControllerClient::send_object_pool_deactivate() const + { + const std::array buffer = { static_cast(ProcessDataCommands::DeviceDescriptor) | + (static_cast(DeviceDescriptorCommands::ObjectPoolActivateDeactivate) << 4), + 0x00, + 0xFF, + 0xFF, + 0xFF, + 0xFF, + 0xFF, + 0xFF }; + + return CANNetworkManager::CANNetwork.send_can_message(static_cast(CANLibParameterGroupNumber::ProcessData), + buffer.data(), + CAN_DATA_LENGTH, + myControlFunction, + partnerControlFunction); + } + + bool TaskControllerClient::send_pdack(std::uint16_t elementNumber, std::uint16_t ddi) const + { + const std::array buffer = { static_cast(static_cast(ProcessDataCommands::ProcessDataAcknowledge) | + static_cast(elementNumber & 0x0F) << 4), + static_cast(elementNumber >> 4), + static_cast(ddi & 0xFF), + static_cast(ddi >> 8), + 0xFF, + 0xFF, + 0xFF, + 0xFF }; + return CANNetworkManager::CANNetwork.send_can_message(static_cast(CANLibParameterGroupNumber::ProcessData), + buffer.data(), + CAN_DATA_LENGTH, + myControlFunction, + partnerControlFunction); + } + + bool TaskControllerClient::send_request_localization_label() const + { + return send_generic_process_data(static_cast(ProcessDataCommands::DeviceDescriptor) | + (static_cast(DeviceDescriptorCommands::RequestLocalizationLabel) << 4)); + } + + bool TaskControllerClient::send_request_object_pool_transfer() const + { + std::size_t binaryPoolSize = generatedBinaryDDOP.size(); + + if (DDOPUploadType::UserProvidedBinaryPointer == ddopUploadMode) + { + binaryPoolSize = userSuppliedBinaryDDOPSize_bytes; + } + + const std::array buffer = { static_cast(ProcessDataCommands::DeviceDescriptor) | + (static_cast(DeviceDescriptorCommands::RequestObjectPoolTransfer) << 4), + static_cast(binaryPoolSize & 0xFF), + static_cast((binaryPoolSize >> 8) & 0xFF), + static_cast((binaryPoolSize >> 16) & 0xFF), + static_cast((binaryPoolSize >> 24) & 0xFF), + 0xFF, + 0xFF, + 0xFF }; + + return CANNetworkManager::CANNetwork.send_can_message(static_cast(CANLibParameterGroupNumber::ProcessData), + buffer.data(), + CAN_DATA_LENGTH, + myControlFunction, + partnerControlFunction); + } + + bool TaskControllerClient::send_request_structure_label() const + { + // When all bytes are 0xFF, the TC will tell us about the latest structure label + return send_generic_process_data(static_cast(ProcessDataCommands::DeviceDescriptor) | + (static_cast(DeviceDescriptorCommands::RequestStructureLabel) << 4)); + } + + bool TaskControllerClient::send_request_version_response() const + { + const std::array buffer = { (static_cast(TechnicalDataMessageCommands::ParameterVersion) << 4), + static_cast(Version::SecondPublishedEdition), + 0xFF, // Must be 0xFF when a client sends it (boot time) + static_cast(static_cast(supportsDocumentation) | + (static_cast(supportsTCGEOWithoutPositionBasedControl) << 1) | + (static_cast(supportsTCGEOWithPositionBasedControl) << 2) | + (static_cast(supportsPeerControlAssignment) << 3) | + (static_cast(supportsImplementSectionControl) << 4)), + 0x00, + numberBoomsSupported, + numberSectionsSupported, + numberChannelsSupportedForPositionBasedControl }; + + return CANNetworkManager::CANNetwork.send_can_message(static_cast(CANLibParameterGroupNumber::ProcessData), + buffer.data(), + CAN_DATA_LENGTH, + myControlFunction, + partnerControlFunction); + } + + bool TaskControllerClient::send_status() const + { + const std::array buffer = { static_cast(ProcessDataCommands::ClientTask) | 0xF0, + 0xFF, // Element number N/A + 0xFF, // DDI N/A + 0xFF, // DDI N/A + static_cast(tcStatusBitfield & 0x01), // Actual TC or DL status + 0x00, // Reserved (0) + 0x00, // Reserved (0) + 0x00 }; // Reserved (0) + + return CANNetworkManager::CANNetwork.send_can_message(static_cast(CANLibParameterGroupNumber::ProcessData), + buffer.data(), + CAN_DATA_LENGTH, + myControlFunction, + partnerControlFunction); + } + + bool TaskControllerClient::send_value_command(std::uint16_t elementNumber, std::uint16_t ddi, std::uint32_t value) const + { + const std::array buffer = { static_cast(static_cast(ProcessDataCommands::Value) | + (static_cast(elementNumber & 0x0F) << 4)), + static_cast(elementNumber >> 4), + static_cast(ddi & 0xFF), + static_cast(ddi >> 8), + static_cast(value), + static_cast(value >> 8), + static_cast(value >> 16), + static_cast(value >> 24) }; + return CANNetworkManager::CANNetwork.send_can_message(static_cast(CANLibParameterGroupNumber::ProcessData), + buffer.data(), + CAN_DATA_LENGTH, + myControlFunction, + partnerControlFunction); + } + + bool TaskControllerClient::send_version_request() const + { + return send_generic_process_data(static_cast(TechnicalDataMessageCommands::ParameterRequestVersion)); + } + + bool TaskControllerClient::send_working_set_master() const + { + const std::array buffer = { numberOfWorkingSetMembers, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF }; + + return CANNetworkManager::CANNetwork.send_can_message(static_cast(CANLibParameterGroupNumber::WorkingSetMaster), + buffer.data(), + CAN_DATA_LENGTH, + myControlFunction, + nullptr); + } + + void TaskControllerClient::set_common_config_items(std::uint8_t maxNumberBoomsSupported, + std::uint8_t maxNumberSectionsSupported, + std::uint8_t maxNumberChannelsSupportedForPositionBasedControl, + bool reportToTCSupportsDocumentation, + bool reportToTCSupportsTCGEOWithoutPositionBasedControl, + bool reportToTCSupportsTCGEOWithPositionBasedControl, + bool reportToTCSupportsPeerControlAssignment, + bool reportToTCSupportsImplementSectionControl) + { + numberBoomsSupported = maxNumberBoomsSupported; + numberSectionsSupported = maxNumberSectionsSupported; + numberChannelsSupportedForPositionBasedControl = maxNumberChannelsSupportedForPositionBasedControl; + supportsDocumentation = reportToTCSupportsDocumentation; + supportsTCGEOWithoutPositionBasedControl = reportToTCSupportsTCGEOWithoutPositionBasedControl; + supportsTCGEOWithPositionBasedControl = reportToTCSupportsTCGEOWithPositionBasedControl; + supportsPeerControlAssignment = reportToTCSupportsPeerControlAssignment; + supportsImplementSectionControl = reportToTCSupportsImplementSectionControl; + } + + void TaskControllerClient::set_state(StateMachineState newState) + { + if (newState != currentState) + { + stateMachineTimestamp_ms = SystemTiming::get_timestamp_ms(); + currentState = newState; + + if (StateMachineState::Disconnected == newState) + { + clear_queues(); + } + } + } + + void TaskControllerClient::set_state(StateMachineState newState, std::uint32_t timestamp) + { + stateMachineTimestamp_ms = timestamp; + currentState = newState; + } + + void TaskControllerClient::worker_thread_function() + { +#if !defined CAN_STACK_DISABLE_THREADS && !defined ARDUINO + for (;;) + { + if (shouldTerminate) + { + break; + } + update(); + std::this_thread::sleep_for(std::chrono::milliseconds(50)); + } +#endif + } + + TaskControllerClient::StateMachineState TaskControllerClient::get_state() const + { + return currentState; + } + + std::uint8_t TaskControllerClient::get_connected_tc_number_booms_supported() const + { + return serverNumberOfBoomsForSectionControl; + } + + std::uint8_t TaskControllerClient::get_connected_tc_number_sections_supported() const + { + return serverNumberOfSectionsForSectionControl; + } + + std::uint8_t TaskControllerClient::get_connected_tc_number_channels_supported() const + { + return serverNumberOfChannelsForPositionBasedControl; + } + + std::uint8_t TaskControllerClient::get_connected_tc_max_boot_time() const + { + return maxServerBootTime_s; + } + + bool TaskControllerClient::get_connected_tc_option_supported(ServerOptions option) const + { + return (0 != (static_cast(option) & serverOptionsByte1)); + } + + TaskControllerClient::Version TaskControllerClient::get_connected_tc_version() const + { + Version retVal = Version::Unknown; + + if (serverVersion <= static_cast(Version::SecondPublishedEdition)) + { + retVal = static_cast(serverVersion); + } + return retVal; + } + + void TaskControllerClient::on_value_changed_trigger(std::uint16_t elementNumber, std::uint16_t DDI) + { + ProcessDataCallbackInfo requestData = { 0, 0, 0, 0, false, false }; +#if !defined CAN_STACK_DISABLE_THREADS && !defined ARDUINO + const std::lock_guard lock(clientMutex); +#endif + + requestData.ackRequested = false; + requestData.elementNumber = elementNumber; + requestData.ddi = DDI; + requestData.processDataValue = 0; + queuedValueRequests.push_back(requestData); + } + + bool TaskControllerClient::request_task_controller_identification() const + { + constexpr std::array buffer = { static_cast(ProcessDataCommands::TechnicalCapabilities) | + (static_cast(TechnicalDataMessageCommands::IdentifyTaskController) << 4), + 0xFF, + 0xFF, + 0xFF, + 0xFF, + 0xFF, + 0xFF, + 0xFF }; + return CANNetworkManager::CANNetwork.send_can_message(static_cast(CANLibParameterGroupNumber::ProcessData), + buffer.data(), + CAN_DATA_LENGTH, + myControlFunction, + nullptr); + } + +} // namespace isobus diff --git a/src/isobus_task_controller_client.hpp b/src/isobus_task_controller_client.hpp new file mode 100644 index 0000000..75d046f --- /dev/null +++ b/src/isobus_task_controller_client.hpp @@ -0,0 +1,618 @@ +//================================================================================================ +/// @file isobus_task_controller_client.hpp +/// +/// @brief A class to manage a client connection to a ISOBUS field computer's task controller +/// @author Adrian Del Grosso +/// +/// @copyright 2022 Adrian Del Grosso +//================================================================================================ +#ifndef ISOBUS_TASK_CONTROLLER_CLIENT_HPP +#define ISOBUS_TASK_CONTROLLER_CLIENT_HPP + +#include "can_internal_control_function.hpp" +#include "can_partnered_control_function.hpp" +#include "isobus_device_descriptor_object_pool.hpp" +#include "isobus_language_command_interface.hpp" +#include "processing_flags.hpp" + +#include +#include + +namespace isobus +{ + class VirtualTerminalClient; // Forward declaring VT client + + /// @brief A class to manage a client connection to a ISOBUS field computer's task controller or data logger + class TaskControllerClient + { + public: + /// @brief Enumerates the different internal state machine states + enum class StateMachineState + { + Disconnected, ///< Not communicating with the TC + WaitForStartUpDelay, ///< Client is waiting for the mandatory 6s startup delay + WaitForServerStatusMessage, ///< Client is waiting to identify the TC via reception of a valid status message + SendWorkingSetMaster, ///< Client initating communication with TC by sending the working set master message + SendStatusMessage, ///< Enables sending the status message + RequestVersion, ///< Requests the TC version and related data from the TC + WaitForRequestVersionResponse, ///< Waiting for the TC to respond to a request for its version + WaitForRequestVersionFromServer, ///< Waiting to see if the TC will request our version (optional) + SendRequestVersionResponse, ///< Sending our response to the TC's request for out version information + RequestLanguage, ///< Client is requesting the language command PGN from the TC + WaitForLanguageResponse, ///< Waiting for a response to our request for the language command PGN + ProcessDDOP, ///< Client is processing the DDOP into a binary DDOP and validating object IDs in the pool + RequestStructureLabel, ///< Client is requesting the DDOP structure label that the TC has (if any) + WaitForStructureLabelResponse, ///< Client is waiting for the TC to respond to our request for its structure label + RequestLocalizationLabel, ///< Client is requesting the DDOP localization label the TC has for us (if any) + WaitForLocalizationLabelResponse, ///< Waiting for a response to our request for the localization label from the TC + SendDeleteObjectPool, ///< Client is sending a request to the TC to delete its current copy of our object pool + WaitForDeleteObjectPoolResponse, ///< Waiting for a response to our request to delete our object pool off the TC + SendRequestTransferObjectPool, ///< Client is requesting to transfer the DDOP to the TC + WaitForRequestTransferObjectPoolResponse, ///< Waiting for a response to our request to transfer the DDOP to the TC + BeginTransferDDOP, ///< Client is initiating the DDOP transfer + WaitForDDOPTransfer, ///< The DDOP transfer in ongoing. Client is waiting for a callback from the transport layer. + WaitForObjectPoolTransferResponse, ///< DDOP has transferred. Waiting for a response to our object pool transfer. + SendObjectPoolActivate, ///< Client is sending the activate object pool message + WaitForObjectPoolActivateResponse, ///< Client is waiting for a response to its request to activate the object pool + Connected, ///< TC is connected + DeactivateObjectPool, ///< Client is shutting down and is therefore sending the deactivate object pool message + WaitForObjectPoolDeactivateResponse ///< Client is waiting for a response to the deactivate object pool message + }; + + /// @brief Enumerates the different task controller versions + enum class Version : std::uint8_t + { + DraftInternationalStandard = 0, ///< The version of the DIS (draft International Standard). + FinalDraftInternationalStandardFirstEdition = 1, ///< The version of the FDIS.1 (final draft International Standard, first edition). + FirstPublishedEdition = 2, ///< The version of the FDIS.2 and the first edition published ss an International Standard. + SecondEditionDraft = 3, ///< The version of the second edition published as a draft International Standard(E2.DIS). + SecondPublishedEdition = 4, ///< The version of the second edition published as the final draft International Standard(E2.FDIS) and as the International Standard(E2.IS) + Unknown = 0xFF + }; + + /// @brief Enumerates the bits stored in our version data that we send to the TC when handshaking + enum class ServerOptions : std::uint8_t + { + SupportsDocumentation = 0x01, + SupportsTCGEOWithoutPositionBasedControl = 0x02, + SupportsTCGEOWithPositionBasedControl = 0x04, + SupportsPeerControlAssignment = 0x08, + SupportsImplementSectionControlFunctionality = 0x10, + ReservedOption1 = 0x20, + ReservedOption2 = 0x40, + ReservedOption3 = 0x80 + }; + + /// @brief A callback for handling a value request command from the TC + using RequestValueCommandCallback = bool (*)(std::uint16_t elementNumber, + std::uint16_t DDI, + std::uint32_t &processVariableValue, + void *parentPointer); + + /// @brief A callback for handling a set value command from the TC + using ValueCommandCallback = bool (*)(std::uint16_t elementNumber, + std::uint16_t DDI, + std::uint32_t processVariableValue, + void *parentPointer); + + /// @brief The constructor for a TaskControllerClient + /// @param[in] partner The TC server control function + /// @param[in] clientSource The internal control function to communicate from + /// @param[in] primaryVT Pointer to our primary VT. This is optional (can be nullptr), but should be provided if possible to provide the best compatibility to TC < version 4. + TaskControllerClient(std::shared_ptr partner, std::shared_ptr clientSource, std::shared_ptr primaryVT); + + /// @brief Destructor for the client + ~TaskControllerClient(); + + // Setup Functions + /// @brief This function starts the state machine. Call this once you have created your DDOP, set up the client capabilities, and are ready to connect. + /// @param[in] spawnThread The client will start a thread to manage itself if this parameter is true. Otherwise you must update it cyclically + /// by calling the `update` function. + void initialize(bool spawnThread); + + /// @brief This adds a callback that will be called when the TC requests the value of one of your variables. + /// @details The task controller will often send a request for the value of a process data variable. + /// When the stack recieves those messages, it will call this callback to request the value from your + /// application. You must provide the value at that time for the associated process data variable identified + /// by its element number and DDI. + /// @param[in] callback The callback to add + /// @param[in] parentPointer A generic context variable that will be passed into the associated callback when it gets called + void add_request_value_callback(RequestValueCommandCallback callback, void *parentPointer); + + /// @brief Adds a callback that will be called when the TC commands a new value for one of your variables. + /// @details The task controller will often send a command to set one of your process data variables to a new value. + /// This callback will get called when that happens, and you will need to set the variable to the commanded value in your + /// application. + /// @param[in] callback The callback to add + /// @param[in] parentPointer A generic context variable that will be passed into the associated callback when it gets called + void add_value_command_callback(ValueCommandCallback callback, void *parentPointer); + + /// @brief Removes the specified callback from the list of value request callbacks + /// @param[in] callback The callback to remove + /// @param[in] parentPointer parent pointer associated to the callback being removed + void remove_request_value_callback(RequestValueCommandCallback callback, void *parentPointer); + + /// @brief Removes the specified callback from the list of value command callbacks + /// @param[in] callback The callback to remove + /// @param[in] parentPointer parent pointer associated to the callback being removed + void remove_value_command_callback(ValueCommandCallback callback, void *parentPointer); + + /// @brief A convenient way to set all client options at once instead of calling the individual setters + /// @details This function sets up the parameters that the client will report to the TC server. + /// These parameters should be tailored to your specific application. + /// @note This version of the configure function takes a DeviceDescriptorObjectPool. + /// The other versions of the configure function take various other kinds of DDOP. + /// @param[in] DDOP The device descriptor object pool to upload to the TC + /// @param[in] maxNumberBoomsSupported Configures the max number of booms the client supports + /// @param[in] maxNumberSectionsSupported Configures the max number of sections supported by the client for section control + /// @param[in] maxNumberChannelsSupportedForPositionBasedControl Configures the max number of channels supported by the client for position based control + /// @param[in] reportToTCSupportsDocumentation Denotes if your app supports documentation + /// @param[in] reportToTCSupportsTCGEOWithoutPositionBasedControl Denotes if your app supports TC-GEO without position based control + /// @param[in] reportToTCSupportsTCGEOWithPositionBasedControl Denotes if your app supports TC-GEO with position based control + /// @param[in] reportToTCSupportsPeerControlAssignment Denotes if your app supports peer control assignment + /// @param[in] reportToTCSupportsImplementSectionControl Denotes if your app supports implement section control + void configure(std::shared_ptr DDOP, + std::uint8_t maxNumberBoomsSupported, + std::uint8_t maxNumberSectionsSupported, + std::uint8_t maxNumberChannelsSupportedForPositionBasedControl, + bool reportToTCSupportsDocumentation, + bool reportToTCSupportsTCGEOWithoutPositionBasedControl, + bool reportToTCSupportsTCGEOWithPositionBasedControl, + bool reportToTCSupportsPeerControlAssignment, + bool reportToTCSupportsImplementSectionControl); + + /// @brief A convenient way to set all client options at once instead of calling the individual setters + /// @details This function sets up the parameters that the client will report to the TC server. + /// These parameters should be tailored to your specific application. + /// @note This version of the configure function takes a pointer to an array of bytes. + /// The other versions of the configure function take various other kinds of DDOP. + /// @param[in] binaryDDOP The device descriptor object pool to upload to the TC + /// @param[in] DDOPSize The number of bytes in the binary DDOP that will be uploaded + /// @param[in] maxNumberBoomsSupported Configures the max number of booms the client supports + /// @param[in] maxNumberSectionsSupported Configures the max number of sections supported by the client for section control + /// @param[in] maxNumberChannelsSupportedForPositionBasedControl Configures the max number of channels supported by the client for position based control + /// @param[in] reportToTCSupportsDocumentation Denotes if your app supports documentation + /// @param[in] reportToTCSupportsTCGEOWithoutPositionBasedControl Denotes if your app supports TC-GEO without position based control + /// @param[in] reportToTCSupportsTCGEOWithPositionBasedControl Denotes if your app supports TC-GEO with position based control + /// @param[in] reportToTCSupportsPeerControlAssignment Denotes if your app supports peer control assignment + /// @param[in] reportToTCSupportsImplementSectionControl Denotes if your app supports implement section control + void configure(std::uint8_t const *binaryDDOP, + std::uint32_t DDOPSize, + std::uint8_t maxNumberBoomsSupported, + std::uint8_t maxNumberSectionsSupported, + std::uint8_t maxNumberChannelsSupportedForPositionBasedControl, + bool reportToTCSupportsDocumentation, + bool reportToTCSupportsTCGEOWithoutPositionBasedControl, + bool reportToTCSupportsTCGEOWithPositionBasedControl, + bool reportToTCSupportsPeerControlAssignment, + bool reportToTCSupportsImplementSectionControl); + + /// @brief A convenient way to set all client options at once instead of calling the individual setters + /// @details This function sets up the parameters that the client will report to the TC server. + /// These parameters should be tailored to your specific application. + /// @note This version of the configure function takes a vector of bytes, and stores a copy of it. + /// The other versions of the configure function take various other kinds of DDOP. + /// @param[in] binaryDDOP The device descriptor object pool to upload to the TC + /// @param[in] maxNumberBoomsSupported Configures the max number of booms the client supports + /// @param[in] maxNumberSectionsSupported Configures the max number of sections supported by the client for section control + /// @param[in] maxNumberChannelsSupportedForPositionBasedControl Configures the max number of channels supported by the client for position based control + /// @param[in] reportToTCSupportsDocumentation Denotes if your app supports documentation + /// @param[in] reportToTCSupportsTCGEOWithoutPositionBasedControl Denotes if your app supports TC-GEO without position based control + /// @param[in] reportToTCSupportsTCGEOWithPositionBasedControl Denotes if your app supports TC-GEO with position based control + /// @param[in] reportToTCSupportsPeerControlAssignment Denotes if your app supports peer control assignment + /// @param[in] reportToTCSupportsImplementSectionControl Denotes if your app supports implement section control + void configure(std::shared_ptr> binaryDDOP, + std::uint8_t maxNumberBoomsSupported, + std::uint8_t maxNumberSectionsSupported, + std::uint8_t maxNumberChannelsSupportedForPositionBasedControl, + bool reportToTCSupportsDocumentation, + bool reportToTCSupportsTCGEOWithoutPositionBasedControl, + bool reportToTCSupportsTCGEOWithPositionBasedControl, + bool reportToTCSupportsPeerControlAssignment, + bool reportToTCSupportsImplementSectionControl); + + // Calling this will stop the worker thread if it exists + /// @brief Terminates the client and joins the worker thread if applicable + void terminate(); + + /// @brief Returns the internal control function being used by the interface to send messages + /// @returns The internal control function being used by the interface to send messages + std::shared_ptr get_internal_control_function() const; + + /// @brief Returns the control function of the TC server with which this TC client communicates. + /// @returns The partner control function for the TC server + std::shared_ptr get_partner_control_function() const; + + /// @brief Returns the previously configured number of booms supported by the client + /// @returns The previously configured number of booms supported by the client + std::uint8_t get_number_booms_supported() const; + + /// @brief Returns the previously configured number of section supported by the client + /// @returns The previously configured number of booms supported by the client + std::uint8_t get_number_sections_supported() const; + + /// @brief Returns the previously configured number of channels supported for position based control + /// @returns The previously configured number of channels supported for position based control + std::uint8_t get_number_channels_supported_for_position_based_control() const; + + /// @brief Returns if the client has been configured to report that it supports documentation to the TC + /// @returns `true` if the client has been configured to report that it supports documentation, otherwise `false` + bool get_supports_documentation() const; + + /// @brief Returns if the client has been configured to report that it supports TC-GEO without position based control to the TC + /// @returns `true` if the client has been configured to report that it supports TC-GEO without position based control, otherwise `false` + bool get_supports_tcgeo_without_position_based_control() const; + + /// @brief Returns if the client has been configured to report that it supports TC-GEO with position based control to the TC + /// @returns `true` if the client has been configured to report that it supports TC-GEO with position based control, otherwise `false` + bool get_supports_tcgeo_with_position_based_control() const; + + /// @brief Returns if the client has been configured to report that it supports peer control assignment to the TC + /// @returns `true` if the client has been configured to report that it supports peer control assignment, otherwise `false` + bool get_supports_peer_control_assignment() const; + + /// @brief Returns if the client has been configured to report that it supports implement section control to the TC + /// @returns `true` if the client has been configured to report that it supports implement section control, otherwise `false` + bool get_supports_implement_section_control() const; + + /// @brief Returns if the client has been initialized + /// @note This does not mean that the client is connected to the TC server + /// @returns true if the client has been initialized + bool get_is_initialized() const; + + /// @brief Check whether the client is connected to the TC server + /// @returns true if cconnected, false otherwise + bool get_is_connected() const; + + /// @brief Returns if a task is active as indicated by the TC + /// @attention Some TCs will report they are always in a task rather than properly reporting this. + /// For example, John Deere TCs have a bad habit of doing this. + /// Use caution before relying on the TC's task status. + /// @returns `true` if the TC is connected and the TC is reporting it is in a task, otherwise `false` + bool get_is_task_active() const; + + /// @brief Returns the current state machine state + /// @returns The current internal state machine state + StateMachineState get_state() const; + + /// @brief Returns the number of booms that the connected TC supports for section control + /// @returns Number of booms that the connected TC supports for section control + std::uint8_t get_connected_tc_number_booms_supported() const; + + /// @brief Returns the number of sections that the connected TC supports for section control + /// @returns Number of sections that the connected TC supports for section control + std::uint8_t get_connected_tc_number_sections_supported() const; + + /// @brief Returns the number of channels that the connected TC supports for position control + /// @returns Number of channels that the connected TC supports for position control + std::uint8_t get_connected_tc_number_channels_supported() const; + + /// @brief Returns the maximum boot time in seconds reported by the connected TC + /// @returns Maximum boot time (seconds) reported by the connected TC, or 0xFF if that info is not available. + std::uint8_t get_connected_tc_max_boot_time() const; + + /// @brief Returns if the connected TC supports a certain option + /// @param[in] option The option to check against + /// @returns `true` if the option was reported as "supported" by the TC, otherwise `false` + bool get_connected_tc_option_supported(ServerOptions option) const; + + /// @brief Returns the version of the connected task controller + /// @returns The version reported by the connected task controller + Version get_connected_tc_version() const; + + /// @brief Tells the TC client that a value was changed or the TC client needs to command + /// a value to the TC server. + /// @details If you provide on-change triggers in your DDOP, this is how you can request the TC client + /// to update the TC server on the current value of your process data variables. + void on_value_changed_trigger(std::uint16_t elementNumber, std::uint16_t DDI); + + /// @brief Sends a broadcast request to TCs to identify themseleves. + /// @details Upon receipt of this message, the TC shall display, for a period of 3 s, the TC Number + /// @returns `true` if the message was sent, otherwise `false` + bool request_task_controller_identification() const; + + /// @brief The cyclic update function for this interface. + /// @note This function may be called by the TC worker thread if you called + /// initialize with a parameter of `true`, otherwise you must call it + /// yourself at some interval. + void update(); + + /// @brief Used to determine the language and unit systems in use by the TC server + LanguageCommandInterface languageCommandInterface; + + protected: + /// @brief Enumerates the different Process Data commands from ISO11783-10 Table B.1 + enum class ProcessDataCommands : std::uint8_t + { + TechnicalCapabilities = 0x00, ///< Used for determining the technical capabilities of a TC, DL, or client. + DeviceDescriptor = 0x01, ///< Subcommand for the transfer and management of device descriptors + RequestValue = 0x02, ///< The value of the data entity specified by the data dictionary identifier is requested. + Value = 0x03, ///< This command is used both to answer a request value command and to set the value of a process data entity. + MeasurementTimeInterval = 0x04, ///< The process data value is the time interval for sending the data element specified by the data dictionary identifier. + MeasurementDistanceInterval = 0x05, ///< The process data value is the distance interval for sending the data element specified by the data dictionary identifier. + MeasurementMinimumWithinThreshold = 0x06, ///< The client has to send the value of this data element to the TC or DL when the value is higher than the threshold value + MeasurementMaximumWithinThreshold = 0x07, ///< The client has to send the value of this data element to the TC or DL when the value is lower than the threshold value. + MeasurementChangeThreshold = 0x08, ///< The client has to send the value of this data element to the TC or DL when the value change is higher than or equal to the change threshold since last transmission. + PeerControlAssignment = 0x09, ///< This message is used to establish a connection between a setpoint value source and a setpoint value user + SetValueAndAcknowledge = 0x0A, ///< This command is used to set the value of a process data entity and request a reception acknowledgement from the recipient + Reserved1 = 0x0B, ///< Reserved. + Reserved2 = 0x0C, ///< Reserved. + ProcessDataAcknowledge = 0x0D, ///< Message is a Process Data Acknowledge (PDACK). + StatusMessage = 0x0E, ///< Message is a Task Controller Status message + ClientTask = 0x0F ///< Sent by the client + }; + + /// @brief Enumerates the subcommands within the technical data message group + enum class TechnicalDataMessageCommands : std::uint8_t + { + ParameterRequestVersion = 0x00, ///< The Request Version message allows the TC, DL, and the client to determine the ISO 11783-10 version of the implementation + ParameterVersion = 0x01, ///< The Version message is sent in response to the request version message and contains the ISO 11783-10 version information of the TC, DL, or client implementation + IdentifyTaskController = 0x02 ///< Upon receipt of this message, the TC shall display, for a period of 3 s, the TC Number. + }; + + /// @brief Enumerates the subcommands within the device descriptor command message group + enum class DeviceDescriptorCommands : std::uint8_t + { + RequestStructureLabel = 0x00, ///< Allows the client to determine the availability of the requested device descriptor structure + StructureLabel = 0x01, ///< The Structure Label message is sent by the TC or DL to inform the client about the availability of the requested version of the device descriptor structure + RequestLocalizationLabel = 0x02, ///< Allows the client to determine the availability of the requested device descriptor localization + LocalizationLabel = 0x03, ///< Sent by the TC or DL to inform the client about the availability of the requested localization version of the device descriptor + RequestObjectPoolTransfer = 0x04, ///< The Request Object-pool Transfer message allows the client to determine whether it is allowed to transfer(part of) the device descriptor object pool to the TC + RequestObjectPoolTransferResponse = 0x05, ///< Sent in response to Request Object-pool Transfer message + ObjectPoolTransfer = 0x06, ///< Enables the client to transfer (part of) the device descriptor object pool to the TC + ObjectPoolTransferResponse = 0x07, ///< Response to an object pool transfer + ObjectPoolActivateDeactivate = 0x08, ///< sent by a client to complete its connection procedure to a TC or DL or to disconnect from a TC or DL. + ObjectPoolActivateDeactivateResponse = 0x09, ///< sent by a client to complete its connection procedure to a TC or DL or to disconnect from a TC or DL. + ObjectPoolDelete = 0x0A, ///< This is a message to delete the device descriptor object pool for the client that sends this message. + ObjectPoolDeleteResponse = 0x0B, ///< TC response to a Object-pool Delete message + ChangeDesignator = 0x0C, ///< This message is used to update the designator of an object. + ChangeDesignatorResponse = 0x0D ///< Sent in response to Change Designator message + }; + + /// @brief The data callback passed to the network manger's send function for the transport layer messages + /// @details We upload the data with callbacks to avoid making yet another complete copy of the pool to + /// accommodate the multiplexor that needs to get passed to the transport layer message's first byte. + /// @param[in] callbackIndex The number of times the callback has been called + /// @param[in] bytesOffset The byte offset at which to get pool data + /// @param[in] numberOfBytesNeeded The number of bytes the protocol needs to send another frame (usually 7) + /// @param[out] chunkBuffer A pointer through which the data should be returned to the protocol + /// @param[in] parentPointer A context variable that is passed back through the callback + /// @returns true if the data was successfully returned via the callback + static bool process_internal_object_pool_upload_callback(std::uint32_t callbackIndex, + std::uint32_t bytesOffset, + std::uint32_t numberOfBytesNeeded, + std::uint8_t *chunkBuffer, + void *parentPointer); + + /// @brief Clears all queued TC commands and responses + void clear_queues(); + + /// @brief Checks if a DDOP was provided via one of the configure functions + /// @returns true if a DDOP was provided, otherwise false + bool get_was_ddop_supplied() const; + + /// @brief Searches the DDOP for a device object and stores that object's structure and localization labels + void process_labels_from_ddop(); + + /// @brief Processes queued TC requests and commands. Calls the user's callbacks if needed. + void process_queued_commands(); + + /// @brief Processes measurement threshold/interval commands + void process_queued_threshold_commands(); + + /// @brief Processes a CAN message destined for any TC client + /// @param[in] message The CAN message being received + /// @param[in] parentPointer A context variable to find the relevant TC client class + static void process_rx_message(const CANMessage &message, void *parentPointer); + + /// @brief The callback passed to the network manager's send function to know when a Tx is completed + static void process_tx_callback(std::uint32_t parameterGroupNumber, + std::uint32_t dataLength, + std::shared_ptr sourceControlFunction, + std::shared_ptr destinationControlFunction, + bool successful, + void *parentPointer); + + /// @brief Sends the delete object pool command to the TC + /// @details This is a message to delete the device descriptor object pool for the client that sends this message. The + /// Object pool Delete message enables a client to delete the entire device descriptor object pool before sending an + /// updated or changed device descriptor object pool with the object pool transfer message. + /// @returns `true` if the message was sent, otherwise `false` + bool send_delete_object_pool() const; + + /// @brief Sends a process data message with 1 mux byte and all 0xFFs as payload + /// @details This just reduces code duplication by consolidating common message formats + /// @returns `true` if the message was sent, otherwise `false` + bool send_generic_process_data(std::uint8_t multiplexor) const; + + /// @brief Sends the activate object pool message + /// @details This message is sent by a client to complete its connection procedure to a TC + /// @returns `true` if the message was sent, otherwise `false` + bool send_object_pool_activate() const; + + /// @brief Sends the deactivate object pool message + /// @details This message is sent by a client to disconnect from a TC + /// @returns `true` if the message was sent otherwise `false` + bool send_object_pool_deactivate() const; + + /// @brief Sends a Process Data ACK + /// @param[in] elementNumber The element number being acked + /// @param[in] ddi The DDI being acked + /// @returns `true` if the message was sent, otherwise `false` + bool send_pdack(std::uint16_t elementNumber, std::uint16_t ddi) const; + + /// @brief Sends a request to the TC for its localization label + /// @details The Request Localization Label message allows the client to determine the availability of the requested + /// device descriptor localization at the TC or DL.If the requested localization label is present, + /// a localization label message with the requested localization label shall be transmitted by the TC or DL + /// to the sender of the Request Localization Label message. Otherwise, a localization label message with all + /// localization label bytes set to value = 0xFF shall be transmitted by the TC or DL to the sender of the + /// Request Localization Label message. + /// @returns `true` if the message was sent, otherwise `false` + bool send_request_localization_label() const; + + /// @brief Sends a request to the TC indicating we wish to transfer an object pool + /// @details The Request Object-pool Transfer message allows the client to determine whether it is allowed to + /// transfer(part of) the device descriptor object pool to the TC or DL. + /// @returns `true` if the message was sent, otherwise false + bool send_request_object_pool_transfer() const; + + /// @brief Sends a request to the TC for its structure label + /// @details The Request Structure Label message allows the client to determine the availability of the requested + /// device descriptor structure at the TC. If the requested structure label is present, a structure label + /// message with the requested structure label shall be transmitted by the TC or DL to the sender + /// of the Request Structure Label message. Otherwise, a structure label message with 7 structure + /// label bytes set to value = 0xFF shall be transmitted by the TC or DL to the sender of the Request Structure Label message + /// @returns `true` if the message was sent, otherwise `false` + bool send_request_structure_label() const; + + /// @brief Sends the response to a request for version from the TC + /// @returns `true` if the message was sent, otherwise `false` + bool send_request_version_response() const; + + /// @brief Sends the status message to the TC + /// @returns `true` if the message was sent, otherwise false + bool send_status() const; + + /// @brief Sends the value command message for a specific DDI/Element number combo + /// @returns `true` if the message was sent, otherwise `false` + bool send_value_command(std::uint16_t elementNumber, std::uint16_t ddi, std::uint32_t value) const; + + /// @brief Sends the version request message to the TC + /// @returns `true` if the message was sent, otherwise `false` + bool send_version_request() const; + + /// @brief Sends the working set master message + /// @returns `true` if the message was sent, otherwise false + bool send_working_set_master() const; + + /// @brief Sets the common items found in all versions of `configure` + /// @param[in] maxNumberBoomsSupported Configures the max number of booms the client supports + /// @param[in] maxNumberSectionsSupported Configures the max number of sections supported by the client for section control + /// @param[in] maxNumberChannelsSupportedForPositionBasedControl Configures the max number of channels supported by the client for position based control + /// @param[in] reportToTCSupportsDocumentation Denotes if your app supports documentation + /// @param[in] reportToTCSupportsTCGEOWithoutPositionBasedControl Denotes if your app supports TC-GEO without position based control + /// @param[in] reportToTCSupportsTCGEOWithPositionBasedControl Denotes if your app supports TC-GEO with position based control + /// @param[in] reportToTCSupportsPeerControlAssignment Denotes if your app supports peer control assignment + /// @param[in] reportToTCSupportsImplementSectionControl Denotes if your app supports implement section control + void set_common_config_items(std::uint8_t maxNumberBoomsSupported, + std::uint8_t maxNumberSectionsSupported, + std::uint8_t maxNumberChannelsSupportedForPositionBasedControl, + bool reportToTCSupportsDocumentation, + bool reportToTCSupportsTCGEOWithoutPositionBasedControl, + bool reportToTCSupportsTCGEOWithPositionBasedControl, + bool reportToTCSupportsPeerControlAssignment, + bool reportToTCSupportsImplementSectionControl); + + /// @brief Changes the internal state machine state and updates the associated timestamp + /// @param[in] newState The new state for the state machine + void set_state(StateMachineState newState); + + /// @brief Changes the internal state machine state and updates the associated timestamp to the specified one + /// @note This is intended for testing purposes only + /// @param[in] newState The new state for the state machine + /// @param[in] timestamp The new value for the state machine timestamp (in milliseconds) + void set_state(StateMachineState newState, std::uint32_t timestamp); + + /// @brief The worker thread will execute this function when it runs, if applicable + void worker_thread_function(); + + static constexpr std::uint32_t SIX_SECOND_TIMEOUT_MS = 6000; ///< The startup delay time defined in the standard + static constexpr std::uint16_t TWO_SECOND_TIMEOUT_MS = 2000; ///< Used for sending the status message to the TC + + private: + /// @brief Stores data related to requests and commands from the TC + struct ProcessDataCallbackInfo + { + /// @brief Allows easy comparison of callback data + /// @param obj the object to compare against + bool operator==(const ProcessDataCallbackInfo &obj) const; + std::uint32_t processDataValue; ///< The value of the value set command + std::uint32_t lastValue; ///< Used for measurement commands to store timestamp or previous values + std::uint16_t elementNumber; ///< The element number for the command + std::uint16_t ddi; ///< The DDI for the command + bool ackRequested; ///< Stores if the TC used the mux that also requires a PDACK + bool thresholdPassed; ///< Used when the structure is being used to track measurement command thresholds to know if the threshold has been passed + }; + + /// @brief Stores a TC value command callback along with its parent pointer + struct RequestValueCommandCallbackInfo + { + /// @brief Allows easy comparison of callback data + /// @param obj the object to compare against + bool operator==(const RequestValueCommandCallbackInfo &obj) const; + RequestValueCommandCallback callback = nullptr; ///< The callback itself + void *parent; ///< The parent pointer, generic context value + }; + + /// @brief Stores a TC value command callback along with its parent pointer + struct ValueCommandCallbackInfo + { + /// @brief Allows easy comparison of callback data + /// @param obj the object to compare against + bool operator==(const ValueCommandCallbackInfo &obj) const; + ValueCommandCallback callback; ///< The callback itself + void *parent; ///< The parent pointer, generic context value + }; + + /// @brief Enumerates the modes that the client may use when dealing with a DDOP + enum class DDOPUploadType + { + ProgramaticallyGenerated, ///< Using the AgIsoStack++ DeviceDescriptorObjectPool class + UserProvidedBinaryPointer, ///< Using a raw pointer to a binary DDOP + UserProvidedVector ///< Uses a vector of bytes that comprise a binary DDOP + }; + + std::shared_ptr partnerControlFunction; ///< The partner control function this client will send to + std::shared_ptr myControlFunction; ///< The internal control function the client uses to send from + std::shared_ptr primaryVirtualTerminal; ///< A pointer to the primary VT. Used for TCs < version 4 + std::shared_ptr clientDDOP; ///< Stores the DDOP for upload to the TC (if needed) + std::uint8_t const *userSuppliedBinaryDDOP = nullptr; ///< Stores a client-provided DDOP if one was provided + std::shared_ptr> userSuppliedVectorDDOP; ///< Stores a client-provided DDOP if one was provided + std::vector generatedBinaryDDOP; ///< Stores the DDOP in binary form after it has been generated + std::vector requestValueCallbacks; ///< A list of callbacks that will be called when the TC requests a process data value + std::vector valueCommandsCallbacks; ///< A list of callbacks that will be called when the TC sets a process data value + std::list queuedValueRequests; ///< A list of queued value requests that will be processed on the next update + std::list queuedValueCommands; ///< A list of queued value commands that will be processed on the next update + std::list measurementTimeIntervalCommands; ///< A list of measurement commands that will be processed on a time interval + std::list measurementMinimumThresholdCommands; ///< A list of measurement commands that will be processed when the value drops below a threshold + std::list measurementMaximumThresholdCommands; ///< A list of measurement commands that will be processed when the value above a threshold + std::list measurementOnChangeThresholdCommands; ///< A list of measurement commands that will be processed when the value changes by the specified amount +#if !defined CAN_STACK_DISABLE_THREADS && !defined ARDUINO + std::mutex clientMutex; ///< A general mutex to protect data in the worker thread against data accessed by the app or the network manager + std::thread *workerThread = nullptr; ///< The worker thread that updates this interface +#endif + std::string ddopStructureLabel; ///< Stores a pre-parsed structure label, helps to avoid processing the whole DDOP during a CAN message callback + std::array ddopLocalizationLabel = { 0 }; ///< Stores a pre-parsed localization label, helps to avoid processing the whole DDOP during a CAN message callback + DDOPUploadType ddopUploadMode = DDOPUploadType::ProgramaticallyGenerated; ///< Determines if DDOPs get generated or raw uploaded + StateMachineState currentState = StateMachineState::Disconnected; ///< Tracks the internal state machine's current state + std::uint32_t stateMachineTimestamp_ms = 0; ///< Timestamp that tracks when the state machine last changed states (in milliseconds) + std::uint32_t statusMessageTimestamp_ms = 0; ///< Timestamp corresponding to the last time we sent a status message to the TC + std::uint32_t serverStatusMessageTimestamp_ms = 0; ///< Timestamp corresponding to the last time we received a status message from the TC + std::uint32_t userSuppliedBinaryDDOPSize_bytes = 0; ///< The number of bytes in the user provided binary DDOP (if one was provided) + std::uint8_t numberOfWorkingSetMembers = 1; ///< The number of working set members that will be reported in the working set master message + std::uint8_t tcStatusBitfield = 0; ///< The last received TC/DL status from the status message + std::uint8_t sourceAddressOfCommandBeingExecuted = 0; ///< Source address of client for which the current command is being executed + std::uint8_t commandBeingExecuted = 0; ///< The current command the TC is executing as reported in the status message + std::uint8_t serverVersion = 0; ///< The detected version of the TC Server + std::uint8_t maxServerBootTime_s = 0; ///< Maximum number of seconds from a power cycle to transmission of first �Task Controller Status message� or 0xFF + std::uint8_t serverOptionsByte1 = 0; ///< The options specified in ISO 11783-10 that this TC, DL, or client meets (The definition of this byte is introduced in ISO11783-10 version 3) + std::uint8_t serverOptionsByte2 = 0; ///< Reserved for ISO assignment, should be zero or 0xFF. + std::uint8_t serverNumberOfBoomsForSectionControl = 0; ///< When reported by the TC, this is the maximum number of section control booms that are supported + std::uint8_t serverNumberOfSectionsForSectionControl = 0; ///< When reported by the TC, this is the maximum number of sections that are supported (or 0xFF for version 2 and earlier). + std::uint8_t serverNumberOfChannelsForPositionBasedControl = 0; ///< When reported by the TC, this is the maximum number of individual control channels that is supported + std::uint8_t numberBoomsSupported = 0; ///< Stores the number of booms this client supports for section control + std::uint8_t numberSectionsSupported = 0; ///< Stores the number of sections this client supports for section control + std::uint8_t numberChannelsSupportedForPositionBasedControl = 0; ///< Stores the number of channels this client supports for position based control + bool initialized = false; ///< Tracks the initialization state of the interface instance + bool shouldTerminate = false; ///< This variable tells the worker thread to exit + bool enableStatusMessage = false; ///< Enables sending the status message to the TC cyclically + bool supportsDocumentation = false; ///< Determines if the client reports documentation support to the TC + bool supportsTCGEOWithoutPositionBasedControl = false; ///< Determines if the client reports TC-GEO without position control capability to the TC + bool supportsTCGEOWithPositionBasedControl = false; ///< Determines if the client reports TC-GEO with position control capability to the TC + bool supportsPeerControlAssignment = false; ///< Determines if the client reports peer control assignment capability to the TC + bool supportsImplementSectionControl = false; ///< Determines if the client reports implement section control capability to the TC + }; +} // namespace isobus + +#endif // ISOBUS_TASK_CONTROLLER_CLIENT_HPP diff --git a/src/isobus_task_controller_client_objects.cpp b/src/isobus_task_controller_client_objects.cpp new file mode 100644 index 0000000..b647f58 --- /dev/null +++ b/src/isobus_task_controller_client_objects.cpp @@ -0,0 +1,600 @@ +//================================================================================================ +/// @file isobus_task_controller_client_objects.cpp +/// +/// @brief Implements the base functionality of the basic task controller objects. +/// @author Adrian Del Grosso +/// +/// @copyright 2023 Adrian Del Grosso +//================================================================================================ +#include "isobus_task_controller_client_objects.hpp" + +#include "platform_endianness.hpp" + +#include +#include +#include + +namespace isobus +{ + namespace task_controller_object + { + Object::Object(std::string objectDesignator, std::uint16_t uniqueID) : + designator(objectDesignator), + objectID(uniqueID) + { + } + + std::string Object::get_designator() const + { + return designator; + } + + void Object::set_designator(const std::string &newDesignator) + { + designator = newDesignator; + } + + std::uint16_t Object::get_object_id() const + { + return objectID; + } + + void Object::set_object_id(std::uint16_t id) + { + objectID = id; + } + + const std::string DeviceObject::tableID = "DVC"; + + DeviceObject::DeviceObject(std::string deviceDesignator, + std::string deviceSoftwareVersion, + std::string deviceSerialNumber, + std::string deviceStructureLabel, + std::array deviceLocalizationLabel, + std::vector deviceExtendedStructureLabel, + std::uint64_t clientIsoNAME, + bool shouldUseExtendedStructureLabel) : + Object(deviceDesignator, 0), + serialNumber(deviceSerialNumber), + softwareVersion(deviceSoftwareVersion), + structureLabel(deviceStructureLabel), + localizationLabel(deviceLocalizationLabel), + extendedStructureLabel(deviceExtendedStructureLabel), + NAME(clientIsoNAME), + useExtendedStructureLabel(shouldUseExtendedStructureLabel) + { + } + + std::string DeviceObject::get_table_id() const + { + return tableID; + } + + ObjectTypes DeviceObject::get_object_type() const + { + return ObjectTypes::Device; + } + + std::vector DeviceObject::get_binary_object() const + { + std::vector retVal; + + retVal.reserve(31 + + designator.size() + + softwareVersion.size() + + serialNumber.size() + + extendedStructureLabel.size()); + + retVal.push_back(tableID[0]); + retVal.push_back(tableID[1]); + retVal.push_back(tableID[2]); + retVal.push_back(static_cast(get_object_id() & 0xFF)); + retVal.push_back(static_cast((get_object_id() >> 8) & 0xFF)); + retVal.push_back(static_cast(designator.size())); + for (std::size_t i = 0; i < designator.size(); i++) + { + retVal.push_back(designator[i]); + } + retVal.push_back(static_cast(softwareVersion.size())); + for (std::size_t i = 0; i < softwareVersion.size(); i++) + { + retVal.push_back(softwareVersion[i]); + } + retVal.push_back(static_cast(NAME & 0xFF)); + retVal.push_back(static_cast((NAME >> 8) & 0xFF)); + retVal.push_back(static_cast((NAME >> 16) & 0xFF)); + retVal.push_back(static_cast((NAME >> 24) & 0xFF)); + retVal.push_back(static_cast((NAME >> 32) & 0xFF)); + retVal.push_back(static_cast((NAME >> 40) & 0xFF)); + retVal.push_back(static_cast((NAME >> 48) & 0xFF)); + retVal.push_back(static_cast((NAME >> 56) & 0xFF)); + retVal.push_back(static_cast(serialNumber.size())); + for (std::size_t i = 0; i < serialNumber.size(); i++) + { + retVal.push_back(serialNumber[i]); + } + for (std::uint_fast8_t i = 0; i < MAX_STRUCTURE_AND_LOCALIZATION_LABEL_LENGTH; i++) + { + if (i < structureLabel.size()) + { + retVal.push_back(structureLabel[i]); + } + else + { + retVal.push_back(' '); + } + } + for (std::uint_fast8_t i = 0; i < MAX_STRUCTURE_AND_LOCALIZATION_LABEL_LENGTH; i++) + { + if (i < localizationLabel.size()) + { + retVal.push_back(localizationLabel[i]); + } + else + { + retVal.push_back(' '); + } + } + if (useExtendedStructureLabel) + { + retVal.push_back(static_cast(extendedStructureLabel.size())); + for (std::size_t i = 0; i < extendedStructureLabel.size(); i++) + { + retVal.push_back(extendedStructureLabel[i]); + } + } + return retVal; + } + + std::string DeviceObject::get_software_version() const + { + return softwareVersion; + } + + void DeviceObject::set_software_version(const std::string &version) + { + softwareVersion = version; + } + + std::string DeviceObject::get_serial_number() const + { + return serialNumber; + } + + void DeviceObject::set_serial_number(const std::string &serial) + { + serialNumber = serial; + } + + std::string DeviceObject::get_structure_label() const + { + return structureLabel; + } + + void DeviceObject::set_structure_label(const std::string &label) + { + structureLabel = label; + } + + std::array DeviceObject::get_localization_label() const + { + return localizationLabel; + } + + void DeviceObject::set_localization_label(std::array label) + { + localizationLabel = label; + } + + std::vector DeviceObject::get_extended_structure_label() const + { + return extendedStructureLabel; + } + + void DeviceObject::set_extended_structure_label(const std::vector &label) + { + extendedStructureLabel = label; + } + + std::uint64_t DeviceObject::get_iso_name() const + { + return NAME; + } + + void DeviceObject::set_iso_name(std::uint64_t name) + { + NAME = name; + } + + bool DeviceObject::get_use_extended_structure_label() const + { + return useExtendedStructureLabel; + } + + void DeviceObject::set_use_extended_structure_label(bool shouldUseExtendedStructureLabel) + { + useExtendedStructureLabel = shouldUseExtendedStructureLabel; + } + + const std::string DeviceElementObject::tableID = "DET"; + + DeviceElementObject::DeviceElementObject(std::string deviceElementDesignator, + std::uint16_t deviceElementNumber, + std::uint16_t parentObjectID, + Type deviceEelementType, + std::uint16_t uniqueID) : + Object(deviceElementDesignator, uniqueID), + elementNumber(deviceElementNumber), + parentObject(parentObjectID), + elementType(deviceEelementType) + { + } + + std::string DeviceElementObject::get_table_id() const + { + return tableID; + } + + ObjectTypes DeviceElementObject::get_object_type() const + { + return ObjectTypes::DeviceElement; + } + + std::vector DeviceElementObject::get_binary_object() const + { + std::vector retVal; + + retVal.reserve(14 + + designator.size() + + 2 * referenceList.size()); + + retVal.push_back(tableID[0]); + retVal.push_back(tableID[1]); + retVal.push_back(tableID[2]); + retVal.push_back(static_cast(get_object_id() & 0xFF)); + retVal.push_back(static_cast((get_object_id() >> 8) & 0xFF)); + retVal.push_back(static_cast(elementType)); + retVal.push_back(static_cast(designator.size())); + for (std::size_t i = 0; i < designator.size(); i++) + { + retVal.push_back(designator[i]); + } + retVal.push_back(static_cast(elementNumber & 0xFF)); + retVal.push_back(static_cast((elementNumber >> 8) & 0xFF)); + retVal.push_back(static_cast(parentObject & 0xFF)); + retVal.push_back(static_cast((parentObject >> 8) & 0xFF)); + std::uint16_t tempSize = static_cast(referenceList.size()); + retVal.push_back(tempSize & 0xFF); + retVal.push_back((tempSize >> 8) & 0xFF); + for (std::size_t i = 0; i < tempSize; i++) + { + retVal.push_back(static_cast(referenceList[i] & 0xFF)); + retVal.push_back(static_cast((referenceList[i] >> 8) & 0xFF)); + } + return retVal; + } + + std::uint16_t DeviceElementObject::get_element_number() const + { + return elementNumber; + } + + void DeviceElementObject::set_element_number(std::uint16_t newElementNumber) + { + elementNumber = newElementNumber; + } + + std::uint16_t DeviceElementObject::get_parent_object() const + { + return parentObject; + } + + void DeviceElementObject::set_parent_object(std::uint16_t parentObjectID) + { + parentObject = parentObjectID; + } + + DeviceElementObject::Type DeviceElementObject::get_type() const + { + return elementType; + } + + void DeviceElementObject::add_reference_to_child_object(std::uint16_t childID) + { + referenceList.push_back(childID); + } + + bool DeviceElementObject::remove_reference_to_child_object(std::uint16_t childID) + { + bool retVal = false; + + auto result = std::find(referenceList.begin(), referenceList.end(), childID); + if (result != referenceList.end()) + { + retVal = true; + referenceList.erase(result); + } + return retVal; + } + + std::size_t DeviceElementObject::get_number_child_objects() const + { + return referenceList.size(); + } + + std::uint16_t DeviceElementObject::get_child_object_id(std::size_t index) + { + std::uint16_t retVal = NULL_OBJECT_ID; + + if (index < get_number_child_objects()) + { + retVal = referenceList[index]; + } + return retVal; + } + + const std::string DeviceProcessDataObject::tableID = "DPD"; + + DeviceProcessDataObject::DeviceProcessDataObject(std::string processDataDesignator, + std::uint16_t processDataDDI, + std::uint16_t deviceValuePresentationObjectID, + std::uint8_t processDataProperties, + std::uint8_t processDataTriggerMethods, + std::uint16_t uniqueID) : + Object(processDataDesignator, uniqueID), + ddi(processDataDDI), + deviceValuePresentationObject(deviceValuePresentationObjectID), + propertiesBitfield(processDataProperties), + triggerMethodsBitfield(processDataTriggerMethods) + { + } + + std::string DeviceProcessDataObject::get_table_id() const + { + return tableID; + } + + ObjectTypes DeviceProcessDataObject::get_object_type() const + { + return ObjectTypes::DeviceProcessData; + } + + std::vector DeviceProcessDataObject::get_binary_object() const + { + std::vector retVal; + + retVal.reserve(11 + designator.size()); + + retVal.push_back(tableID[0]); + retVal.push_back(tableID[1]); + retVal.push_back(tableID[2]); + retVal.push_back(static_cast(get_object_id() & 0xFF)); + retVal.push_back(static_cast((get_object_id() >> 8) & 0xFF)); + retVal.push_back(static_cast(ddi & 0xFF)); + retVal.push_back(static_cast((ddi >> 8) & 0xFF)); + retVal.push_back(propertiesBitfield); + retVal.push_back(triggerMethodsBitfield); + retVal.push_back(static_cast(designator.size())); + for (std::size_t i = 0; i < designator.size(); i++) + { + retVal.push_back(designator[i]); + } + retVal.push_back(deviceValuePresentationObject & 0xFF); + retVal.push_back((deviceValuePresentationObject >> 8) & 0xFF); + return retVal; + } + + std::uint16_t DeviceProcessDataObject::get_ddi() const + { + return ddi; + } + + void DeviceProcessDataObject::set_ddi(std::uint16_t newDDI) + { + ddi = newDDI; + } + + std::uint16_t DeviceProcessDataObject::get_device_value_presentation_object_id() const + { + return deviceValuePresentationObject; + } + + void DeviceProcessDataObject::set_device_value_presentation_object_id(std::uint16_t id) + { + deviceValuePresentationObject = id; + } + + std::uint8_t DeviceProcessDataObject::get_properties_bitfield() const + { + return propertiesBitfield; + } + + void DeviceProcessDataObject::set_properties_bitfield(std::uint8_t properties) + { + propertiesBitfield = properties; + } + + std::uint8_t DeviceProcessDataObject::get_trigger_methods_bitfield() const + { + return triggerMethodsBitfield; + } + + void DeviceProcessDataObject::set_trigger_methods_bitfield(std::uint8_t methods) + { + triggerMethodsBitfield = methods; + } + + const std::string DevicePropertyObject::tableID = "DPT"; + + DevicePropertyObject::DevicePropertyObject(std::string propertyDesignator, + std::int32_t propertyValue, + std::uint16_t propertyDDI, + std::uint16_t valuePresentationObject, + std::uint16_t uniqueID) : + Object(propertyDesignator, uniqueID), + value(propertyValue), + ddi(propertyDDI), + deviceValuePresentationObject(valuePresentationObject) + { + } + + std::string DevicePropertyObject::get_table_id() const + { + return tableID; + } + + ObjectTypes DevicePropertyObject::get_object_type() const + { + return ObjectTypes::DeviceProperty; + } + + std::vector DevicePropertyObject::get_binary_object() const + { + std::vector retVal; + + retVal.reserve(13 + designator.size()); + + retVal.push_back(tableID[0]); + retVal.push_back(tableID[1]); + retVal.push_back(tableID[2]); + retVal.push_back(static_cast(get_object_id() & 0xFF)); + retVal.push_back(static_cast((get_object_id() >> 8) & 0xFF)); + retVal.push_back(static_cast(ddi & 0xFF)); + retVal.push_back(static_cast((ddi >> 8) & 0xFF)); + retVal.push_back(static_cast(value & 0xFF)); + retVal.push_back(static_cast((value >> 8) & 0xFF)); + retVal.push_back(static_cast((value >> 16) & 0xFF)); + retVal.push_back(static_cast((value >> 24) & 0xFF)); + retVal.push_back(static_cast(designator.size())); + for (std::size_t i = 0; i < designator.size(); i++) + { + retVal.push_back(designator[i]); + } + retVal.push_back(deviceValuePresentationObject & 0xFF); + retVal.push_back((deviceValuePresentationObject >> 8) & 0xFF); + return retVal; + } + + std::int32_t DevicePropertyObject::get_value() const + { + return value; + } + + void DevicePropertyObject::set_value(std::int32_t newValue) + { + value = newValue; + } + + std::uint16_t DevicePropertyObject::get_ddi() const + { + return ddi; + } + + void DevicePropertyObject::set_ddi(std::uint16_t newDDI) + { + ddi = newDDI; + } + + std::uint16_t DevicePropertyObject::get_device_value_presentation_object_id() const + { + return deviceValuePresentationObject; + } + + void DevicePropertyObject::set_device_value_presentation_object_id(std::uint16_t id) + { + deviceValuePresentationObject = id; + } + + const std::string DeviceValuePresentationObject::tableID = "DVP"; + + DeviceValuePresentationObject::DeviceValuePresentationObject(std::string unitDesignator, + std::int32_t offsetValue, + float scaleFactor, + std::uint8_t numberDecimals, + std::uint16_t uniqueID) : + Object(unitDesignator, uniqueID), + offset(offsetValue), + scale(scaleFactor), + numberOfDecimals(numberDecimals) + { + } + + std::string DeviceValuePresentationObject::get_table_id() const + { + return tableID; + } + + ObjectTypes DeviceValuePresentationObject::get_object_type() const + { + return ObjectTypes::DeviceValuePresentation; + } + + std::vector DeviceValuePresentationObject::get_binary_object() const + { + std::vector retVal; + + retVal.reserve(16 + designator.size()); + + retVal.push_back(tableID[0]); + retVal.push_back(tableID[1]); + retVal.push_back(tableID[2]); + retVal.push_back(static_cast(get_object_id() & 0xFF)); + retVal.push_back(static_cast((get_object_id() >> 8) & 0xFF)); + retVal.push_back(static_cast(offset & 0xFF)); + retVal.push_back(static_cast((offset >> 8) & 0xFF)); + retVal.push_back(static_cast((offset >> 16) & 0xFF)); + retVal.push_back(static_cast((offset >> 24) & 0xFF)); + static_assert(sizeof(float) == 4, "Float must be 4 bytes"); + std::array floatBytes = { 0 }; + memcpy(floatBytes.data(), &scale, sizeof(float)); + + if (is_big_endian()) + { + std::reverse(floatBytes.begin(), floatBytes.end()); + } + + for (std::uint_fast8_t i = 0; i < 4; i++) + { + retVal.push_back(floatBytes[i]); + } + retVal.push_back(numberOfDecimals); + retVal.push_back(static_cast(designator.size())); + for (std::size_t i = 0; i < designator.size(); i++) + { + retVal.push_back(designator[i]); + } + return retVal; + } + + std::int32_t DeviceValuePresentationObject::get_offset() const + { + return offset; + } + + void DeviceValuePresentationObject::set_offset(std::int32_t newOffset) + { + offset = newOffset; + } + + float DeviceValuePresentationObject::get_scale() const + { + return scale; + } + + void DeviceValuePresentationObject::set_scale(float newScale) + { + scale = newScale; + } + + std::uint8_t DeviceValuePresentationObject::get_number_of_decimals() const + { + return numberOfDecimals; + } + + void DeviceValuePresentationObject::set_number_of_decimals(std::uint8_t decimals) + { + numberOfDecimals = decimals; + } + + } // namespace task_controller_object +} // namespace isobus diff --git a/src/isobus_task_controller_client_objects.hpp b/src/isobus_task_controller_client_objects.hpp new file mode 100644 index 0000000..f80c1e0 --- /dev/null +++ b/src/isobus_task_controller_client_objects.hpp @@ -0,0 +1,518 @@ +//================================================================================================ +/// @file isobus_task_controller_client_objects.hpp +/// +/// @brief Defines a set of C++ objects that represent a DDOP +/// @author Adrian Del Grosso +/// +/// @copyright 2023 Adrian Del Grosso +//================================================================================================ +#ifndef ISOBUS_TASK_CONTROLLER_CLIENT_OBJECTS_HPP +#define ISOBUS_TASK_CONTROLLER_CLIENT_OBJECTS_HPP + +#include +#include +#include +#include + +namespace isobus +{ + /// @brief A namespace that contains the generic task controller objects + namespace task_controller_object + { + /// @brief Enumerates the different kinds of DDOP objects + enum class ObjectTypes + { + Device, ///< The root object. Each device shall have one single Device + DeviceElement, ///< Subcomponent of a device. Has multiple sub-types + DeviceProcessData, ///< Contains a single process data variable definition + DeviceProperty, ///< A device property element + DeviceValuePresentation ///< Contains the presentation information to display the value of a DeviceProcessData or DeviceProperty object + }; + + /// @brief A base class for a Task Controller Object + class Object + { + public: + /// @brief Constructor for the TC object base class + /// @param[in] objectDesignator Descriptive text for this object, UTF-8 encoded, 32 characters max + /// @param[in] uniqueID The object ID of the object. Must be unique in the DDOP. + Object(std::string objectDesignator, std::uint16_t uniqueID); + + /// @brief Destructor for a TC Object + virtual ~Object() = default; + + /// @brief Returns the Descriptive text for this object, UTF-8 encoded, 32 characters max + /// @returns Descriptive text for this object, UTF-8 encoded, 32 characters max + std::string get_designator() const; + + /// @brief Updates the designator to a new value + /// @param[in] newDesignator The designator to set, UTF-8 encoded, 32 characters max + void set_designator(const std::string &newDesignator); + + /// @brief Returns the object ID of the object + /// @returns The object ID of the object + std::uint16_t get_object_id() const; + + /// @brief Updates the object ID of the object to a new value. + /// @param[in] id The object ID to set. IDs must be unique in the DDOP and less than or equal to MAX_OBJECT_ID + void set_object_id(std::uint16_t id); + + /// @brief Returns the XML namespace for the object + /// @returns the XML namespace for the object + virtual std::string get_table_id() const = 0; + + /// @brief Returns the derived TC object type fot the object + /// @returns The derived TC object type for this object + virtual ObjectTypes get_object_type() const = 0; + + /// @brief Returns the binary representation of the TC object, or an empty vector if object is invalid + /// @returns The binary representation of the TC object, or an empty vector if object is invalid + virtual std::vector get_binary_object() const = 0; + + /// @brief The max allowable "valid" object ID + static constexpr std::uint16_t MAX_OBJECT_ID = 65534; + + /// @brief Special ID used to indicate no object + static constexpr std::uint16_t NULL_OBJECT_ID = 65535; + + /// @brief Defines the max length of a designator (in bytes) + static constexpr std::size_t MAX_DESIGNATOR_LENGTH = 128; + + /// @brief Defines the max length of a designator (in bytes) for TCs older than version 4 + static constexpr std::size_t MAX_DESIGNATOR_LEGACY_LENGTH = 32; + + protected: + std::string designator; ///< UTF-8 Descriptive text to identify this object. Max length of 32. + std::uint16_t objectID; ///< Unique object ID in the DDOP + }; + + /// @brief Each device shall have one single DeviceObject in its device descriptor object pool. + /// see A.2 in ISO11783-10 + class DeviceObject : public Object + { + public: + /// @brief Constructor for a DeviceObject + /// @param[in] deviceDesignator Descriptive text for the object, UTF-8, 32-128 chars max depending on TC version + /// @param[in] deviceSoftwareVersion Software version indicating text (UTF-8) + /// @param[in] deviceSerialNumber Device and manufacturer-specific serial number of the Device (UTF-8) + /// @param[in] deviceStructureLabel This label allows the device to identify the current version of the device descriptor object pool (byte array /ascii) + /// @param[in] deviceLocalizationLabel Defined by the language command PGN (ascii / byte array) + /// @param[in] deviceExtendedStructureLabel Continuation of the Label given by Device to identify the Device descriptor Structure (byte array) + /// @param[in] clientIsoNAME NAME of client device as defined in ISO 11783-5 + /// @param[in] shouldUseExtendedStructureLabel If the device should include the extended structure label during binary serialization + DeviceObject(std::string deviceDesignator, + std::string deviceSoftwareVersion, + std::string deviceSerialNumber, + std::string deviceStructureLabel, + std::array deviceLocalizationLabel, + std::vector deviceExtendedStructureLabel, + std::uint64_t clientIsoNAME, + bool shouldUseExtendedStructureLabel); + + /// @brief Destructor for a DeviceObject + ~DeviceObject() override = default; + + /// @brief Returns the XML namespace for the object + /// @returns the XML namespace for the object + std::string get_table_id() const override; + + /// @brief Returns the object type + /// @returns The object type for this object (Object::Device) + ObjectTypes get_object_type() const override; + + /// @brief Returns the binary representation of the TC object, or an empty vector if object is invalid + /// @returns The binary representation of the TC object, or an empty vector if object is invalid + std::vector get_binary_object() const override; + + /// @brief Returns the software version of the device + /// @returns The software version of the device + std::string get_software_version() const; + + /// @brief Sets the software version for the device, as reported in the DDOP. + /// @param[in] version The software version to set as a UTF-8 string (or ascii). + void set_software_version(const std::string &version); + + /// @brief Returns the serial number for the device + /// @returns The serial number for the device + std::string get_serial_number() const; + + /// @brief Sets the serial number for the device as reported in the DDOP + /// @param[in] serial The serial number to set as a UTF-8 string (or ascii) + void set_serial_number(const std::string &serial); + + /// @brief Returns the structure label for this DDOP + /// @returns The structure label for this DDOP + std::string get_structure_label() const; + + /// @brief Sets the device structure label to a new value + /// @param[in] label The new structure label to set + void set_structure_label(const std::string &label); + + /// @brief Returns the localization label for this DDOP + /// @returns The localization label for this DDOP + std::array get_localization_label() const; + + /// @brief Changes the localization label to a new value + /// @param[in] label The new label to set + void set_localization_label(std::array label); + + /// @brief Returns the extended structure label (if applicable) + /// @returns The extended structure label (if applicable) + std::vector get_extended_structure_label() const; + + /// @brief Sets the extended structure label to a new value. Only used for TCs with version 4+. + /// @param[in] label The extended structure label to report or an empty vector if none is being used. + void set_extended_structure_label(const std::vector &label); + + /// @brief Returns the ISO NAME associated with this DDOP + /// @returns The raw ISO NAME associated with this DDOP + std::uint64_t get_iso_name() const; + + /// @brief Changes the stored ISO NAME to a new value + /// @param[in] name The new ISO NAME to set + void set_iso_name(std::uint64_t name); + + /// @brief Returns if the class will append the extended structure label to its serialized form + /// @details This is TC version 4 behavior. For version 3, this should return false. + /// @returns `true` if the class will append the extended structure label to its serialized form, otherwise `false` + bool get_use_extended_structure_label() const; + + /// @brief Sets the class' behavior for dealing with the extended structure label. + /// @details When this is set to true, the class will use TC version 4 behavior for the extended structure label. + /// When it is false, it will use < version 4 behavior (the label will not be included in the binary object). + /// @param[in] shouldUseExtendedStructureLabel `true` to use version 4 behavior, `false` to use earlier version behavior + void set_use_extended_structure_label(bool shouldUseExtendedStructureLabel); + + /// @brief Defines the max length of the device structure label and device localization label (in bytes) + static constexpr std::size_t MAX_STRUCTURE_AND_LOCALIZATION_LABEL_LENGTH = 7; + + /// @brief Defines the max length of the device extended structure label (in bytes) + static constexpr std::size_t MAX_EXTENDED_STRUCTURE_LABEL_LENGTH = 32; + + private: + static const std::string tableID; ///< XML element namespace for device. + std::string serialNumber; ///< Device and manufacturer-specific serial number of the Device + std::string softwareVersion; ///< Software version of the device + std::string structureLabel; ///< Label given by device to identify the device descriptor structure + std::array localizationLabel; ///< Label given by device to identify the device descriptor localization + std::vector extendedStructureLabel; ///< Continuation of the Label given by Device to identify the Device descriptor Structure + std::uint64_t NAME; ///< The NAME of client device as defined in ISO 11783-5. MUST match your address claim + bool useExtendedStructureLabel; ///< Tells the device if it should generate binary info using the extended structure label or ignore it + }; + + /// @brief DeviceElementObject is the object definition of the XML element DeviceElement. + /// The attribute Type specifies the type of this particular element definition + /// @details Referable Child Objects: DeviceProcessDataObject, DevicePropertyObject + class DeviceElementObject : public Object + { + public: + /// @brief Enumerates the types of device element object + enum class Type : std::uint8_t + { + Device = 1, ///< The device descriptor object pool shall have one device element of type device + Function = 2, ///< This device element type can be used as a generic device element to define individually accessible components of a device like valves or sensors + Bin = 3, ///< This is, for instance, the tank of a sprayer or the bin of a seeder. + Section = 4, ///< This is, for instance, the section of a spray boom, seed toolbar, or planter toolbar. + Unit = 5, ///< This device element type is, for example, used for spray boom nozzles, seeder openers, or planter row units. + Connector = 6, ///< This device element type specifies the mounting/connection position of the device + NavigationReference = 7 ///< This device element type defines the navigation reference position for navigation devices such as GPS receivers + }; + + /// @brief Constructor for a DeviceElementObject + /// @param[in] deviceElementDesignator Descriptive text for the object, UTF-8, 32-128 chars max depending on TC version + /// @param[in] deviceElementNumber The Element number for process data variable addressing + /// @param[in] parentObjectID Object ID of parent DeviceElementObject or DeviceObject in order to establish a hierarchical order of DeviceElements + /// @param[in] deviceEelementType The type of element, such as "device" or "bin" + /// @param[in] uniqueID The object ID of the object. Must be unique in the DDOP. + DeviceElementObject(std::string deviceElementDesignator, + std::uint16_t deviceElementNumber, + std::uint16_t parentObjectID, + Type deviceEelementType, + std::uint16_t uniqueID); + + /// @brief Destructor for a DeviceElementObject + ~DeviceElementObject() override = default; + + /// @brief Returns the XML namespace for the object + /// @returns The string "DET", the XML namespace for the DeviceElementObject + std::string get_table_id() const override; + + /// @brief Returns the object type + /// @returns The object type for this object (Object::DeviceElement) + ObjectTypes get_object_type() const override; + + /// @brief Returns the binary representation of the TC object, or an empty vector if object is invalid + /// @returns The binary representation of the TC object, or an empty vector if object is invalid + std::vector get_binary_object() const override; + + /// @brief Returns the element number + /// @returns The element number + std::uint16_t get_element_number() const; + + /// @brief Update the object's element number to a new value. + /// @param[in] newElementNumber The element number to set + void set_element_number(std::uint16_t newElementNumber); + + /// @brief Returns the parent object ID + /// @returns The parent object ID + std::uint16_t get_parent_object() const; + + /// @brief Updates the object ID associated to this object's parent object + /// @param[in] parentObjectID The object ID to set as the parent to this object + void set_parent_object(std::uint16_t parentObjectID); + + /// @brief Returns the type of the element object + /// @returns The type of the element object + Type get_type() const; + + /// @brief This function can be called to add an object as a child of this object. + /// @note You should only add Device or Device Element objects as children of this object + /// @param[in] childID The object ID of the child to reference from this object + void add_reference_to_child_object(std::uint16_t childID); + + /// @brief Removes a child object reference from this object. + /// @param[in] childID An object ID associated to a child object to remove. + /// @returns true if the child object ID was found and removed, otherwise false + bool remove_reference_to_child_object(std::uint16_t childID); + + /// @brief Returns the number of child objects added with `add_reference_to_child_object` + /// @returns The number of child objects added with `add_reference_to_child_object` + std::size_t get_number_child_objects() const; + + /// @brief Returns a child object ID by index + /// @returns Child object ID by index, or NULL_OBJECT_ID if the index is out of range + std::uint16_t get_child_object_id(std::size_t index); + + private: + static const std::string tableID; ///< XML element namespace for DeviceElement. + std::vector referenceList; ///< List of references to DeviceProcessDataObjects or DevicePropertyObjects + std::uint16_t elementNumber; ///< Element number for process data variable addressing + std::uint16_t parentObject; ///< Object ID of parent DeviceElementObject or DeviceObject in order to establish a hierarchical order of DeviceElements + Type elementType; ///< See the comments on `Type` or ISO11783-10 table A.2 + }; + + /// @brief The DeviceProcessDataObject is the object definition of the XML element DeviceProcessData. Each + /// object contains a single process data variable definition + /// @details Referable child object: DeviceValuePresentationObject + class DeviceProcessDataObject : public Object + { + public: + /// @brief Enumerates the properties in the properties bitset of this object + enum class PropertiesBit : std::uint8_t + { + MemberOfDefaultSet = 0x01, ///< member of default set + Settable = 0x02, ///< if this object is settable + ControlSource = 0x04 ///< Version 4, mutually exclusive with bit 2 + }; + + /// @brief Enumerates the trigger methods that can be set in the available trigger bitset of this object + enum class AvailableTriggerMethods + { + TimeInterval = 0x01, ///< The device can provide these device process data based on a time interval + DistanceInterval = 0x02, ///< The device can provide these device process data based on a distance interval. + ThresholdLimits = 0x04, ///< The device can provide these device process data based on a surpassing of the value threshold + OnChange = 0x08, ///< The device can provide these device process data when its value changes + Total = 0x10 ///< These device process data are a total + }; + + /// @brief Constructor for a DeviceProcessDataObject + /// @param[in] processDataDesignator Descriptive text for the object, UTF-8, 32 chars max + /// @param[in] processDataDDI Identifier of process data variable (DDI) according to definitions in Annex B and ISO 11783 - 11 + /// @param[in] deviceValuePresentationObjectID Object identifier of a DeviceValuePresentationObject, or the null ID + /// @param[in] processDataProperties A bitset of properties associated to this object. Some combination of `PropertiesBit` + /// @param[in] processDataTriggerMethods A bitset of available trigger methods, built from some combination of `AvailableTriggerMethods` + /// @param[in] uniqueID The object ID of the object. Must be unique in the DDOP. + DeviceProcessDataObject(std::string processDataDesignator, + std::uint16_t processDataDDI, + std::uint16_t deviceValuePresentationObjectID, + std::uint8_t processDataProperties, + std::uint8_t processDataTriggerMethods, + std::uint16_t uniqueID); + + /// @brief Destructor for a DeviceProcessDataObject + ~DeviceProcessDataObject() override = default; + + /// @brief Returns the XML element namespace for DeviceProcess-Data. + /// @returns The string "DPD", the XML element namespace for DeviceProcess-Data. + std::string get_table_id() const override; + + /// @brief Returns the object type + /// @returns The object type for this object (Object::DeviceProcessData) + ObjectTypes get_object_type() const override; + + /// @brief Returns the binary representation of the TC object, or an empty vector if object is invalid + /// @returns The binary representation of the TC object, or an empty vector if object is invalid + std::vector get_binary_object() const override; + + /// @brief Returns the DDI + /// @returns the DDI for this property + std::uint16_t get_ddi() const; + + /// @brief Updates the DDI associated to this DPD object + /// @param[in] newDDI The DDI to associate with this object + void set_ddi(std::uint16_t newDDI); + + /// @brief Returns Object identifier of the DeviceValuePresentation-Object for this object, or the null ID + /// @returns Object identifier of DeviceValuePresentation-Object for this object + std::uint16_t get_device_value_presentation_object_id() const; + + /// @brief Updates the object ID to use as an associated presentation for this object + /// @param[in] id Object identifier of the DeviceValuePresentation to use for this object, or the null ID for none + void set_device_value_presentation_object_id(std::uint16_t id); + + /// @brief Returns the object's properties bitfield + /// @returns The properties bitfield for this object + std::uint8_t get_properties_bitfield() const; + + /// @brief Updates the properties bitfield to a new value + /// @param[in] properties The new properties bitfield to set + void set_properties_bitfield(std::uint8_t properties); + + /// @brief Returns the object's available trigger methods + /// @returns The available trigger methods bitfield for this object + std::uint8_t get_trigger_methods_bitfield() const; + + /// @brief Updates the object's available trigger methods bitfield to a new value + /// @param[in] methods The new trigger methods bitfield to set + void set_trigger_methods_bitfield(std::uint8_t methods); + + private: + static const std::string tableID; ///< XML element namespace for DeviceProcessData. + std::uint16_t ddi; ///< Identifier of process data variable + std::uint16_t deviceValuePresentationObject; ///< Object identifier of DeviceValuePresentation-Object + std::uint8_t propertiesBitfield; ///< A bitset of properties for this object + std::uint8_t triggerMethodsBitfield; ///< A bitset defined in A.4.1 to A.4.5 + }; + + /// @brief DevicePropertyObject is the object definition of the XML element DeviceProperty. Each object contains + /// a single DeviceElementProperty definition + /// @details Referable child object: DeviceValuePresentationObject + class DevicePropertyObject : public Object + { + public: + /// @brief Constructor for a DevicePropertyObject + /// @param[in] propertyDesignator Descriptive text for the object, UTF-8, 32 chars max + /// @param[in] propertyValue The value of the property + /// @param[in] propertyDDI Identifier of property (DDI) according to definitions in Annex B and ISO 11783 - 11. + /// @param[in] valuePresentationObject Object identifier of DeviceValuePresentationObject, or NULL object ID + /// @param[in] uniqueID The object ID of the object. Must be unique in the DDOP. + DevicePropertyObject(std::string propertyDesignator, + std::int32_t propertyValue, + std::uint16_t propertyDDI, + std::uint16_t valuePresentationObject, + std::uint16_t uniqueID); + + /// @brief Destructor for a DevicePropertyObject + ~DevicePropertyObject() override = default; + + /// @brief Returns the XML element namespace for DeviceProperty. + /// @returns The string "DPT", the XML element namespace for DeviceProperty. + std::string get_table_id() const override; + + /// @brief Returns the object type + /// @returns The object type for this object (Object::DeviceProperty) + ObjectTypes get_object_type() const override; + + /// @brief Returns the binary representation of the TC object, or an empty vector if object is invalid + /// @returns The binary representation of the TC object, or an empty vector if object is invalid + std::vector get_binary_object() const override; + + /// @brief Returns the property's value + /// @returns The property's value + std::int32_t get_value() const; + + /// @brief Sets the property value + /// @param[in] newValue The value to set the property to + void set_value(std::int32_t newValue); + + /// @brief Returns the DDI for this object + /// @returns The DDI for this object + std::uint16_t get_ddi() const; + + /// @brief Updates the DDI associated with this DPT object to a new value + /// @param[in] newDDI The DDI to associate with this object's value + void set_ddi(std::uint16_t newDDI); + + /// @brief Returns the object identifier of an associated DeviceValuePresentationObject + /// @returns The object identifier of an associated DeviceValuePresentationObject + std::uint16_t get_device_value_presentation_object_id() const; + + /// @brief Updates the object ID to use as an associated presentation for this object + /// @param[in] id Object identifier of the DeviceValuePresentation to use for this object, or the null ID for none + void set_device_value_presentation_object_id(std::uint16_t id); + + private: + static const std::string tableID; ///< XML element namespace for DeviceProperty. + std::int32_t value; ///< The value of the property. + std::uint16_t ddi; ///< Identifier of property (DDI) according to definitions in Annex B and ISO 11783 - 11. + std::uint16_t deviceValuePresentationObject; ///< Object identifier of DeviceValuePresentationObject + }; + + /// @brief This object contains the presentation information to display the value of a DeviceProcessData or + /// DeviceProperty object.The device can update these objects when the language and/or units of measure + /// are changed by the operator + /// @details Referable child objects: none. + class DeviceValuePresentationObject : public Object + { + public: + /// @brief The constructor for a DeviceValuePresentationObject + /// @param[in] unitDesignator Unit designator for this value presentation + /// @param[in] offsetValue Offset to be applied to the value for presentation. + /// @param[in] scaleFactor Scale to be applied to the value for presentation. + /// @param[in] numberDecimals Specifies the number of decimals to display after the decimal point. + /// @param[in] uniqueID The object ID of the object. Must be unique in the DDOP. + DeviceValuePresentationObject(std::string unitDesignator, + std::int32_t offsetValue, + float scaleFactor, + std::uint8_t numberDecimals, + std::uint16_t uniqueID); + + /// @brief Destructor for a DeviceValuePresentationObject + ~DeviceValuePresentationObject() override = default; + + /// @brief Returns the XML element namespace for DeviceValuePresentation. + /// @returns The string "DPD", the XML element namespace for DeviceProcessData. + std::string get_table_id() const override; + + /// @brief Returns the object type + /// @returns The object type for this object (Object::DeviceValuePresentation) + ObjectTypes get_object_type() const override; + + /// @brief Returns the binary representation of the TC object, or an empty vector if object is invalid + /// @returns The binary representation of the TC object, or an empty vector if object is invalid + std::vector get_binary_object() const override; + + /// @brief Returns the offset that is applied to the value for presentation + /// @returns The offset that is applied to the value for presentation + std::int32_t get_offset() const; + + /// @brief Sets the offset that is applied to the value for presentation + /// @param[in] newOffset The offset to set for this object's value + void set_offset(std::int32_t newOffset); + + /// @brief Returns the scale that is applied to the value for presentation + /// @returns The scale that is applied to the value for presentation + float get_scale() const; + + /// @brief Sets the scale which will be applied to the value for presentation + /// @param[in] newScale The scale to set for this object's value + void set_scale(float newScale); + + /// @brief Returns the number of decimals shown after the decimal point + /// @returns The number of decimals shown after the decimal point + std::uint8_t get_number_of_decimals() const; + + /// @brief Sets the number of decimals to show when presenting objects associated with this presentation + /// @param[in] decimals The number of decimals to show after the decimal point + void set_number_of_decimals(std::uint8_t decimals); + + private: + static const std::string tableID; ///< XML element namespace for DeviceValuePresentation. + std::int32_t offset; ///< Offset to be applied to the value for presentation + float scale; ///< Scale to be applied to the value for presentation + std::uint8_t numberOfDecimals; ///< Specify number of decimals to display after the decimal point + }; + } // namespace task_controller_object +} // namespace isobus + +#endif // ISOBUS_TASK_CONTROLLER_CLIENT_OBJECTS_HPP diff --git a/src/isobus_virtual_terminal_client.cpp b/src/isobus_virtual_terminal_client.cpp new file mode 100644 index 0000000..5ec4c9e --- /dev/null +++ b/src/isobus_virtual_terminal_client.cpp @@ -0,0 +1,4589 @@ +//================================================================================================ +/// @file isobus_virtual_terminal_client.cpp +/// +/// @brief Implements the client for a virtual terminal +/// @author Adrian Del Grosso +/// +/// @copyright 2022 Adrian Del Grosso +//================================================================================================ + +#include "isobus_virtual_terminal_client.hpp" +#include "can_general_parameter_group_numbers.hpp" +#include "can_network_manager.hpp" +#include "can_stack_logger.hpp" +#include "platform_endianness.hpp" +#include "system_timing.hpp" +#include "to_string.hpp" + +#include +#include +#include +#include +#include +#include + +namespace isobus +{ + VirtualTerminalClient::VirtualTerminalClient(std::shared_ptr partner, std::shared_ptr clientSource) : + languageCommandInterface(clientSource, partner), + partnerControlFunction(partner), + myControlFunction(clientSource), + txFlags(static_cast(TransmitFlags::NumberFlags), process_flags, this) + { + } + + VirtualTerminalClient::~VirtualTerminalClient() + { + terminate(); + } + + void VirtualTerminalClient::initialize(bool spawnThread) + { + if (shouldTerminate) + { + shouldTerminate = false; + initialized = false; + } + + if (!initialized) + { + if (nullptr != partnerControlFunction) + { + partnerControlFunction->add_parameter_group_number_callback(static_cast(CANLibParameterGroupNumber::VirtualTerminalToECU), process_rx_message, this); + partnerControlFunction->add_parameter_group_number_callback(static_cast(CANLibParameterGroupNumber::Acknowledge), process_rx_message, this); + CANNetworkManager::CANNetwork.add_global_parameter_group_number_callback(static_cast(CANLibParameterGroupNumber::VirtualTerminalToECU), process_rx_message, this); + CANNetworkManager::CANNetwork.add_global_parameter_group_number_callback(static_cast(CANLibParameterGroupNumber::ECUtoVirtualTerminal), process_rx_message, this); + } + + if (!languageCommandInterface.get_initialized()) + { + languageCommandInterface.initialize(); + } +#if !defined CAN_STACK_DISABLE_THREADS && !defined ARDUINO + if (spawnThread) + { + workerThread = new std::thread([this]() { worker_thread_function(); }); + } +#endif + initialized = true; + } + } + + bool VirtualTerminalClient::get_is_initialized() const + { + return initialized; + } + + bool VirtualTerminalClient::get_is_connected() const + { + return (StateMachineState::Connected == state); + } + + void VirtualTerminalClient::terminate() + { + if (initialized) + { + if (nullptr != partnerControlFunction) + { + if ((StateMachineState::Connected == state) && + (send_delete_object_pool())) + { + CANStackLogger::debug("[VT]: Requested object pool deletion from volatile VT memory."); + } + partnerControlFunction->remove_parameter_group_number_callback(static_cast(CANLibParameterGroupNumber::VirtualTerminalToECU), process_rx_message, this); + partnerControlFunction->remove_parameter_group_number_callback(static_cast(CANLibParameterGroupNumber::Acknowledge), process_rx_message, this); + CANNetworkManager::CANNetwork.remove_global_parameter_group_number_callback(static_cast(CANLibParameterGroupNumber::VirtualTerminalToECU), process_rx_message, this); + CANNetworkManager::CANNetwork.remove_global_parameter_group_number_callback(static_cast(CANLibParameterGroupNumber::ECUtoVirtualTerminal), process_rx_message, this); + } + + shouldTerminate = true; +#if !defined CAN_STACK_DISABLE_THREADS && !defined ARDUINO + if (nullptr != workerThread) + { + workerThread->join(); + delete workerThread; + workerThread = nullptr; + } +#endif + initialized = false; + set_state(StateMachineState::Disconnected); + CANStackLogger::info("[VT]: VT Client connection has been terminated."); + } + } + + void VirtualTerminalClient::restart_communication() + { + CANStackLogger::info("[VT]:VT Client connection restart requested. Client will now terminate and reinitialize."); +#if !defined CAN_STACK_DISABLE_THREADS && !defined ARDUINO + bool workerNeeded = (nullptr != workerThread); +#else + bool workerNeeded = false; +#endif + terminate(); + initialize(workerNeeded); + } + + std::shared_ptr VirtualTerminalClient::get_partner_control_function() const + { + return partnerControlFunction; + } + + std::shared_ptr VirtualTerminalClient::get_internal_control_function() const + { + return myControlFunction; + } + + std::uint8_t VirtualTerminalClient::get_active_working_set_master_address() const + { + std::uint8_t retVal = NULL_CAN_ADDRESS; + + if (get_is_connected()) + { + retVal = activeWorkingSetMasterAddress; + } + return retVal; + } + + std::shared_ptr VirtualTerminalClient::add_vt_soft_key_event_listener(std::function callback) + { + return softKeyEventDispatcher.add_listener(callback); + } + + std::shared_ptr VirtualTerminalClient::add_vt_button_event_listener(std::function callback) + { + return buttonEventDispatcher.add_listener(callback); + } + + std::shared_ptr VirtualTerminalClient::add_vt_pointing_event_listener(std::function callback) + { + return pointingEventDispatcher.add_listener(callback); + } + + std::shared_ptr VirtualTerminalClient::add_vt_select_input_object_event_listener(std::function callback) + { + return selectInputObjectEventDispatcher.add_listener(callback); + } + + std::shared_ptr VirtualTerminalClient::add_vt_esc_message_event_listener(std::function callback) + { + return escMessageEventDispatcher.add_listener(callback); + } + + std::shared_ptr VirtualTerminalClient::add_vt_change_numeric_value_event_listener(std::function callback) + { + return changeNumericValueEventDispatcher.add_listener(callback); + } + + std::shared_ptr VirtualTerminalClient::add_vt_change_active_mask_event_listener(std::function callback) + { + return changeActiveMaskEventDispatcher.add_listener(callback); + } + + std::shared_ptr VirtualTerminalClient::add_vt_change_soft_key_mask_event_listener(std::function callback) + { + return changeSoftKeyMaskEventDispatcher.add_listener(callback); + } + + std::shared_ptr VirtualTerminalClient::add_vt_change_string_value_event_listener(std::function callback) + { + return changeStringValueEventDispatcher.add_listener(callback); + } + + std::shared_ptr VirtualTerminalClient::add_vt_user_layout_hide_show_event_listener(std::function callback) + { + return userLayoutHideShowEventDispatcher.add_listener(callback); + } + + std::shared_ptr VirtualTerminalClient::add_vt_control_audio_signal_termination_event_listener(std::function callback) + { + return audioSignalTerminationEventDispatcher.add_listener(callback); + } + + std::shared_ptr VirtualTerminalClient::add_auxiliary_function_event_listener(std::function callback) + { + return auxiliaryFunctionEventDispatcher.add_listener(callback); + } + + void VirtualTerminalClient::set_auxiliary_input_model_identification_code(std::uint16_t modelIdentificationCode) + { + ourModelIdentificationCode = modelIdentificationCode; + } + + bool VirtualTerminalClient::get_auxiliary_input_learn_mode_enabled() const + { + return 0x40 == (busyCodesBitfield & 0x40); + } + + void VirtualTerminalClient::add_auxiliary_input_object_id(const std::uint16_t auxiliaryInputID) + { + ourAuxiliaryInputs[auxiliaryInputID] = AuxiliaryInputState{ 0, false, false, false, 0, 0 }; + } + + void VirtualTerminalClient::remove_auxiliary_input_object_id(const std::uint16_t auxiliaryInputID) + { + if (ourAuxiliaryInputs.count(auxiliaryInputID)) + { + ourAuxiliaryInputs.erase(auxiliaryInputID); + CANStackLogger::debug("[AUX-N] Removed auxiliary input with ID: " + isobus::to_string(static_cast(auxiliaryInputID))); + } + } + + void VirtualTerminalClient::update_auxiliary_input(const std::uint16_t auxiliaryInputID, const std::uint16_t value1, const std::uint16_t value2, const bool controlLocked) + { + if (!ourAuxiliaryInputs.count(auxiliaryInputID)) + { + CANStackLogger::warn("[AUX-N] Auxiliary input with ID '" + isobus::to_string(static_cast(auxiliaryInputID)) + "' has not been registered. Ignoring update"); + return; + } + + if (state == StateMachineState::Connected) + { + if ((value1 != ourAuxiliaryInputs.at(auxiliaryInputID).value1) || (value2 != ourAuxiliaryInputs.at(auxiliaryInputID).value2)) + { + ourAuxiliaryInputs.at(auxiliaryInputID).value1 = value1; + ourAuxiliaryInputs.at(auxiliaryInputID).value2 = value2; + ourAuxiliaryInputs.at(auxiliaryInputID).controlLocked = controlLocked; + ourAuxiliaryInputs.at(auxiliaryInputID).hasInteraction = true; + update_auxiliary_input_status(auxiliaryInputID); + } + } + } + + isobus::VirtualTerminalClient::AssignedAuxiliaryFunction::AssignedAuxiliaryFunction(const std::uint16_t functionObjectID, const std::uint16_t inputObjectID, const AuxiliaryTypeTwoFunctionType functionType) : + functionObjectID(functionObjectID), inputObjectID(inputObjectID), functionType(functionType) + { + } + + bool VirtualTerminalClient::AssignedAuxiliaryFunction::operator==(const AssignedAuxiliaryFunction &other) const + { + return (functionObjectID == other.functionObjectID) && (inputObjectID == other.inputObjectID) && (functionType == other.functionType); + } + + bool VirtualTerminalClient::send_hide_show_object(std::uint16_t objectID, HideShowObjectCommand command) const + { + const std::array buffer = { static_cast(Function::HideShowObjectCommand), + static_cast(objectID & 0xFF), + static_cast(objectID >> 8), + static_cast(command), + 0xFF, + 0xFF, + 0xFF, + 0xFF }; + + return CANNetworkManager::CANNetwork.send_can_message(static_cast(CANLibParameterGroupNumber::ECUtoVirtualTerminal), + buffer.data(), + CAN_DATA_LENGTH, + myControlFunction, + partnerControlFunction, + CANIdentifier::PriorityLowest7); + } + + bool VirtualTerminalClient::send_enable_disable_object(std::uint16_t objectID, EnableDisableObjectCommand command) const + { + const std::array buffer = { static_cast(Function::EnableDisableObjectCommand), + static_cast(objectID & 0xFF), + static_cast(objectID >> 8), + static_cast(command), + 0xFF, + 0xFF, + 0xFF, + 0xFF }; + return CANNetworkManager::CANNetwork.send_can_message(static_cast(CANLibParameterGroupNumber::ECUtoVirtualTerminal), + buffer.data(), + CAN_DATA_LENGTH, + myControlFunction, + partnerControlFunction, + CANIdentifier::PriorityLowest7); + } + + bool VirtualTerminalClient::send_select_input_object(std::uint16_t objectID, SelectInputObjectOptions option) const + { + const std::array buffer = { static_cast(Function::SelectInputObjectCommand), + static_cast(objectID & 0xFF), + static_cast(objectID >> 8), + static_cast(option), + 0xFF, + 0xFF, + 0xFF, + 0xFF }; + return CANNetworkManager::CANNetwork.send_can_message(static_cast(CANLibParameterGroupNumber::ECUtoVirtualTerminal), + buffer.data(), + CAN_DATA_LENGTH, + myControlFunction, + partnerControlFunction, + CANIdentifier::PriorityLowest7); + } + + bool VirtualTerminalClient::send_ESC() const + { + const std::array buffer = { static_cast(Function::ESCCommand), + 0xFF, + 0xFF, + 0xFF, + 0xFF, + 0xFF, + 0xFF, + 0xFF }; + return CANNetworkManager::CANNetwork.send_can_message(static_cast(CANLibParameterGroupNumber::ECUtoVirtualTerminal), + buffer.data(), + CAN_DATA_LENGTH, + myControlFunction, + partnerControlFunction, + CANIdentifier::PriorityLowest7); + } + + bool VirtualTerminalClient::send_control_audio_signal(std::uint8_t activations, std::uint16_t frequency_hz, std::uint16_t duration_ms, std::uint16_t offTimeDuration_ms) const + { + const std::array buffer = { static_cast(Function::ControlAudioSignalCommand), + activations, + static_cast(frequency_hz & 0xFF), + static_cast(frequency_hz >> 8), + static_cast(duration_ms & 0xFF), + static_cast(duration_ms >> 8), + static_cast(offTimeDuration_ms & 0xFF), + static_cast(offTimeDuration_ms >> 8) }; + return CANNetworkManager::CANNetwork.send_can_message(static_cast(CANLibParameterGroupNumber::ECUtoVirtualTerminal), + buffer.data(), + CAN_DATA_LENGTH, + myControlFunction, + partnerControlFunction, + CANIdentifier::PriorityLowest7); + } + + bool VirtualTerminalClient::send_set_audio_volume(std::uint8_t volume_percent) const + { + constexpr std::uint8_t MAX_VOLUME_PERCENT = 100; + + if (volume_percent > MAX_VOLUME_PERCENT) + { + volume_percent = MAX_VOLUME_PERCENT; + CANStackLogger::warn("[VT]: Cannot try to set audio volume greater than 100 percent. Value will be capped at 100."); + } + + const std::array buffer = { static_cast(Function::SetAudioVolumeCommand), + volume_percent, + 0xFF, + 0xFF, + 0xFF, + 0xFF, + 0xFF, + 0xFF }; + return CANNetworkManager::CANNetwork.send_can_message(static_cast(CANLibParameterGroupNumber::ECUtoVirtualTerminal), + buffer.data(), + CAN_DATA_LENGTH, + myControlFunction, + partnerControlFunction, + CANIdentifier::PriorityLowest7); + } + + bool VirtualTerminalClient::send_change_child_location(std::uint16_t objectID, std::uint16_t parentObjectID, std::uint8_t relativeXPositionChange, std::uint8_t relativeYPositionChange) const + { + const std::array buffer = { static_cast(Function::ChangeChildLocationCommand), + static_cast(parentObjectID & 0xFF), + static_cast(parentObjectID >> 8), + static_cast(objectID & 0xFF), + static_cast(objectID >> 8), + relativeXPositionChange, + relativeYPositionChange, + 0xFF }; + return CANNetworkManager::CANNetwork.send_can_message(static_cast(CANLibParameterGroupNumber::ECUtoVirtualTerminal), + buffer.data(), + CAN_DATA_LENGTH, + myControlFunction, + partnerControlFunction, + CANIdentifier::PriorityLowest7); + } + + bool VirtualTerminalClient::send_change_child_position(std::uint16_t objectID, std::uint16_t parentObjectID, std::uint16_t xPosition, std::uint16_t yPosition) const + { + const std::array buffer = { + static_cast(Function::ChangeChildPositionCommand), + static_cast(parentObjectID & 0xFF), + static_cast(parentObjectID >> 8), + static_cast(objectID & 0xFF), + static_cast(objectID >> 8), + static_cast(xPosition & 0xFF), + static_cast(xPosition >> 8), + static_cast(yPosition & 0xFF), + static_cast(yPosition >> 8), + }; + return CANNetworkManager::CANNetwork.send_can_message(static_cast(CANLibParameterGroupNumber::ECUtoVirtualTerminal), + buffer.data(), + buffer.size(), + myControlFunction, + partnerControlFunction, + CANIdentifier::PriorityLowest7); + } + + bool VirtualTerminalClient::send_change_size_command(std::uint16_t objectID, std::uint16_t newWidth, std::uint16_t newHeight) const + { + const std::array buffer = { static_cast(Function::ChangeSizeCommand), + static_cast(objectID & 0xFF), + static_cast(objectID >> 8), + static_cast(newWidth & 0xFF), + static_cast(newWidth >> 8), + static_cast(newHeight & 0xFF), + static_cast(newHeight >> 8), + 0xFF }; + return CANNetworkManager::CANNetwork.send_can_message(static_cast(CANLibParameterGroupNumber::ECUtoVirtualTerminal), + buffer.data(), + CAN_DATA_LENGTH, + myControlFunction, + partnerControlFunction, + CANIdentifier::PriorityLowest7); + } + + bool VirtualTerminalClient::send_change_background_colour(std::uint16_t objectID, std::uint8_t colour) const + { + const std::array buffer = { static_cast(Function::ChangeBackgroundColourCommand), + static_cast(objectID & 0xFF), + static_cast(objectID >> 8), + colour, + 0xFF, + 0xFF, + 0xFF, + 0xFF }; + return CANNetworkManager::CANNetwork.send_can_message(static_cast(CANLibParameterGroupNumber::ECUtoVirtualTerminal), + buffer.data(), + CAN_DATA_LENGTH, + myControlFunction, + partnerControlFunction, + CANIdentifier::PriorityLowest7); + } + + bool VirtualTerminalClient::send_change_numeric_value(std::uint16_t objectID, std::uint32_t value) const + { + const std::array buffer = { + static_cast(Function::ChangeNumericValueCommand), + static_cast(objectID & 0xFF), + static_cast(objectID >> 8), + 0xFF, + static_cast(value & 0xFF), + static_cast((value >> 8) & 0xFF), + static_cast((value >> 16) & 0xFF), + static_cast((value >> 24) & 0xFF), + }; + return CANNetworkManager::CANNetwork.send_can_message(static_cast(CANLibParameterGroupNumber::ECUtoVirtualTerminal), + buffer.data(), + CAN_DATA_LENGTH, + myControlFunction, + partnerControlFunction, + CANIdentifier::PriorityLowest7); + } + + bool VirtualTerminalClient::send_change_string_value(std::uint16_t objectID, uint16_t stringLength, const char *value) const + { + bool retVal = false; + + if (nullptr != value) + { + std::vector buffer; + buffer.resize(5 + stringLength); + buffer[0] = static_cast(Function::ChangeStringValueCommand); + buffer[1] = static_cast(objectID & 0xFF); + buffer[2] = static_cast(objectID >> 8); + buffer[3] = static_cast(stringLength & 0xFF); + buffer[4] = static_cast(stringLength >> 8); + for (std::uint16_t i = 0; i < stringLength; i++) + { + buffer[5 + i] = value[i]; + } + + while (buffer.size() < CAN_DATA_LENGTH) + { + buffer.push_back(0xFF); // Pad to minimum length + } + retVal = CANNetworkManager::CANNetwork.send_can_message(static_cast(CANLibParameterGroupNumber::ECUtoVirtualTerminal), + buffer.data(), + buffer.size(), + myControlFunction, + partnerControlFunction, + CANIdentifier::PriorityLowest7); + } + return retVal; + } + + bool VirtualTerminalClient::send_change_string_value(std::uint16_t objectID, const std::string &value) const + { + return send_change_string_value(objectID, value.size(), value.c_str()); + } + + bool VirtualTerminalClient::send_change_endpoint(std::uint16_t objectID, std::uint16_t width_px, std::uint16_t height_px, LineDirection direction) const + { + const std::array buffer = { static_cast(Function::ChangeEndPointCommand), + static_cast(objectID & 0xFF), + static_cast(objectID >> 8), + static_cast(width_px & 0xFF), + static_cast(width_px >> 8), + static_cast(height_px & 0xFF), + static_cast(height_px >> 8), + static_cast(direction) }; + return CANNetworkManager::CANNetwork.send_can_message(static_cast(CANLibParameterGroupNumber::ECUtoVirtualTerminal), + buffer.data(), + CAN_DATA_LENGTH, + myControlFunction, + partnerControlFunction, + CANIdentifier::PriorityLowest7); + } + + bool VirtualTerminalClient::send_change_font_attributes(std::uint16_t objectID, std::uint8_t colour, FontSize size, std::uint8_t type, std::uint8_t styleBitfield) const + { + const std::array buffer = { static_cast(Function::ChangeFontAttributesCommand), + static_cast(objectID & 0xFF), + static_cast(objectID >> 8), + colour, + static_cast(size), + type, + styleBitfield, + 0xFF }; + return CANNetworkManager::CANNetwork.send_can_message(static_cast(CANLibParameterGroupNumber::ECUtoVirtualTerminal), + buffer.data(), + CAN_DATA_LENGTH, + myControlFunction, + partnerControlFunction, + CANIdentifier::PriorityLowest7); + } + + bool VirtualTerminalClient::send_change_line_attributes(std::uint16_t objectID, std::uint8_t colour, std::uint8_t width, std::uint16_t lineArtBitmask) const + { + const std::array buffer = { static_cast(Function::ChangeLineAttributesCommand), + static_cast(objectID & 0xFF), + static_cast(objectID >> 8), + colour, + static_cast(width), + static_cast(lineArtBitmask & 0xFF), + static_cast(lineArtBitmask >> 8), + 0xFF }; + return CANNetworkManager::CANNetwork.send_can_message(static_cast(CANLibParameterGroupNumber::ECUtoVirtualTerminal), + buffer.data(), + CAN_DATA_LENGTH, + myControlFunction, + partnerControlFunction, + CANIdentifier::PriorityLowest7); + } + + bool VirtualTerminalClient::send_change_fill_attributes(std::uint16_t objectID, FillType fillType, std::uint8_t colour, std::uint16_t fillPatternObjectID) const + { + const std::array buffer = { static_cast(Function::ChangeFillAttributesCommand), + static_cast(objectID & 0xFF), + static_cast(objectID >> 8), + static_cast(fillType), + colour, + static_cast(fillPatternObjectID & 0xFF), + static_cast(fillPatternObjectID >> 8), + 0xFF }; + return CANNetworkManager::CANNetwork.send_can_message(static_cast(CANLibParameterGroupNumber::ECUtoVirtualTerminal), + buffer.data(), + CAN_DATA_LENGTH, + myControlFunction, + partnerControlFunction, + CANIdentifier::PriorityLowest7); + } + + bool VirtualTerminalClient::send_change_active_mask(std::uint16_t workingSetObjectID, std::uint16_t newActiveMaskObjectID) const + { + const std::array buffer = { static_cast(Function::ChangeActiveMaskCommand), + static_cast(workingSetObjectID & 0xFF), + static_cast(workingSetObjectID >> 8), + static_cast(newActiveMaskObjectID & 0xFF), + static_cast(newActiveMaskObjectID >> 8), + 0xFF, + 0xFF, + 0xFF }; + return CANNetworkManager::CANNetwork.send_can_message(static_cast(CANLibParameterGroupNumber::ECUtoVirtualTerminal), + buffer.data(), + CAN_DATA_LENGTH, + myControlFunction, + partnerControlFunction, + CANIdentifier::PriorityLowest7); + } + + bool VirtualTerminalClient::send_change_softkey_mask(MaskType type, std::uint16_t dataOrAlarmMaskObjectID, std::uint16_t newSoftKeyMaskObjectID) const + { + const std::array buffer = { static_cast(Function::ChangeSoftKeyMaskCommand), + static_cast(type), + static_cast(dataOrAlarmMaskObjectID & 0xFF), + static_cast(dataOrAlarmMaskObjectID >> 8), + static_cast(newSoftKeyMaskObjectID & 0xFF), + static_cast(newSoftKeyMaskObjectID >> 8), + 0xFF, + 0xFF }; + return CANNetworkManager::CANNetwork.send_can_message(static_cast(CANLibParameterGroupNumber::ECUtoVirtualTerminal), + buffer.data(), + CAN_DATA_LENGTH, + myControlFunction, + partnerControlFunction, + CANIdentifier::PriorityLowest7); + } + + bool VirtualTerminalClient::send_change_attribute(std::uint16_t objectID, std::uint8_t attributeID, std::uint32_t value) const + { + const std::array buffer = { static_cast(Function::ChangeAttributeCommand), + static_cast(objectID & 0xFF), + static_cast(objectID >> 8), + attributeID, + static_cast(value & 0xFF), + static_cast((value >> 8) & 0xFF), + static_cast((value >> 16) & 0xFF), + static_cast((value >> 24) & 0xFF) }; + return CANNetworkManager::CANNetwork.send_can_message(static_cast(CANLibParameterGroupNumber::ECUtoVirtualTerminal), + buffer.data(), + CAN_DATA_LENGTH, + myControlFunction, + partnerControlFunction, + CANIdentifier::PriorityLowest7); + } + + bool VirtualTerminalClient::send_change_attribute(std::uint16_t objectID, std::uint8_t attributeID, float value) const + { + static_assert(sizeof(float) == 4, "Float must be 4 bytes"); + std::array floatBytes = { 0 }; + memcpy(floatBytes.data(), &value, sizeof(float)); + + if (is_big_endian()) + { + std::reverse(floatBytes.begin(), floatBytes.end()); + } + + const std::array buffer = { static_cast(Function::ChangeAttributeCommand), + static_cast(objectID & 0xFF), + static_cast(objectID >> 8), + attributeID, + floatBytes[0], + floatBytes[1], + floatBytes[2], + floatBytes[3] }; + return CANNetworkManager::CANNetwork.send_can_message(static_cast(CANLibParameterGroupNumber::ECUtoVirtualTerminal), + buffer.data(), + CAN_DATA_LENGTH, + myControlFunction, + partnerControlFunction, + CANIdentifier::PriorityLowest7); + } + + bool VirtualTerminalClient::send_change_priority(std::uint16_t alarmMaskObjectID, AlarmMaskPriority priority) const + { + const std::array buffer = { static_cast(Function::ChangePriorityCommand), + static_cast(alarmMaskObjectID & 0xFF), + static_cast(alarmMaskObjectID >> 8), + static_cast(priority), + 0xFF, + 0xFF, + 0xFF, + 0xFF }; + return CANNetworkManager::CANNetwork.send_can_message(static_cast(CANLibParameterGroupNumber::ECUtoVirtualTerminal), + buffer.data(), + CAN_DATA_LENGTH, + myControlFunction, + partnerControlFunction, + CANIdentifier::PriorityLowest7); + } + + bool VirtualTerminalClient::send_change_list_item(std::uint16_t objectID, std::uint8_t listIndex, std::uint16_t newObjectID) const + { + const std::array buffer = { static_cast(Function::ChangeListItemCommand), + static_cast(objectID & 0xFF), + static_cast(objectID >> 8), + listIndex, + static_cast(newObjectID & 0xFF), + static_cast(newObjectID >> 8), + 0xFF, + 0xFF }; + return CANNetworkManager::CANNetwork.send_can_message(static_cast(CANLibParameterGroupNumber::ECUtoVirtualTerminal), + buffer.data(), + CAN_DATA_LENGTH, + myControlFunction, + partnerControlFunction, + CANIdentifier::PriorityLowest7); + } + + bool VirtualTerminalClient::send_lock_unlock_mask(MaskLockState state, std::uint16_t objectID, std::uint16_t timeout_ms) const + { + const std::array buffer = { static_cast(Function::LockUnlockMaskCommand), + static_cast(state), + static_cast(objectID & 0xFF), + static_cast(objectID >> 8), + static_cast(timeout_ms & 0xFF), + static_cast(timeout_ms >> 8), + 0xFF, + 0xFF }; + return CANNetworkManager::CANNetwork.send_can_message(static_cast(CANLibParameterGroupNumber::ECUtoVirtualTerminal), + buffer.data(), + CAN_DATA_LENGTH, + myControlFunction, + partnerControlFunction, + CANIdentifier::PriorityLowest7); + } + + bool VirtualTerminalClient::send_execute_macro(std::uint16_t objectID) const + { + const std::array buffer = { static_cast(Function::ExecuteMacroCommand), + static_cast(objectID & 0xFF), + static_cast(objectID >> 8), + 0xFF, + 0xFF, + 0xFF, + 0xFF, + 0xFF }; + return CANNetworkManager::CANNetwork.send_can_message(static_cast(CANLibParameterGroupNumber::ECUtoVirtualTerminal), + buffer.data(), + CAN_DATA_LENGTH, + myControlFunction, + partnerControlFunction, + CANIdentifier::PriorityLowest7); + } + + bool VirtualTerminalClient::send_change_object_label(std::uint16_t objectID, std::uint16_t labelStringObjectID, std::uint8_t fontType, std::uint16_t graphicalDesignatorObjectID) const + { + const std::array buffer = { static_cast(Function::ChangeObjectLabelCommand), + static_cast(objectID & 0xFF), + static_cast(objectID >> 8), + static_cast(labelStringObjectID & 0xFF), + static_cast(labelStringObjectID >> 8), + fontType, + static_cast(graphicalDesignatorObjectID & 0xFF), + static_cast(graphicalDesignatorObjectID >> 8) }; + return CANNetworkManager::CANNetwork.send_can_message(static_cast(CANLibParameterGroupNumber::ECUtoVirtualTerminal), + buffer.data(), + CAN_DATA_LENGTH, + myControlFunction, + partnerControlFunction, + CANIdentifier::PriorityLowest7); + } + + bool VirtualTerminalClient::send_change_polygon_point(std::uint16_t objectID, std::uint8_t pointIndex, std::uint16_t newXValue, std::uint16_t newYValue) const + { + const std::array buffer = { static_cast(Function::ChangePolygonPointCommand), + static_cast(objectID & 0xFF), + static_cast(objectID >> 8), + pointIndex, + static_cast(newXValue & 0xFF), + static_cast(newXValue >> 8), + static_cast(newYValue & 0xFF), + static_cast(newYValue >> 8) }; + return CANNetworkManager::CANNetwork.send_can_message(static_cast(CANLibParameterGroupNumber::ECUtoVirtualTerminal), + buffer.data(), + CAN_DATA_LENGTH, + myControlFunction, + partnerControlFunction, + CANIdentifier::PriorityLowest7); + } + + bool VirtualTerminalClient::send_change_polygon_scale(std::uint16_t objectID, std::uint16_t widthAttribute, std::uint16_t heightAttribute) const + { + const std::array buffer = { static_cast(Function::ChangePolygonScaleCommand), + static_cast(objectID & 0xFF), + static_cast(objectID >> 8), + static_cast(widthAttribute & 0xFF), + static_cast(widthAttribute >> 8), + static_cast(heightAttribute & 0xFF), + static_cast(heightAttribute >> 8), + 0xFF }; + return CANNetworkManager::CANNetwork.send_can_message(static_cast(CANLibParameterGroupNumber::ECUtoVirtualTerminal), + buffer.data(), + CAN_DATA_LENGTH, + myControlFunction, + partnerControlFunction, + CANIdentifier::PriorityLowest7); + } + + bool VirtualTerminalClient::send_select_colour_map_or_palette(std::uint16_t objectID) const + { + const std::array buffer = { static_cast(Function::SelectColourMapCommand), + static_cast(objectID & 0xFF), + static_cast(objectID >> 8), + 0xFF, + 0xFF, + 0xFF, + 0xFF, + 0xFF }; + return CANNetworkManager::CANNetwork.send_can_message(static_cast(CANLibParameterGroupNumber::ECUtoVirtualTerminal), + buffer.data(), + CAN_DATA_LENGTH, + myControlFunction, + partnerControlFunction, + CANIdentifier::PriorityLowest7); + } + + bool VirtualTerminalClient::send_execute_extended_macro(std::uint16_t objectID) const + { + const std::array buffer = { static_cast(Function::ExecuteExtendedMacroCommand), + static_cast(objectID & 0xFF), + static_cast(objectID >> 8), + 0xFF, + 0xFF, + 0xFF, + 0xFF, + 0xFF }; + return CANNetworkManager::CANNetwork.send_can_message(static_cast(CANLibParameterGroupNumber::ECUtoVirtualTerminal), + buffer.data(), + CAN_DATA_LENGTH, + myControlFunction, + partnerControlFunction, + CANIdentifier::PriorityLowest7); + } + + bool VirtualTerminalClient::send_select_active_working_set(std::uint64_t NAMEofWorkingSetMasterForDesiredWorkingSet) const + { + const std::array buffer = { static_cast(Function::SelectActiveWorkingSet), + static_cast(NAMEofWorkingSetMasterForDesiredWorkingSet & 0xFF), + static_cast((NAMEofWorkingSetMasterForDesiredWorkingSet >> 8) & 0xFF), + static_cast((NAMEofWorkingSetMasterForDesiredWorkingSet >> 16) & 0xFF), + static_cast((NAMEofWorkingSetMasterForDesiredWorkingSet >> 24) & 0xFF), + static_cast((NAMEofWorkingSetMasterForDesiredWorkingSet >> 32) & 0xFF), + static_cast((NAMEofWorkingSetMasterForDesiredWorkingSet >> 40) & 0xFF), + static_cast((NAMEofWorkingSetMasterForDesiredWorkingSet >> 48) & 0xFF), + static_cast((NAMEofWorkingSetMasterForDesiredWorkingSet >> 56) & 0xFF) }; + return CANNetworkManager::CANNetwork.send_can_message(static_cast(CANLibParameterGroupNumber::ECUtoVirtualTerminal), + buffer.data(), + buffer.size(), + myControlFunction, + partnerControlFunction, + CANIdentifier::PriorityLowest7); + } + + bool VirtualTerminalClient::send_set_graphics_cursor(std::uint16_t objectID, std::int16_t xPosition, std::int16_t yPosition) const + { + const std::array buffer = { static_cast(Function::GraphicsContextCommand), + static_cast(objectID & 0xFF), + static_cast(objectID >> 8), + static_cast(GraphicsContextSubCommandID::SetGraphicsCursor), + static_cast(xPosition & 0xFF), + static_cast(xPosition >> 8), + static_cast(yPosition & 0xFF), + static_cast(yPosition >> 8) }; + return CANNetworkManager::CANNetwork.send_can_message(static_cast(CANLibParameterGroupNumber::ECUtoVirtualTerminal), + buffer.data(), + CAN_DATA_LENGTH, + myControlFunction, + partnerControlFunction, + CANIdentifier::PriorityLowest7); + } + + bool VirtualTerminalClient::send_move_graphics_cursor(std::uint16_t objectID, std::int16_t xOffset, std::int16_t yOffset) const + { + const std::array buffer = { static_cast(Function::GraphicsContextCommand), + static_cast(objectID & 0xFF), + static_cast(objectID >> 8), + static_cast(GraphicsContextSubCommandID::MoveGraphicsCursor), + static_cast(xOffset & 0xFF), + static_cast(xOffset >> 8), + static_cast(yOffset & 0xFF), + static_cast(yOffset >> 8) }; + return CANNetworkManager::CANNetwork.send_can_message(static_cast(CANLibParameterGroupNumber::ECUtoVirtualTerminal), + buffer.data(), + CAN_DATA_LENGTH, + myControlFunction, + partnerControlFunction, + CANIdentifier::PriorityLowest7); + } + + bool VirtualTerminalClient::send_set_foreground_colour(std::uint16_t objectID, std::uint8_t colour) const + { + const std::array buffer = { static_cast(Function::GraphicsContextCommand), + static_cast(objectID & 0xFF), + static_cast(objectID >> 8), + static_cast(GraphicsContextSubCommandID::SetForegroundColour), + colour, + 0xFF, + 0xFF, + 0xFF }; + return CANNetworkManager::CANNetwork.send_can_message(static_cast(CANLibParameterGroupNumber::ECUtoVirtualTerminal), + buffer.data(), + CAN_DATA_LENGTH, + myControlFunction, + partnerControlFunction, + CANIdentifier::PriorityLowest7); + } + + bool VirtualTerminalClient::send_set_background_colour(std::uint16_t objectID, std::uint8_t colour) const + { + const std::array buffer = { static_cast(Function::GraphicsContextCommand), + static_cast(objectID & 0xFF), + static_cast(objectID >> 8), + static_cast(GraphicsContextSubCommandID::SetBackgroundColour), + colour, + 0xFF, + 0xFF, + 0xFF }; + return CANNetworkManager::CANNetwork.send_can_message(static_cast(CANLibParameterGroupNumber::ECUtoVirtualTerminal), + buffer.data(), + CAN_DATA_LENGTH, + myControlFunction, + partnerControlFunction, + CANIdentifier::PriorityLowest7); + } + + bool VirtualTerminalClient::send_set_line_attributes_object_id(std::uint16_t objectID, std::uint16_t lineAttributesObjectID) const + { + const std::array buffer = { static_cast(Function::GraphicsContextCommand), + static_cast(objectID & 0xFF), + static_cast(objectID >> 8), + static_cast(GraphicsContextSubCommandID::SetLineAttributesObjectID), + static_cast(lineAttributesObjectID & 0xFF), + static_cast(lineAttributesObjectID >> 8), + 0xFF, + 0xFF }; + return CANNetworkManager::CANNetwork.send_can_message(static_cast(CANLibParameterGroupNumber::ECUtoVirtualTerminal), + buffer.data(), + CAN_DATA_LENGTH, + myControlFunction, + partnerControlFunction, + CANIdentifier::PriorityLowest7); + } + + bool VirtualTerminalClient::send_set_fill_attributes_object_id(std::uint16_t objectID, std::uint16_t fillAttributesObjectID) const + { + const std::array buffer = { static_cast(Function::GraphicsContextCommand), + static_cast(objectID & 0xFF), + static_cast(objectID >> 8), + static_cast(GraphicsContextSubCommandID::SetFillAttributesObjectID), + static_cast(fillAttributesObjectID & 0xFF), + static_cast(fillAttributesObjectID >> 8), + 0xFF, + 0xFF }; + return CANNetworkManager::CANNetwork.send_can_message(static_cast(CANLibParameterGroupNumber::ECUtoVirtualTerminal), + buffer.data(), + CAN_DATA_LENGTH, + myControlFunction, + partnerControlFunction, + CANIdentifier::PriorityLowest7); + } + + bool VirtualTerminalClient::send_set_font_attributes_object_id(std::uint16_t objectID, std::uint16_t fontAttributesObjectID) const + { + const std::array buffer = { static_cast(Function::GraphicsContextCommand), + static_cast(objectID & 0xFF), + static_cast(objectID >> 8), + static_cast(GraphicsContextSubCommandID::SetFontAttributesObjectID), + static_cast(fontAttributesObjectID & 0xFF), + static_cast(fontAttributesObjectID >> 8), + 0xFF, + 0xFF }; + return CANNetworkManager::CANNetwork.send_can_message(static_cast(CANLibParameterGroupNumber::ECUtoVirtualTerminal), + buffer.data(), + CAN_DATA_LENGTH, + myControlFunction, + partnerControlFunction, + CANIdentifier::PriorityLowest7); + } + + bool VirtualTerminalClient::send_erase_rectangle(std::uint16_t objectID, std::uint16_t width, std::uint16_t height) const + { + const std::array buffer = { static_cast(Function::GraphicsContextCommand), + static_cast(objectID & 0xFF), + static_cast(objectID >> 8), + static_cast(GraphicsContextSubCommandID::EraseRectangle), + static_cast(width & 0xFF), + static_cast(width >> 8), + static_cast(height & 0xFF), + static_cast(height >> 8) }; + return CANNetworkManager::CANNetwork.send_can_message(static_cast(CANLibParameterGroupNumber::ECUtoVirtualTerminal), + buffer.data(), + CAN_DATA_LENGTH, + myControlFunction, + partnerControlFunction, + CANIdentifier::PriorityLowest7); + } + + bool VirtualTerminalClient::send_draw_point(std::uint16_t objectID, std::int16_t xOffset, std::int16_t yOffset) const + { + const std::array buffer = { static_cast(Function::GraphicsContextCommand), + static_cast(objectID & 0xFF), + static_cast(objectID >> 8), + static_cast(GraphicsContextSubCommandID::DrawPoint), + static_cast(xOffset & 0xFF), + static_cast(xOffset >> 8), + static_cast(yOffset & 0xFF), + static_cast(yOffset >> 8) }; + return CANNetworkManager::CANNetwork.send_can_message(static_cast(CANLibParameterGroupNumber::ECUtoVirtualTerminal), + buffer.data(), + CAN_DATA_LENGTH, + myControlFunction, + partnerControlFunction, + CANIdentifier::PriorityLowest7); + } + + bool VirtualTerminalClient::send_draw_line(std::uint16_t objectID, std::int16_t xOffset, std::int16_t yOffset) const + { + const std::array buffer = { static_cast(Function::GraphicsContextCommand), + static_cast(objectID & 0xFF), + static_cast(objectID >> 8), + static_cast(GraphicsContextSubCommandID::DrawLine), + static_cast(xOffset & 0xFF), + static_cast(xOffset >> 8), + static_cast(yOffset & 0xFF), + static_cast(yOffset >> 8) }; + return CANNetworkManager::CANNetwork.send_can_message(static_cast(CANLibParameterGroupNumber::ECUtoVirtualTerminal), + buffer.data(), + CAN_DATA_LENGTH, + myControlFunction, + partnerControlFunction, + CANIdentifier::PriorityLowest7); + } + + bool VirtualTerminalClient::send_draw_rectangle(std::uint16_t objectID, std::uint16_t width, std::uint16_t height) const + { + const std::array buffer = { static_cast(Function::GraphicsContextCommand), + static_cast(objectID & 0xFF), + static_cast(objectID >> 8), + static_cast(GraphicsContextSubCommandID::DrawRectangle), + static_cast(width & 0xFF), + static_cast(width >> 8), + static_cast(height & 0xFF), + static_cast(height >> 8) }; + return CANNetworkManager::CANNetwork.send_can_message(static_cast(CANLibParameterGroupNumber::ECUtoVirtualTerminal), + buffer.data(), + CAN_DATA_LENGTH, + myControlFunction, + partnerControlFunction, + CANIdentifier::PriorityLowest7); + } + + bool VirtualTerminalClient::send_draw_closed_ellipse(std::uint16_t objectID, std::uint16_t width, std::uint16_t height) const + { + const std::array buffer = { static_cast(Function::GraphicsContextCommand), + static_cast(objectID & 0xFF), + static_cast(objectID >> 8), + static_cast(GraphicsContextSubCommandID::DrawClosedEllipse), + static_cast(width & 0xFF), + static_cast(width >> 8), + static_cast(height & 0xFF), + static_cast(height >> 8) }; + return CANNetworkManager::CANNetwork.send_can_message(static_cast(CANLibParameterGroupNumber::ECUtoVirtualTerminal), + buffer.data(), + CAN_DATA_LENGTH, + myControlFunction, + partnerControlFunction, + CANIdentifier::PriorityLowest7); + } + + bool VirtualTerminalClient::send_draw_polygon(std::uint16_t objectID, std::uint8_t numberOfPoints, std::int16_t *listOfXOffsetsRelativeToCursor, std::int16_t *listOfYOffsetsRelativeToCursor) const + { + bool retVal = false; + + if ((numberOfPoints > 0) && + (nullptr != listOfXOffsetsRelativeToCursor) && + (nullptr != listOfYOffsetsRelativeToCursor)) + + { + const std::uint16_t messageLength = (9 + (4 * numberOfPoints)); + std::vector buffer; + buffer.resize(messageLength); + buffer[0] = static_cast(Function::GraphicsContextCommand); + buffer[1] = static_cast(objectID & 0xFF); + buffer[2] = static_cast(objectID >> 8); + buffer[3] = static_cast(GraphicsContextSubCommandID::DrawPolygon); + buffer[4] = numberOfPoints; + for (std::uint16_t i = 0; i < numberOfPoints; i += 4) + { + buffer[5 + i] = static_cast(listOfXOffsetsRelativeToCursor[0] & 0xFF); + buffer[6 + i] = static_cast((listOfXOffsetsRelativeToCursor[0] >> 8) & 0xFF); + buffer[7 + i] = static_cast(listOfYOffsetsRelativeToCursor[0] & 0xFF); + buffer[8 + i] = static_cast((listOfYOffsetsRelativeToCursor[0] >> 8) & 0xFF); + } + retVal = CANNetworkManager::CANNetwork.send_can_message(static_cast(CANLibParameterGroupNumber::ECUtoVirtualTerminal), + buffer.data(), + buffer.size(), + myControlFunction, + partnerControlFunction, + CANIdentifier::PriorityLowest7); + } + return retVal; + } + + bool VirtualTerminalClient::send_draw_text(std::uint16_t objectID, bool transparent, std::uint8_t textLength, const char *value) const + { + bool retVal = false; + + if ((nullptr != value) && + (0 != textLength)) + { + std::uint16_t messageLength = (6 + textLength); + std::vector buffer; + buffer.resize(messageLength); + buffer[0] = static_cast(Function::GraphicsContextCommand); + buffer[1] = static_cast(objectID & 0xFF); + buffer[2] = static_cast(objectID >> 8); + buffer[3] = static_cast(GraphicsContextSubCommandID::DrawText); + buffer[4] = static_cast(transparent); + buffer[5] = textLength; + memcpy(&buffer[6], value, textLength); + + while (buffer.size() < CAN_DATA_LENGTH) + { + buffer.push_back(0xFF); // Pad short text to minimum message length + } + retVal = CANNetworkManager::CANNetwork.send_can_message(static_cast(CANLibParameterGroupNumber::ECUtoVirtualTerminal), + buffer.data(), + buffer.size(), + myControlFunction, + partnerControlFunction, + CANIdentifier::PriorityLowest7); + } + return retVal; + } + + bool VirtualTerminalClient::send_pan_viewport(std::uint16_t objectID, std::int16_t xAttribute, std::int16_t yAttribute) const + { + const std::array buffer = { static_cast(Function::GraphicsContextCommand), + static_cast(objectID & 0xFF), + static_cast(objectID >> 8), + static_cast(GraphicsContextSubCommandID::PanViewport), + static_cast(xAttribute & 0xFF), + static_cast(xAttribute >> 8), + static_cast(yAttribute & 0xFF), + static_cast(yAttribute >> 8) }; + return CANNetworkManager::CANNetwork.send_can_message(static_cast(CANLibParameterGroupNumber::ECUtoVirtualTerminal), + buffer.data(), + CAN_DATA_LENGTH, + myControlFunction, + partnerControlFunction, + CANIdentifier::PriorityLowest7); + } + + bool VirtualTerminalClient::send_zoom_viewport(std::uint16_t objectID, float zoom) const + { + static_assert(sizeof(float) == 4, "Float must be 4 bytes"); + std::array floatBytes = { 0 }; + memcpy(floatBytes.data(), &zoom, sizeof(float)); + + if (is_big_endian()) + { + std::reverse(floatBytes.begin(), floatBytes.end()); + } + + const std::array buffer = { static_cast(Function::GraphicsContextCommand), + static_cast(objectID & 0xFF), + static_cast(objectID >> 8), + static_cast(GraphicsContextSubCommandID::ZoomViewport), + floatBytes[0], + floatBytes[1], + floatBytes[2], + floatBytes[3] }; + return CANNetworkManager::CANNetwork.send_can_message(static_cast(CANLibParameterGroupNumber::ECUtoVirtualTerminal), + buffer.data(), + CAN_DATA_LENGTH, + myControlFunction, + partnerControlFunction, + CANIdentifier::PriorityLowest7); + } + + bool VirtualTerminalClient::send_pan_and_zoom_viewport(std::uint16_t objectID, std::int16_t xAttribute, std::int16_t yAttribute, float zoom) const + { + static_assert(sizeof(float) == 4, "Float must be 4 bytes"); + std::array floatBytes = { 0 }; + memcpy(floatBytes.data(), &zoom, sizeof(float)); + + if (is_big_endian()) + { + std::reverse(floatBytes.begin(), floatBytes.end()); + } + + const std::array buffer = { static_cast(Function::GraphicsContextCommand), + static_cast(objectID & 0xFF), + static_cast(objectID >> 8), + static_cast(GraphicsContextSubCommandID::PanAndZoomViewport), + static_cast(xAttribute & 0xFF), + static_cast(xAttribute >> 8), + static_cast(yAttribute & 0xFF), + static_cast(yAttribute >> 8), + floatBytes[0], + floatBytes[1], + floatBytes[2], + floatBytes[3] }; + return CANNetworkManager::CANNetwork.send_can_message(static_cast(CANLibParameterGroupNumber::ECUtoVirtualTerminal), + buffer.data(), + buffer.size(), + myControlFunction, + partnerControlFunction, + CANIdentifier::PriorityLowest7); + } + + bool VirtualTerminalClient::send_change_viewport_size(std::uint16_t objectID, std::uint16_t width, std::uint16_t height) const + { + constexpr std::uint16_t MAX_WIDTH_HEIGHT = 32767; + bool retVal = false; + + if ((width <= MAX_WIDTH_HEIGHT) && + (height <= MAX_WIDTH_HEIGHT)) + { + const std::array buffer = { static_cast(Function::GraphicsContextCommand), + static_cast(objectID & 0xFF), + static_cast(objectID >> 8), + static_cast(GraphicsContextSubCommandID::ChangeViewportSize), + static_cast(width & 0xFF), + static_cast(width >> 8), + static_cast(height & 0xFF), + static_cast(height >> 8) }; + retVal = CANNetworkManager::CANNetwork.send_can_message(static_cast(CANLibParameterGroupNumber::ECUtoVirtualTerminal), + buffer.data(), + CAN_DATA_LENGTH, + myControlFunction, + partnerControlFunction, + CANIdentifier::PriorityLowest7); + } + return retVal; + } + + bool VirtualTerminalClient::send_draw_vt_object(std::uint16_t graphicsContextObjectID, std::uint16_t objectID) const + { + const std::array buffer = { static_cast(Function::GraphicsContextCommand), + static_cast(graphicsContextObjectID & 0xFF), + static_cast(graphicsContextObjectID >> 8), + static_cast(GraphicsContextSubCommandID::DrawVTObject), + static_cast(objectID & 0xFF), + static_cast(objectID >> 8), + 0xFF, + 0xFF }; + return CANNetworkManager::CANNetwork.send_can_message(static_cast(CANLibParameterGroupNumber::ECUtoVirtualTerminal), + buffer.data(), + CAN_DATA_LENGTH, + myControlFunction, + partnerControlFunction, + CANIdentifier::PriorityLowest7); + } + + bool VirtualTerminalClient::send_copy_canvas_to_picture_graphic(std::uint16_t graphicsContextObjectID, std::uint16_t objectID) const + { + const std::array buffer = { static_cast(Function::GraphicsContextCommand), + static_cast(graphicsContextObjectID & 0xFF), + static_cast(graphicsContextObjectID >> 8), + static_cast(GraphicsContextSubCommandID::CopyCanvasToPictureGraphic), + static_cast(objectID & 0xFF), + static_cast(objectID >> 8), + 0xFF, + 0xFF }; + return CANNetworkManager::CANNetwork.send_can_message(static_cast(CANLibParameterGroupNumber::ECUtoVirtualTerminal), + buffer.data(), + CAN_DATA_LENGTH, + myControlFunction, + partnerControlFunction, + CANIdentifier::PriorityLowest7); + } + + bool VirtualTerminalClient::send_copy_viewport_to_picture_graphic(std::uint16_t graphicsContextObjectID, std::uint16_t objectID) const + { + const std::array buffer = { static_cast(Function::GraphicsContextCommand), + static_cast(graphicsContextObjectID & 0xFF), + static_cast(graphicsContextObjectID >> 8), + static_cast(GraphicsContextSubCommandID::CopyViewportToPictureGraphic), + static_cast(objectID & 0xFF), + static_cast(objectID >> 8), + 0xFF, + 0xFF }; + return CANNetworkManager::CANNetwork.send_can_message(static_cast(CANLibParameterGroupNumber::ECUtoVirtualTerminal), + buffer.data(), + CAN_DATA_LENGTH, + myControlFunction, + partnerControlFunction, + CANIdentifier::PriorityLowest7); + } + + bool VirtualTerminalClient::send_get_attribute_value(std::uint16_t objectID, std::uint8_t attributeID) const + { + const std::array buffer = { static_cast(Function::GetAttributeValueMessage), + static_cast(objectID & 0xFF), + static_cast(objectID >> 8), + attributeID, + 0xFF, + 0xFF, + 0xFF, + 0xFF }; + return CANNetworkManager::CANNetwork.send_can_message(static_cast(CANLibParameterGroupNumber::ECUtoVirtualTerminal), + buffer.data(), + CAN_DATA_LENGTH, + myControlFunction, + partnerControlFunction, + CANIdentifier::PriorityLowest7); + } + + std::uint8_t VirtualTerminalClient::get_softkey_x_axis_pixels() const + { + return softKeyXAxisPixels; + } + + std::uint8_t VirtualTerminalClient::get_softkey_y_axis_pixels() const + { + return softKeyYAxisPixels; + } + + std::uint8_t VirtualTerminalClient::get_number_virtual_softkeys() const + { + return numberVirtualSoftkeysPerSoftkeyMask; + } + + std::uint8_t VirtualTerminalClient::get_number_physical_softkeys() const + { + return numberPhysicalSoftkeys; + } + + bool VirtualTerminalClient::get_font_size_supported(FontSize value) const + { + bool retVal = false; + + switch (value) + { + case FontSize::Size6x8: + case FontSize::Size8x8: + case FontSize::Size8x12: + case FontSize::Size12x16: + case FontSize::Size16x16: + case FontSize::Size16x24: + case FontSize::Size24x32: + case FontSize::Size32x32: + { + retVal = (0 != (smallFontSizesBitfield & (1 << static_cast(value)))); + } + break; + + case FontSize::Size32x48: + case FontSize::Size48x64: + case FontSize::Size64x64: + case FontSize::Size64x96: + case FontSize::Size96x128: + case FontSize::Size128x128: + case FontSize::Size128x192: + { + retVal = (0 != (largeFontSizesBitfield & (1 << (static_cast(value) - static_cast(FontSize::Size32x48))))); + } + break; + } + return retVal; + } + + bool VirtualTerminalClient::get_font_style_supported(FontStyleBits value) const + { + return (0 != (static_cast(value) & fontStylesBitfield)); + } + + VirtualTerminalClient::GraphicMode VirtualTerminalClient::get_graphic_mode() const + { + return supportedGraphicsMode; + } + + bool VirtualTerminalClient::get_support_touchscreen_with_pointing_message() const + { + return (0 != (hardwareFeaturesBitfield & 0x01)); + } + + bool VirtualTerminalClient::get_support_pointing_device_with_pointing_message() const + { + return (0 != (hardwareFeaturesBitfield & 0x02)); + } + + bool VirtualTerminalClient::get_multiple_frequency_audio_output() const + { + return (0 != (hardwareFeaturesBitfield & 0x04)); + } + + bool VirtualTerminalClient::get_has_adjustable_volume_output() const + { + return (0 != (hardwareFeaturesBitfield & 0x08)); + } + + bool VirtualTerminalClient::get_support_simultaneous_activation_physical_keys() const + { + return (0 != (hardwareFeaturesBitfield & 0x10)); + } + + bool VirtualTerminalClient::get_support_simultaneous_activation_buttons_and_softkeys() const + { + return (0 != (hardwareFeaturesBitfield & 0x20)); + } + + bool VirtualTerminalClient::get_support_drag_operation() const + { + return (0 != (hardwareFeaturesBitfield & 0x40)); + } + + bool VirtualTerminalClient::get_support_intermediate_coordinates_during_drag_operations() const + { + return (0 != (hardwareFeaturesBitfield & 0x80)); + } + + std::uint16_t VirtualTerminalClient::get_number_x_pixels() const + { + return xPixels; + } + + std::uint16_t VirtualTerminalClient::get_number_y_pixels() const + { + return yPixels; + } + + VirtualTerminalClient::VTVersion VirtualTerminalClient::get_connected_vt_version() const + { + VTVersion retVal; + + switch (connectedVTVersion) + { + case 0x03: + { + retVal = VTVersion::Version3; + } + break; + + case 0x04: + { + retVal = VTVersion::Version4; + } + break; + + case 0x05: + { + retVal = VTVersion::Version5; + } + break; + + case 0x06: + { + retVal = VTVersion::Version6; + } + break; + + case 0xFF: + { + retVal = VTVersion::Version2OrOlder; + } + break; + + default: + { + retVal = VTVersion::ReservedOrUnknown; + } + break; + } + return retVal; + } + + bool VirtualTerminalClient::get_vt_version_supported(VTVersion minimumVersion) const + { + bool retVal = false; + + if (get_connected_vt_version() != VTVersion::ReservedOrUnknown) + { + retVal = get_connected_vt_version() >= minimumVersion; + } + + return retVal; + } + + std::uint16_t VirtualTerminalClient::get_visible_data_mask() const + { + return activeWorkingSetDataMaskObjectID; + } + + std::uint16_t VirtualTerminalClient::get_visible_soft_key_mask() const + { + return activeWorkingSetSoftKeyMaskObjectID; + } + + void VirtualTerminalClient::set_object_pool(std::uint8_t poolIndex, VTVersion poolSupportedVTVersion, const std::uint8_t *pool, std::uint32_t size, std::string version) + { + if ((nullptr != pool) && + (0 != size)) + { + ObjectPoolDataStruct tempData; + + tempData.objectPoolDataPointer = pool; + tempData.objectPoolVectorPointer = nullptr; + tempData.dataCallback = nullptr; + tempData.objectPoolSize = size; + tempData.autoScaleDataMaskOriginalDimension = 0; + tempData.autoScaleSoftKeyDesignatorOriginalHeight = 0; + tempData.version = poolSupportedVTVersion; + tempData.useDataCallback = false; + tempData.uploaded = false; + tempData.versionLabel = version; + + if (poolIndex < objectPools.size()) + { + objectPools[poolIndex] = tempData; + } + else + { + objectPools.resize(poolIndex + 1); + objectPools[poolIndex] = tempData; + } + } + } + + void VirtualTerminalClient::set_object_pool(std::uint8_t poolIndex, VTVersion poolSupportedVTVersion, const std::vector *pool, std::string version) + { + if ((nullptr != pool) && + (0 != pool->size())) + { + ObjectPoolDataStruct tempData; + + tempData.objectPoolDataPointer = nullptr; + tempData.objectPoolVectorPointer = pool; + tempData.dataCallback = nullptr; + tempData.objectPoolSize = pool->size(); + tempData.autoScaleDataMaskOriginalDimension = 0; + tempData.autoScaleSoftKeyDesignatorOriginalHeight = 0; + tempData.version = poolSupportedVTVersion; + tempData.useDataCallback = false; + tempData.uploaded = false; + tempData.versionLabel = version; + + if (poolIndex < objectPools.size()) + { + objectPools[poolIndex] = tempData; + } + else + { + objectPools.resize(poolIndex + 1); + objectPools[poolIndex] = tempData; + } + } + } + + void VirtualTerminalClient::set_object_pool_scaling(std::uint8_t poolIndex, + std::uint32_t originalDataMaskDimensions_px, + std::uint32_t originalSoftKyeDesignatorHeight_px) + { + // You have to call set_object_pool or register_object_pool_data_chunk_callback before calling this function + assert(poolIndex < objectPools.size()); + objectPools[poolIndex].autoScaleDataMaskOriginalDimension = originalDataMaskDimensions_px; + objectPools[poolIndex].autoScaleSoftKeyDesignatorOriginalHeight = originalSoftKyeDesignatorHeight_px; + } + + void VirtualTerminalClient::register_object_pool_data_chunk_callback(std::uint8_t poolIndex, VTVersion poolSupportedVTVersion, std::uint32_t poolTotalSize, DataChunkCallback value, std::string version) + { + if ((nullptr != value) && + (0 != poolTotalSize)) + { + ObjectPoolDataStruct tempData; + + tempData.objectPoolDataPointer = nullptr; + tempData.objectPoolVectorPointer = nullptr; + tempData.dataCallback = value; + tempData.objectPoolSize = poolTotalSize; + tempData.version = poolSupportedVTVersion; + tempData.useDataCallback = true; + tempData.uploaded = false; + tempData.autoScaleSoftKeyDesignatorOriginalHeight = 0; + tempData.autoScaleDataMaskOriginalDimension = 0; + tempData.versionLabel = version; + + if (poolIndex < objectPools.size()) + { + objectPools[poolIndex] = tempData; + } + else + { + objectPools.resize(poolIndex + 1); + objectPools[poolIndex] = tempData; + } + } + } + + void VirtualTerminalClient::update() + { + StateMachineState previousStateMachineState = state; // Save state to see if it changes this update + + if (nullptr != partnerControlFunction) + { + switch (state) + { + case StateMachineState::Disconnected: + { + sendWorkingSetMaintenance = false; + sendAuxiliaryMaintenance = false; + + if (partnerControlFunction->get_address_valid()) + { + set_state(StateMachineState::WaitForPartnerVTStatusMessage); + } + } + break; + + case StateMachineState::WaitForPartnerVTStatusMessage: + { + if (0 != lastVTStatusTimestamp_ms) + { + set_state(StateMachineState::SendWorkingSetMasterMessage); + } + } + break; + + case StateMachineState::SendWorkingSetMasterMessage: + { + if (send_working_set_master()) + { + set_state(StateMachineState::ReadyForObjectPool); + } + } + break; + + case StateMachineState::ReadyForObjectPool: + { + // If we're in this state, we are ready to upload the + // object pool but no pool has been set to this class + // so the state machine cannot progress. + if (SystemTiming::time_expired_ms(lastVTStatusTimestamp_ms, VT_STATUS_TIMEOUT_MS)) + { + CANStackLogger::CAN_stack_log(CANStackLogger::LoggingLevel::Error, "[VT]: Ready to upload pool, but VT server has timed out. Disconnecting."); + set_state(StateMachineState::Disconnected); + } + + if (0 != objectPools.size()) + { + set_state(StateMachineState::SendGetMemory); + send_working_set_maintenance(true, objectPools[0].version); + lastWorkingSetMaintenanceTimestamp_ms = SystemTiming::get_timestamp_ms(); + sendWorkingSetMaintenance = true; + sendAuxiliaryMaintenance = true; + } + } + break; + + case StateMachineState::SendGetMemory: + { + std::uint32_t totalPoolSize = 0; + + for (auto &pool : objectPools) + { + totalPoolSize += pool.objectPoolSize; + } + + if (send_get_memory(totalPoolSize)) + { + set_state(StateMachineState::WaitForGetMemoryResponse); + } + } + break; + + case StateMachineState::WaitForGetMemoryResponse: + { + if (SystemTiming::time_expired_ms(stateMachineTimestamp_ms, VT_STATUS_TIMEOUT_MS)) + { + set_state(StateMachineState::Failed); + CANStackLogger::CAN_stack_log(CANStackLogger::LoggingLevel::Error, "[VT]: Get Memory Response Timeout"); + } + } + break; + + case StateMachineState::SendGetNumberSoftkeys: + { + if (send_get_number_of_softkeys()) + { + set_state(StateMachineState::WaitForGetNumberSoftKeysResponse); + } + } + break; + + case StateMachineState::WaitForGetNumberSoftKeysResponse: + { + if (SystemTiming::time_expired_ms(stateMachineTimestamp_ms, VT_STATUS_TIMEOUT_MS)) + { + set_state(StateMachineState::Failed); + CANStackLogger::CAN_stack_log(CANStackLogger::LoggingLevel::Error, "[VT]: Get Number Softkeys Response Timeout"); + } + } + break; + + case StateMachineState::SendGetTextFontData: + { + if (send_get_text_font_data()) + { + set_state(StateMachineState::WaitForGetTextFontDataResponse); + } + } + break; + + case StateMachineState::WaitForGetTextFontDataResponse: + { + if (SystemTiming::time_expired_ms(stateMachineTimestamp_ms, VT_STATUS_TIMEOUT_MS)) + { + set_state(StateMachineState::Failed); + CANStackLogger::CAN_stack_log(CANStackLogger::LoggingLevel::Error, "[VT]: Get Text Font Data Response Timeout"); + } + } + break; + + case StateMachineState::SendGetHardware: + { + if (send_get_hardware()) + { + set_state(StateMachineState::WaitForGetHardwareResponse); + } + } + break; + + case StateMachineState::WaitForGetHardwareResponse: + { + if (SystemTiming::time_expired_ms(stateMachineTimestamp_ms, VT_STATUS_TIMEOUT_MS)) + { + set_state(StateMachineState::Failed); + CANStackLogger::CAN_stack_log(CANStackLogger::LoggingLevel::Error, "[VT]: Get Hardware Response Timeout"); + } + } + break; + + case StateMachineState::SendGetVersions: + { + if (SystemTiming::time_expired_ms(stateMachineTimestamp_ms, VT_STATUS_TIMEOUT_MS)) + { + set_state(StateMachineState::Failed); + CANStackLogger::CAN_stack_log(CANStackLogger::LoggingLevel::Error, "[VT]: Get Versions Timeout"); + } + else if ((!objectPools.empty()) && + (!objectPools[0].versionLabel.empty()) && + (send_get_versions())) + { + set_state(StateMachineState::WaitForGetVersionsResponse); + } + } + break; + + case StateMachineState::WaitForGetVersionsResponse: + { + if (SystemTiming::time_expired_ms(stateMachineTimestamp_ms, VT_STATUS_TIMEOUT_MS)) + { + set_state(StateMachineState::Failed); + CANStackLogger::CAN_stack_log(CANStackLogger::LoggingLevel::Error, "[VT]: Get Versions Response Timeout"); + } + } + break; + + case StateMachineState::SendLoadVersion: + { + if (SystemTiming::time_expired_ms(stateMachineTimestamp_ms, VT_STATUS_TIMEOUT_MS)) + { + set_state(StateMachineState::Failed); + CANStackLogger::CAN_stack_log(CANStackLogger::LoggingLevel::Error, "[VT]: Send Load Version Timeout"); + } + else + { + constexpr std::uint8_t VERSION_LABEL_LENGTH = 7; + std::array tempVersionBuffer; + + // Unused bytes filled with spaces + tempVersionBuffer[0] = ' '; + tempVersionBuffer[1] = ' '; + tempVersionBuffer[2] = ' '; + tempVersionBuffer[3] = ' '; + tempVersionBuffer[4] = ' '; + tempVersionBuffer[5] = ' '; + tempVersionBuffer[6] = ' '; + + for (std::size_t i = 0; ((i < VERSION_LABEL_LENGTH) && (i < objectPools[0].versionLabel.size())); i++) + { + tempVersionBuffer[i] = objectPools[0].versionLabel[i]; + } + + if (send_load_version(tempVersionBuffer)) + { + set_state(StateMachineState::WaitForLoadVersionResponse); + } + } + } + break; + + case StateMachineState::WaitForLoadVersionResponse: + { + if (SystemTiming::time_expired_ms(stateMachineTimestamp_ms, VT_STATUS_TIMEOUT_MS)) + { + set_state(StateMachineState::Failed); + CANStackLogger::CAN_stack_log(CANStackLogger::LoggingLevel::Error, "[VT]: Load Version Response Timeout"); + } + } + break; + + case StateMachineState::SendStoreVersion: + { + if (SystemTiming::time_expired_ms(stateMachineTimestamp_ms, VT_STATUS_TIMEOUT_MS)) + { + set_state(StateMachineState::Failed); + CANStackLogger::CAN_stack_log(CANStackLogger::LoggingLevel::Error, "[VT]: Send Store Version Timeout"); + } + else + { + constexpr std::uint8_t VERSION_LABEL_LENGTH = 7; + std::array tempVersionBuffer; + + // Unused bytes filled with spaces + tempVersionBuffer[0] = ' '; + tempVersionBuffer[1] = ' '; + tempVersionBuffer[2] = ' '; + tempVersionBuffer[3] = ' '; + tempVersionBuffer[4] = ' '; + tempVersionBuffer[5] = ' '; + tempVersionBuffer[6] = ' '; + + for (std::size_t i = 0; ((i < VERSION_LABEL_LENGTH) && (i < objectPools[0].versionLabel.size())); i++) + { + tempVersionBuffer[i] = objectPools[0].versionLabel[i]; + } + + if (send_store_version(tempVersionBuffer)) + { + set_state(StateMachineState::WaitForStoreVersionResponse); + } + } + } + break; + + case StateMachineState::WaitForStoreVersionResponse: + { + if (SystemTiming::time_expired_ms(stateMachineTimestamp_ms, VT_STATUS_TIMEOUT_MS)) + { + set_state(StateMachineState::Failed); + CANStackLogger::CAN_stack_log(CANStackLogger::LoggingLevel::Error, "[VT]: Store Version Response Timeout"); + } + } + break; + + case StateMachineState::UploadObjectPool: + { + bool allPoolsProcessed = true; + + if (firstTimeInState) + { + if (get_any_pool_needs_scaling()) + { + // Scale object pools before upload. + if (!scale_object_pools()) + { + set_state(StateMachineState::Failed); + } + } + } + + for (std::uint32_t i = 0; i < objectPools.size(); i++) + { + if (((nullptr != objectPools[i].objectPoolDataPointer) || + (nullptr != objectPools[i].dataCallback)) && + (objectPools[i].objectPoolSize > 0)) + { + if (!objectPools[i].uploaded) + { + allPoolsProcessed = false; + } + + if (CurrentObjectPoolUploadState::Uninitialized == currentObjectPoolState) + { + if (!objectPools[i].uploaded) + { + bool transmitSuccessful = CANNetworkManager::CANNetwork.send_can_message(static_cast(CANLibParameterGroupNumber::ECUtoVirtualTerminal), + nullptr, + objectPools[i].objectPoolSize + 1, // Account for Mux byte + myControlFunction, + partnerControlFunction, + CANIdentifier::CANPriority::PriorityLowest7, + process_callback, + this, + process_internal_object_pool_upload_callback); + + if (transmitSuccessful) + { + currentObjectPoolState = CurrentObjectPoolUploadState::InProgress; + } + } + else + { + // Pool already uploaded, move on to the next one + } + } + else if (CurrentObjectPoolUploadState::Success == currentObjectPoolState) + { + objectPools[i].uploaded = true; + currentObjectPoolState = CurrentObjectPoolUploadState::Uninitialized; + } + else if (CurrentObjectPoolUploadState::Failed == currentObjectPoolState) + { + currentObjectPoolState = CurrentObjectPoolUploadState::Uninitialized; + CANStackLogger::CAN_stack_log(CANStackLogger::LoggingLevel::Error, "[VT]: An object pool failed to upload. Resetting connection to VT."); + set_state(StateMachineState::Disconnected); + } + else + { + // Transfer is in progress. Nothing to do now. + break; + } + } + else + { + CANStackLogger::CAN_stack_log(CANStackLogger::LoggingLevel::Warning, "[VT]: An object pool was supplied with an invalid size or pointer. Ignoring it."); + objectPools[i].uploaded = true; + } + } + + if (allPoolsProcessed) + { + set_state(StateMachineState::SendEndOfObjectPool); + } + } + break; + + case StateMachineState::SendEndOfObjectPool: + { + if (send_end_of_object_pool()) + { + set_state(StateMachineState::WaitForEndOfObjectPoolResponse); + } + } + break; + + case StateMachineState::WaitForEndOfObjectPoolResponse: + { + if (SystemTiming::time_expired_ms(stateMachineTimestamp_ms, VT_STATUS_TIMEOUT_MS)) + { + set_state(StateMachineState::Failed); + CANStackLogger::CAN_stack_log(CANStackLogger::LoggingLevel::Error, "[VT]: Get End of Object Pool Response Timeout"); + } + } + break; + + case StateMachineState::Connected: + { + // Check for timeouts + if (SystemTiming::time_expired_ms(lastVTStatusTimestamp_ms, VT_STATUS_TIMEOUT_MS)) + { + set_state(StateMachineState::Disconnected); + CANStackLogger::CAN_stack_log(CANStackLogger::LoggingLevel::Error, "[VT]: Status Timeout"); + } + update_auxiliary_input_status(); + } + break; + + case StateMachineState::Failed: + { + constexpr std::uint32_t VT_STATE_MACHINE_RETRY_TIMEOUT_MS = 5000; + sendWorkingSetMaintenance = false; + sendAuxiliaryMaintenance = false; + + // Retry connecting after a while + if (SystemTiming::time_expired_ms(stateMachineTimestamp_ms, VT_STATE_MACHINE_RETRY_TIMEOUT_MS)) + { + CANStackLogger::CAN_stack_log(CANStackLogger::LoggingLevel::Info, "[VT]: Resetting Failed VT Connection"); + set_state(StateMachineState::Disconnected); + } + } + break; + + default: + { + } + break; + } + } + else + { + set_state(StateMachineState::Disconnected); + } + + if ((sendWorkingSetMaintenance) && + (SystemTiming::time_expired_ms(lastWorkingSetMaintenanceTimestamp_ms, WORKING_SET_MAINTENANCE_TIMEOUT_MS))) + { + txFlags.set_flag(static_cast(TransmitFlags::SendWorkingSetMaintenance)); + } + if ((sendAuxiliaryMaintenance) && + (!ourAuxiliaryInputs.empty()) && + (SystemTiming::time_expired_ms(lastAuxiliaryMaintenanceTimestamp_ms, AUXILIARY_MAINTENANCE_TIMEOUT_MS))) + { + /// @todo We should make sure that when we disconnect/reconnect atleast 500ms has passed since the last auxiliary maintenance message + txFlags.set_flag(static_cast(TransmitFlags::SendAuxiliaryMaintenance)); + } + txFlags.process_all_flags(); + + if (state == previousStateMachineState) + { + firstTimeInState = false; + } + } + + bool VirtualTerminalClient::send_delete_object_pool() const + { + constexpr std::array buffer = { static_cast(Function::DeleteObjectPoolCommand), + 0xFF, + 0xFF, + 0xFF, + 0xFF, + 0xFF, + 0xFF, + 0xFF }; + return CANNetworkManager::CANNetwork.send_can_message(static_cast(CANLibParameterGroupNumber::ECUtoVirtualTerminal), + buffer.data(), + CAN_DATA_LENGTH, + myControlFunction, + partnerControlFunction, + CANIdentifier::PriorityLowest7); + } + + bool VirtualTerminalClient::send_working_set_maintenance(bool initializing, VTVersion workingSetVersion) const + { + std::uint8_t versionByte; + std::uint8_t bitmask = (initializing ? 0x01 : 0x00); + + switch (workingSetVersion) + { + case VTVersion::Version3: + { + versionByte = 0x03; + } + break; + + case VTVersion::Version4: + { + versionByte = 0x04; + } + break; + + case VTVersion::Version5: + { + versionByte = 0x05; + } + break; + + case VTVersion::Version6: + { + versionByte = 0x06; + } + break; + + default: + { + versionByte = 0xFF; + } + break; + } + + const std::array buffer = { static_cast(Function::WorkingSetMaintenanceMessage), + bitmask, + versionByte, + 0xFF, + 0xFF, + 0xFF, + 0xFF, + 0xFF }; + return CANNetworkManager::CANNetwork.send_can_message(static_cast(CANLibParameterGroupNumber::ECUtoVirtualTerminal), + buffer.data(), + CAN_DATA_LENGTH, + myControlFunction, + partnerControlFunction, + CANIdentifier::PriorityLowest7); + } + + bool VirtualTerminalClient::send_get_memory(std::uint32_t requiredMemory) const + { + const std::array buffer = { static_cast(Function::GetMemoryMessage), + 0xFF, + static_cast(requiredMemory & 0xFF), + static_cast((requiredMemory >> 8) & 0xFF), + static_cast((requiredMemory >> 16) & 0xFF), + static_cast((requiredMemory >> 24) & 0xFF), + 0xFF, + 0xFF }; + return CANNetworkManager::CANNetwork.send_can_message(static_cast(CANLibParameterGroupNumber::ECUtoVirtualTerminal), + buffer.data(), + CAN_DATA_LENGTH, + myControlFunction, + partnerControlFunction, + CANIdentifier::PriorityLowest7); + } + + bool VirtualTerminalClient::send_get_number_of_softkeys() const + { + constexpr std::array buffer = { static_cast(Function::GetNumberOfSoftKeysMessage), + 0xFF, + 0xFF, + 0xFF, + 0xFF, + 0xFF, + 0xFF, + 0xFF }; + return CANNetworkManager::CANNetwork.send_can_message(static_cast(CANLibParameterGroupNumber::ECUtoVirtualTerminal), + buffer.data(), + CAN_DATA_LENGTH, + myControlFunction, + partnerControlFunction, + CANIdentifier::PriorityLowest7); + } + + bool VirtualTerminalClient::send_get_text_font_data() const + { + constexpr std::array buffer = { static_cast(Function::GetTextFontDataMessage), + 0xFF, + 0xFF, + 0xFF, + 0xFF, + 0xFF, + 0xFF, + 0xFF }; + return CANNetworkManager::CANNetwork.send_can_message(static_cast(CANLibParameterGroupNumber::ECUtoVirtualTerminal), + buffer.data(), + CAN_DATA_LENGTH, + myControlFunction, + partnerControlFunction, + CANIdentifier::PriorityLowest7); + } + + bool VirtualTerminalClient::send_get_hardware() const + { + constexpr std::array buffer = { static_cast(Function::GetHardwareMessage), + 0xFF, + 0xFF, + 0xFF, + 0xFF, + 0xFF, + 0xFF, + 0xFF }; + return CANNetworkManager::CANNetwork.send_can_message(static_cast(CANLibParameterGroupNumber::ECUtoVirtualTerminal), + buffer.data(), + CAN_DATA_LENGTH, + myControlFunction, + partnerControlFunction, + CANIdentifier::PriorityLowest7); + } + + bool VirtualTerminalClient::send_get_supported_widechars() const + { + constexpr std::array buffer = { static_cast(Function::GetSupportedWidecharsMessage), + 0xFF, + 0xFF, + 0xFF, + 0xFF, + 0xFF, + 0xFF, + 0xFF }; + return CANNetworkManager::CANNetwork.send_can_message(static_cast(CANLibParameterGroupNumber::ECUtoVirtualTerminal), + buffer.data(), + CAN_DATA_LENGTH, + myControlFunction, + partnerControlFunction, + CANIdentifier::PriorityLowest7); + } + + bool VirtualTerminalClient::send_get_window_mask_data() const + { + constexpr std::array buffer = { static_cast(Function::GetWindowMaskDataMessage), + 0xFF, + 0xFF, + 0xFF, + 0xFF, + 0xFF, + 0xFF, + 0xFF }; + return CANNetworkManager::CANNetwork.send_can_message(static_cast(CANLibParameterGroupNumber::ECUtoVirtualTerminal), + buffer.data(), + CAN_DATA_LENGTH, + myControlFunction, + partnerControlFunction, + CANIdentifier::PriorityLowest7); + } + + bool VirtualTerminalClient::send_get_supported_objects() const + { + constexpr std::array buffer = { static_cast(Function::GetSupportedObjectsMessage), + 0xFF, + 0xFF, + 0xFF, + 0xFF, + 0xFF, + 0xFF, + 0xFF }; + return CANNetworkManager::CANNetwork.send_can_message(static_cast(CANLibParameterGroupNumber::ECUtoVirtualTerminal), + buffer.data(), + CAN_DATA_LENGTH, + myControlFunction, + partnerControlFunction, + CANIdentifier::PriorityLowest7); + } + + bool VirtualTerminalClient::send_get_versions() const + { + constexpr std::array buffer = { static_cast(Function::GetVersionsMessage), + 0xFF, + 0xFF, + 0xFF, + 0xFF, + 0xFF, + 0xFF, + 0xFF }; + return CANNetworkManager::CANNetwork.send_can_message(static_cast(CANLibParameterGroupNumber::ECUtoVirtualTerminal), + buffer.data(), + CAN_DATA_LENGTH, + myControlFunction, + partnerControlFunction, + CANIdentifier::PriorityLowest7); + } + + bool VirtualTerminalClient::send_store_version(std::array versionLabel) const + { + const std::array buffer = { static_cast(Function::StoreVersionCommand), + versionLabel[0], + versionLabel[1], + versionLabel[2], + versionLabel[3], + versionLabel[4], + versionLabel[5], + versionLabel[6] }; + return CANNetworkManager::CANNetwork.send_can_message(static_cast(CANLibParameterGroupNumber::ECUtoVirtualTerminal), + buffer.data(), + CAN_DATA_LENGTH, + myControlFunction, + partnerControlFunction, + CANIdentifier::PriorityLowest7); + } + + bool VirtualTerminalClient::send_load_version(std::array versionLabel) const + { + const std::array buffer = { static_cast(Function::LoadVersionCommand), + versionLabel[0], + versionLabel[1], + versionLabel[2], + versionLabel[3], + versionLabel[4], + versionLabel[5], + versionLabel[6] }; + return CANNetworkManager::CANNetwork.send_can_message(static_cast(CANLibParameterGroupNumber::ECUtoVirtualTerminal), + buffer.data(), + CAN_DATA_LENGTH, + myControlFunction, + partnerControlFunction, + CANIdentifier::PriorityLowest7); + } + + bool VirtualTerminalClient::send_delete_version(std::array versionLabel) const + { + const std::array buffer = { static_cast(Function::DeleteVersionCommand), + versionLabel[0], + versionLabel[1], + versionLabel[2], + versionLabel[3], + versionLabel[4], + versionLabel[5], + versionLabel[6] }; + return CANNetworkManager::CANNetwork.send_can_message(static_cast(CANLibParameterGroupNumber::ECUtoVirtualTerminal), + buffer.data(), + CAN_DATA_LENGTH, + myControlFunction, + partnerControlFunction, + CANIdentifier::PriorityLowest7); + } + + bool VirtualTerminalClient::send_extended_get_versions() const + { + constexpr std::array buffer = { static_cast(Function::ExtendedDeleteVersionCommand), + 0xFF, + 0xFF, + 0xFF, + 0xFF, + 0xFF, + 0xFF, + 0xFF }; + return CANNetworkManager::CANNetwork.send_can_message(static_cast(CANLibParameterGroupNumber::ECUtoVirtualTerminal), + buffer.data(), + CAN_DATA_LENGTH, + myControlFunction, + partnerControlFunction, + CANIdentifier::PriorityLowest7); + } + + bool VirtualTerminalClient::send_extended_store_version(std::array versionLabel) const + { + std::array buffer; + buffer[0] = static_cast(Function::ExtendedStoreVersionCommand); + memcpy(&buffer[1], versionLabel.data(), 32); + return CANNetworkManager::CANNetwork.send_can_message(static_cast(CANLibParameterGroupNumber::ECUtoVirtualTerminal), + buffer.data(), + buffer.size(), + myControlFunction, + partnerControlFunction, + CANIdentifier::PriorityLowest7); + } + + bool VirtualTerminalClient::send_extended_load_version(std::array versionLabel) const + { + std::array buffer; + buffer[0] = static_cast(Function::ExtendedLoadVersionCommand); + memcpy(&buffer[1], versionLabel.data(), 32); + return CANNetworkManager::CANNetwork.send_can_message(static_cast(CANLibParameterGroupNumber::ECUtoVirtualTerminal), + buffer.data(), + buffer.size(), + myControlFunction, + partnerControlFunction, + CANIdentifier::PriorityLowest7); + } + + bool VirtualTerminalClient::send_extended_delete_version(std::array versionLabel) const + { + std::array buffer; + buffer[0] = static_cast(Function::ExtendedDeleteVersionCommand); + memcpy(&buffer[1], versionLabel.data(), 32); + return CANNetworkManager::CANNetwork.send_can_message(static_cast(CANLibParameterGroupNumber::ECUtoVirtualTerminal), + buffer.data(), + buffer.size(), + myControlFunction, + partnerControlFunction, + CANIdentifier::PriorityLowest7); + } + + bool VirtualTerminalClient::send_end_of_object_pool() const + { + constexpr std::array buffer = { static_cast(Function::EndOfObjectPoolMessage), + 0xFF, + 0xFF, + 0xFF, + 0xFF, + 0xFF, + 0xFF, + 0xFF }; + return CANNetworkManager::CANNetwork.send_can_message(static_cast(CANLibParameterGroupNumber::ECUtoVirtualTerminal), + buffer.data(), + CAN_DATA_LENGTH, + myControlFunction, + partnerControlFunction, + CANIdentifier::PriorityLowest7); + } + + bool VirtualTerminalClient::send_working_set_master() const + { + constexpr std::array buffer = { 0x01, + 0xFF, + 0xFF, + 0xFF, + 0xFF, + 0xFF, + 0xFF, + 0xFF }; + return CANNetworkManager::CANNetwork.send_can_message(static_cast(CANLibParameterGroupNumber::WorkingSetMaster), + buffer.data(), + CAN_DATA_LENGTH, + myControlFunction, + nullptr, + CANIdentifier::PriorityLowest7); + } + + bool VirtualTerminalClient::send_auxiliary_functions_preferred_assignment() const + { + //! @todo load preferred assignment from saved configuration + std::vector buffer = { static_cast(Function::PreferredAssignmentCommand), 0 }; + if (buffer.size() < CAN_DATA_LENGTH) + { + buffer.resize(CAN_DATA_LENGTH); + } + return CANNetworkManager::CANNetwork.send_can_message(static_cast(CANLibParameterGroupNumber::ECUtoVirtualTerminal), + buffer.data(), + buffer.size(), + myControlFunction, + partnerControlFunction, + CANIdentifier::PriorityLowest7); + } + + bool VirtualTerminalClient::send_auxiliary_function_assignment_response(std::uint16_t functionObjectID, bool hasError, bool isAlreadyAssigned) const + { + std::uint8_t errorCode = 0; + if (hasError) + { + errorCode |= 0x01; + } + if ((isAlreadyAssigned) && (false == get_vt_version_supported(VTVersion::Version6))) + { + errorCode |= 0x02; + } + const std::array buffer = { static_cast(Function::AuxiliaryAssignmentTypeTwoCommand), + static_cast(functionObjectID), + static_cast(functionObjectID >> 8), + errorCode, + 0xFF, + 0xFF, + 0xFF, + 0xFF }; + return CANNetworkManager::CANNetwork.send_can_message(static_cast(CANLibParameterGroupNumber::ECUtoVirtualTerminal), + buffer.data(), + CAN_DATA_LENGTH, + myControlFunction, + partnerControlFunction, + CANIdentifier::PriorityLowest7); + } + + bool VirtualTerminalClient::send_auxiliary_input_maintenance() const + { + const std::array buffer = { static_cast(Function::AuxiliaryInputTypeTwoMaintenanceMessage), + static_cast(ourModelIdentificationCode), + static_cast(ourModelIdentificationCode >> 8), + static_cast(StateMachineState::Connected == state ? 0x01 : 0x00), + 0xFF, + 0xFF, + 0xFF, + 0xFF }; + return CANNetworkManager::CANNetwork.send_can_message(static_cast(CANLibParameterGroupNumber::ECUtoVirtualTerminal), + buffer.data(), + CAN_DATA_LENGTH, + myControlFunction, + nullptr, + CANIdentifier::Priority3); + } + + bool VirtualTerminalClient::send_auxiliary_input_status_enable_response(std::uint16_t objectID, bool isEnabled, bool invalidObjectID) const + { + const std::array buffer = { static_cast(Function::AuxiliaryInputStatusTypeTwoEnableCommand), + static_cast(objectID), + static_cast(objectID >> 8), + static_cast(isEnabled ? 0x01 : 0x00), + static_cast(invalidObjectID ? 0x01 : 0x00), + 0xFF, + 0xFF, + 0xFF }; + return CANNetworkManager::CANNetwork.send_can_message(static_cast(CANLibParameterGroupNumber::ECUtoVirtualTerminal), + buffer.data(), + CAN_DATA_LENGTH, + myControlFunction, + partnerControlFunction, + CANIdentifier::PriorityLowest7); + } + + void VirtualTerminalClient::update_auxiliary_input_status() + { + for (auto &auxiliaryInput : ourAuxiliaryInputs) + { + update_auxiliary_input_status(auxiliaryInput.first); + } + } + + bool VirtualTerminalClient::update_auxiliary_input_status(std::uint16_t objectID) + { + bool retVal = false; + AuxiliaryInputState &state = ourAuxiliaryInputs.at(objectID); + /// @todo Change status message every 50ms to every 200ms for non-latched boolean inputs on interaction + if (SystemTiming::time_expired_ms(state.lastStatusUpdate, AUXILIARY_INPUT_STATUS_DELAY) || + (state.hasInteraction && + !get_auxiliary_input_learn_mode_enabled() && + SystemTiming::time_expired_ms(state.lastStatusUpdate, AUXILIARY_INPUT_STATUS_DELAY_INTERACTION))) + { + state.lastStatusUpdate = SystemTiming::get_timestamp_ms(); + + std::uint8_t operatingState = 0; + if (get_auxiliary_input_learn_mode_enabled()) + { + operatingState |= 0x01; + if (state.hasInteraction) + { + operatingState |= 0x02; + } + } + if (state.controlLocked) + { + operatingState |= 0x04; + if (state.hasInteraction) + { + operatingState |= 0x08; + } + } + state.hasInteraction = false; // reset interaction flag + + /// @todo Change values based on state of auxiliary input, e.g. for non-latched boolean inputs we have to change from value=1 (momentary) to value=2 (held) + const std::array buffer = { static_cast(Function::AuxiliaryInputTypeTwoStatusMessage), + static_cast(objectID), + static_cast(objectID >> 8), + static_cast(state.value1), + static_cast(state.value1 >> 8), + static_cast(state.value2), + static_cast(state.value2 >> 8), + operatingState }; + if (get_auxiliary_input_learn_mode_enabled()) + { + retVal = CANNetworkManager::CANNetwork.send_can_message(static_cast(CANLibParameterGroupNumber::ECUtoVirtualTerminal), + buffer.data(), + CAN_DATA_LENGTH, + myControlFunction, + partnerControlFunction, + CANIdentifier::Priority3); + } + else + { + retVal = CANNetworkManager::CANNetwork.send_can_message(static_cast(CANLibParameterGroupNumber::VirtualTerminalToECU), + buffer.data(), + CAN_DATA_LENGTH, + myControlFunction, + nullptr, + CANIdentifier::Priority3); + } + } + return retVal; + } + + void VirtualTerminalClient::set_state(StateMachineState value) + { + stateMachineTimestamp_ms = SystemTiming::get_timestamp_ms(); + + if (value != state) + { + firstTimeInState = true; + } + + state = value; + + if (StateMachineState::Disconnected == value) + { + lastVTStatusTimestamp_ms = 0; + for (std::size_t i = 0; i < objectPools.size(); i++) + { + objectPools[i].uploaded = false; + } + } + } + + void VirtualTerminalClient::process_flags(std::uint32_t flag, void *parent) + { + if ((flag <= static_cast(TransmitFlags::NumberFlags)) && + (nullptr != parent)) + { + TransmitFlags flagToProcess = static_cast(flag); + VirtualTerminalClient *vtClient = static_cast(parent); + bool transmitSuccessful = false; + + switch (flagToProcess) + { + case TransmitFlags::SendWorkingSetMaintenance: + { + if (!vtClient->objectPools.empty()) + { + transmitSuccessful = vtClient->send_working_set_maintenance(false, vtClient->objectPools[0].version); + + if (transmitSuccessful) + { + vtClient->lastWorkingSetMaintenanceTimestamp_ms = SystemTiming::get_timestamp_ms(); + } + } + } + break; + + case TransmitFlags::SendAuxiliaryMaintenance: + { + transmitSuccessful = vtClient->send_auxiliary_input_maintenance(); + + if (transmitSuccessful) + { + vtClient->lastAuxiliaryMaintenanceTimestamp_ms = SystemTiming::get_timestamp_ms(); + } + } + break; + + case TransmitFlags::NumberFlags: + default: + { + } + break; + } + + if (false == transmitSuccessful) + { + vtClient->txFlags.set_flag(flag); + } + } + } + + void VirtualTerminalClient::process_rx_message(const CANMessage &message, void *parentPointer) + { + VirtualTerminalClient *parentVT = static_cast(parentPointer); + if ((nullptr != parentPointer) && + (CAN_DATA_LENGTH <= message.get_data_length()) && + ((nullptr == message.get_destination_control_function()) || + (parentVT->myControlFunction == message.get_destination_control_function()))) + { + switch (message.get_identifier().get_parameter_group_number()) + { + case static_cast(CANLibParameterGroupNumber::Acknowledge): + { + if (AcknowledgementType::Negative == static_cast(message.get_uint8_at(0))) + { + std::uint32_t targetParameterGroupNumber = message.get_uint24_at(5); + if (static_cast(CANLibParameterGroupNumber::ECUtoVirtualTerminal) == targetParameterGroupNumber) + { + CANStackLogger::CAN_stack_log(CANStackLogger::LoggingLevel::Error, "[VT]: The VT Server is NACK-ing our VT messages. Disconnecting."); + parentVT->set_state(StateMachineState::Disconnected); + } + } + } + break; + + case static_cast(CANLibParameterGroupNumber::VirtualTerminalToECU): + { + switch (message.get_uint8_at(0)) + { + case static_cast(Function::SoftKeyActivationMessage): + { + std::uint8_t keyCode = message.get_uint8_at(1); + if (keyCode <= static_cast(KeyActivationCode::ButtonPressAborted)) + { + std::uint16_t objectID = message.get_uint16_at(2); + std::uint16_t parentObjectID = message.get_uint16_at(4); + std::uint8_t keyNumber = message.get_uint8_at(6); + if (parentVT->get_vt_version_supported(VTVersion::Version6)) + { + //! @todo process TAN + } + + parentVT->softKeyEventDispatcher.invoke({ parentVT, objectID, parentObjectID, keyNumber, static_cast(keyCode) }); + } + } + break; + + case static_cast(Function::ButtonActivationMessage): + { + std::uint8_t keyCode = message.get_uint8_at(1); + if (keyCode <= static_cast(KeyActivationCode::ButtonPressAborted)) + { + std::uint16_t objectID = message.get_uint16_at(2); + std::uint16_t parentObjectID = message.get_uint16_at(4); + std::uint8_t keyNumber = message.get_uint8_at(6); + if (parentVT->get_vt_version_supported(VTVersion::Version6)) + { + //! @todo process TAN + } + parentVT->buttonEventDispatcher.invoke({ parentVT, objectID, parentObjectID, keyNumber, static_cast(keyCode) }); + } + } + break; + + case static_cast(Function::PointingEventMessage): + { + std::uint16_t xPosition = message.get_uint16_at(1); + std::uint16_t yPosition = message.get_uint16_at(3); + + std::uint8_t touchState = static_cast(KeyActivationCode::ButtonPressedOrLatched); + std::uint16_t parentMaskObjectID = NULL_OBJECT_ID; + if (parentVT->get_vt_version_supported(VTVersion::Version6)) + { + // VT version is at least 6 + touchState = message.get_uint8_at(5) & 0x0F; + parentMaskObjectID = message.get_uint16_at(6); + //! @todo process TAN + } + else if (parentVT->get_vt_version_supported(VTVersion::Version4)) + { + // VT version is either 4 or 5 + touchState = message.get_uint8_at(5); + } + + if (touchState <= static_cast(KeyActivationCode::ButtonPressAborted)) + { + parentVT->pointingEventDispatcher.invoke({ parentVT, xPosition, yPosition, parentMaskObjectID, static_cast(touchState) }); + } + } + break; + + case static_cast(Function::VTSelectInputObjectMessage): + { + std::uint16_t objectID = message.get_uint16_at(1); + bool objectSelected = (0x01 == message.get_uint8_at(3)); + bool objectOpenForInput = true; + + if (parentVT->get_vt_version_supported(VTVersion::Version4)) + { + objectOpenForInput = message.get_bool_at(4, 0); + } + + if (parentVT->get_vt_version_supported(VTVersion::Version6)) + { + //! @todo process TAN + } + + parentVT->selectInputObjectEventDispatcher.invoke({ parentVT, objectID, objectSelected, objectOpenForInput }); + } + break; + + case static_cast(Function::VTESCMessage): + { + std::uint16_t objectID = message.get_uint16_at(1); + std::uint8_t errorCode = message.get_uint8_at(3) & 0x1F; + if ((errorCode == static_cast(ESCMessageErrorCode::OtherError)) || + (errorCode <= static_cast(ESCMessageErrorCode::NoInputFieldOpen))) + { + if (parentVT->get_vt_version_supported(VTVersion::Version6)) + { + //! @todo process TAN + } + + parentVT->escMessageEventDispatcher.invoke({ parentVT, objectID, static_cast(errorCode) }); + } + } + break; + + case static_cast(Function::VTChangeNumericValueMessage): + { + std::uint16_t objectID = message.get_uint16_at(1); + std::uint32_t value = message.get_uint32_at(4); + + if (parentVT->get_vt_version_supported(VTVersion::Version6)) + { + //! @todo process TAN + } + parentVT->changeNumericValueEventDispatcher.invoke({ parentVT, value, objectID }); + } + break; + + case static_cast(Function::VTChangeActiveMaskMessage): + { + std::uint16_t maskObjectID = message.get_uint16_at(1); + + bool missingObjects = message.get_bool_at(3, 2); + bool maskOrChildHasErrors = message.get_bool_at(3, 3); + bool anyOtherError = message.get_bool_at(3, 4); + bool poolDeleted = message.get_bool_at(3, 5); + + std::uint16_t errorObjectID = message.get_uint16_at(4); + std::uint16_t parentObjectID = message.get_uint16_at(6); + + parentVT->changeActiveMaskEventDispatcher.invoke({ parentVT, + maskObjectID, + errorObjectID, + parentObjectID, + missingObjects, + maskOrChildHasErrors, + anyOtherError, + poolDeleted }); + } + break; + + case static_cast(Function::VTChangeSoftKeyMaskMessage): + { + std::uint16_t dataOrAlarmMaskID = message.get_uint16_at(1); + std::uint16_t softKeyMaskID = message.get_uint16_at(3); + + bool missingObjects = message.get_bool_at(5, 2); + bool maskOrChildHasErrors = message.get_bool_at(5, 3); + bool anyOtherError = message.get_bool_at(5, 4); + bool poolDeleted = message.get_bool_at(5, 5); + + parentVT->changeSoftKeyMaskEventDispatcher.invoke({ parentVT, + dataOrAlarmMaskID, + softKeyMaskID, + missingObjects, + maskOrChildHasErrors, + anyOtherError, + poolDeleted }); + } + break; + + case static_cast(Function::VTChangeStringValueMessage): + { + std::uint16_t objectID = message.get_uint16_at(1); + std::uint8_t stringLength = message.get_uint8_at(3); + std::string value = std::string(message.get_data().begin() + 4, message.get_data().begin() + 4 + stringLength); + + parentVT->changeStringValueEventDispatcher.invoke({ value, parentVT, objectID }); + } + break; + + case static_cast(Function::VTOnUserLayoutHideShowMessage): + { + std::uint16_t objectID = message.get_uint16_at(1); + bool hidden = !message.get_bool_at(3, 0); + + parentVT->userLayoutHideShowEventDispatcher.invoke({ parentVT, objectID, hidden }); + + // There could be two layout messages in one packet + objectID = message.get_uint16_at(4); + if (objectID != NULL_OBJECT_ID) + { + hidden = !message.get_bool_at(6, 0); + parentVT->userLayoutHideShowEventDispatcher.invoke({ parentVT, objectID, hidden }); + } + + if (parentVT->get_vt_version_supported(VTVersion::Version6)) + { + //! @todo process TAN + } + } + break; + + case static_cast(Function::VTControlAudioSignalTerminationMessage): + { + bool terminated = message.get_bool_at(1, 0); + + parentVT->audioSignalTerminationEventDispatcher.invoke({ parentVT, terminated }); + + if (parentVT->get_vt_version_supported(VTVersion::Version6)) + { + //! @todo process TAN + } + } + break; + + case static_cast(Function::PreferredAssignmentCommand): + { + if (message.get_bool_at(1, 0)) + { + CANStackLogger::CAN_stack_log(CANStackLogger::LoggingLevel::Error, "[AUX-N]: Preferred Assignment Error - Auxiliary Input Unit(s) (NAME or Model Identification Code) not valid"); + } + if (message.get_bool_at(1, 1)) + { + CANStackLogger::CAN_stack_log(CANStackLogger::LoggingLevel::Error, "[AUX-N]: Preferred Assignment Error - Function Object ID(S) not valid"); + } + if (message.get_bool_at(1, 2)) + { + CANStackLogger::CAN_stack_log(CANStackLogger::LoggingLevel::Error, "[AUX-N]: Preferred Assignment Error - Input Object ID(s) not valid"); + } + if (message.get_bool_at(1, 3)) + { + CANStackLogger::CAN_stack_log(CANStackLogger::LoggingLevel::Error, "[AUX-N]: Preferred Assignment Error - Duplicate Object ID of Auxiliary Function"); + } + if (message.get_bool_at(1, 4)) + { + CANStackLogger::CAN_stack_log(CANStackLogger::LoggingLevel::Error, "[AUX-N]: Preferred Assignment Error - Other"); + } + + if (0 != message.get_uint8_at(1)) + { + std::uint16_t faultyObjectID = message.get_uint16_at(2); + CANStackLogger::CAN_stack_log(CANStackLogger::LoggingLevel::Error, "[AUX-N]: Auxiliary Function Object ID of faulty assignment: " + isobus::to_string(faultyObjectID)); + } + else + { + CANStackLogger::CAN_stack_log(CANStackLogger::LoggingLevel::Debug, "[AUX-N]: Preferred Assignment OK"); + //! @todo load the preferred assignment into parentVT->assignedAuxiliaryInputDevices + } + } + break; + + case static_cast(Function::AuxiliaryAssignmentTypeTwoCommand): + { + if (14 == message.get_data_length()) + { + std::uint64_t isoName = message.get_uint64_at(1); + bool storeAsPreferred = message.get_bool_at(9, 7); + std::uint8_t functionType = (message.get_uint8_at(9) & 0x1F); + std::uint16_t inputObjectID = message.get_uint16_at(10); + std::uint16_t functionObjectID = message.get_uint16_at(12); + + bool hasError = false; + bool isAlreadyAssigned = false; + if (DEFAULT_NAME == isoName && 0x1F == functionType) + { + if (NULL_OBJECT_ID == functionObjectID) + { + for (AssignedAuxiliaryInputDevice &aux : parentVT->assignedAuxiliaryInputDevices) + { + aux.functions.clear(); + } + CANStackLogger::CAN_stack_log(CANStackLogger::LoggingLevel::Info, "[AUX-N] Unassigned all functions"); + } + else if (NULL_OBJECT_ID == inputObjectID) + { + for (AssignedAuxiliaryInputDevice &aux : parentVT->assignedAuxiliaryInputDevices) + { + for (auto iter = aux.functions.begin(); iter != aux.functions.end();) + { + if (iter->functionObjectID == functionObjectID) + { + aux.functions.erase(iter); + if (storeAsPreferred) + { + //! @todo save preferred assignment to persistent configuration + } + CANStackLogger::CAN_stack_log(CANStackLogger::LoggingLevel::Info, "[AUX-N] Unassigned function " + isobus::to_string(static_cast(functionObjectID)) + " from input " + isobus::to_string(static_cast(inputObjectID))); + } + else + { + ++iter; + } + } + } + } + } + else + { + auto result = std::find_if(parentVT->assignedAuxiliaryInputDevices.begin(), parentVT->assignedAuxiliaryInputDevices.end(), [&isoName](const AssignedAuxiliaryInputDevice &aux) { + return aux.name == isoName; + }); + if (result != std::end(parentVT->assignedAuxiliaryInputDevices)) + { + if (static_cast(AuxiliaryTypeTwoFunctionType::QuadratureBooleanMomentary) >= functionType) + { + AssignedAuxiliaryFunction assignment(functionObjectID, inputObjectID, static_cast(functionType)); + auto location = std::find(result->functions.begin(), result->functions.end(), assignment); + if (location == std::end(result->functions)) + { + result->functions.push_back(assignment); + if (storeAsPreferred) + { + //! @todo save preferred assignment to persistent configuration + } + CANStackLogger::CAN_stack_log(CANStackLogger::LoggingLevel::Info, "[AUX-N]: Assigned function " + isobus::to_string(static_cast(functionObjectID)) + " to input " + isobus::to_string(static_cast(inputObjectID))); + } + else + { + hasError = true; + isAlreadyAssigned = true; + CANStackLogger::CAN_stack_log(CANStackLogger::LoggingLevel::Warning, "[AUX-N]: Unable to store preferred assignment due to missing auxiliary input device with name: " + isobus::to_string(isoName)); + } + } + else + { + hasError = true; + CANStackLogger::CAN_stack_log(CANStackLogger::LoggingLevel::Warning, "[AUX-N]: Unable to store preferred assignment due to unsupported function type: " + isobus::to_string(functionType)); + } + } + else + { + hasError = true; + CANStackLogger::CAN_stack_log(CANStackLogger::LoggingLevel::Warning, "[AUX-N]: Unable to store preferred assignment due to missing auxiliary input device with name: " + isobus::to_string(isoName)); + } + } + parentVT->send_auxiliary_function_assignment_response(functionObjectID, hasError, isAlreadyAssigned); + } + else + { + CANStackLogger::CAN_stack_log(CANStackLogger::LoggingLevel::Warning, "[AUX-N]: Received AuxiliaryAssignmentTypeTwoCommand with wrong data length: " + isobus::to_string(message.get_data_length()) + " but expected 14."); + } + } + break; + + case static_cast(Function::AuxiliaryInputTypeTwoStatusMessage): + { + std::uint16_t inputObjectID = message.get_uint16_at(1); + std::uint16_t value1 = message.get_uint16_at(3); + std::uint16_t value2 = message.get_uint16_at(5); + /// @todo figure out how to best pass other status properties below to application + /// @todo The standard requires us to not perform any auxiliary function when learn mode is active, so we probably want to let the application know about that somehow + // bool learnModeActive = message.get_bool_at(7, 0); + // bool inputActive = message.get_bool_at(7, 1); // Only in learn mode? + // bool controlIsLocked = false; + // bool interactionWhileLocked = false; + if (parentVT->get_vt_version_supported(VTVersion::Version6)) + { + // controlIsLocked = message.get_bool_at(7, 2); + // interactionWhileLocked = message.get_bool_at(7, 3); + } + for (AssignedAuxiliaryInputDevice &aux : parentVT->assignedAuxiliaryInputDevices) + { + auto result = std::find_if(aux.functions.begin(), aux.functions.end(), [&inputObjectID](const AssignedAuxiliaryFunction &assignment) { + return assignment.inputObjectID == inputObjectID; + }); + if (aux.functions.end() != result) + { + parentVT->auxiliaryFunctionEventDispatcher.invoke({ *result, parentVT, value1, value2 }); + } + } + } + break; + + case static_cast(Function::AuxiliaryInputStatusTypeTwoEnableCommand): + { + std::uint16_t inputObjectID = message.get_uint16_at(1); + bool shouldEnable = message.get_bool_at(3, 0); + auto result = std::find_if(parentVT->ourAuxiliaryInputs.begin(), parentVT->ourAuxiliaryInputs.end(), [&inputObjectID](const std::pair &input) { + return input.first == inputObjectID; + }); + bool isInvalidObjectID = (result == std::end(parentVT->ourAuxiliaryInputs)); + if (!isInvalidObjectID) + { + result->second.enabled = shouldEnable; + } + parentVT->send_auxiliary_input_status_enable_response(inputObjectID, isInvalidObjectID ? false : shouldEnable, isInvalidObjectID); + } + break; + + case static_cast(Function::VTStatusMessage): + { + parentVT->lastVTStatusTimestamp_ms = SystemTiming::get_timestamp_ms(); + parentVT->activeWorkingSetMasterAddress = message.get_uint8_at(1); + parentVT->activeWorkingSetDataMaskObjectID = message.get_uint16_at(2); + parentVT->activeWorkingSetSoftKeyMaskObjectID = message.get_uint16_at(4); + parentVT->busyCodesBitfield = message.get_uint8_at(6); + parentVT->currentCommandFunctionCode = message.get_uint8_at(7); + } + break; + + case static_cast(Function::GetMemoryMessage): + { + if (StateMachineState::WaitForGetMemoryResponse == parentVT->state) + { + parentVT->connectedVTVersion = message.get_uint8_at(1); + + if (0 == message.get_uint8_at(2)) + { + // There IS enough memory + parentVT->set_state(StateMachineState::SendGetNumberSoftkeys); + } + else + { + parentVT->set_state(StateMachineState::Failed); + CANStackLogger::CAN_stack_log(CANStackLogger::LoggingLevel::Error, "[VT]: Connection Failed Not Enough Memory"); + } + } + } + break; + + case static_cast(Function::GetNumberOfSoftKeysMessage): + { + if (StateMachineState::WaitForGetNumberSoftKeysResponse == parentVT->state) + { + parentVT->softKeyXAxisPixels = message.get_uint8_at(4); + parentVT->softKeyYAxisPixels = message.get_uint8_at(5); + parentVT->numberVirtualSoftkeysPerSoftkeyMask = message.get_uint8_at(6); + parentVT->numberPhysicalSoftkeys = message.get_uint8_at(7); + parentVT->set_state(StateMachineState::SendGetTextFontData); + } + } + break; + + case static_cast(Function::GetTextFontDataMessage): + { + if (StateMachineState::WaitForGetTextFontDataResponse == parentVT->state) + { + parentVT->smallFontSizesBitfield = message.get_uint8_at(5); + parentVT->largeFontSizesBitfield = message.get_uint8_at(6); + parentVT->fontStylesBitfield = message.get_uint8_at(7); + parentVT->set_state(StateMachineState::SendGetHardware); + } + } + break; + + case static_cast(Function::GetHardwareMessage): + { + if (StateMachineState::WaitForGetHardwareResponse == parentVT->state) + { + if (message.get_uint8_at(2) <= static_cast(GraphicMode::TwoHundredFiftySixColour)) + { + parentVT->supportedGraphicsMode = static_cast(message.get_uint8_at(2)); + } + parentVT->hardwareFeaturesBitfield = message.get_uint8_at(3); + parentVT->xPixels = message.get_uint16_at(4); + parentVT->yPixels = message.get_uint16_at(6); + parentVT->lastObjectPoolIndex = 0; + + // Check if we need to ask for pool versions + // Ony check the first pool, all pools are labeled the same per working set. + if ((!parentVT->objectPools.empty()) && + (!parentVT->objectPools[0].versionLabel.empty())) + { + parentVT->set_state(StateMachineState::SendGetVersions); + } + else + { + parentVT->set_state(StateMachineState::UploadObjectPool); + } + } + } + break; + + case static_cast(Function::GetVersionsResponse): + { + if (StateMachineState::WaitForGetVersionsResponse == parentVT->state) + { + // See if the server returned any labels + const std::uint8_t numberOfLabels = message.get_uint8_at(1); + constexpr std::size_t LABEL_LENGTH = 7; + + if (numberOfLabels > 0) + { + // Check for label match + bool labelMatched = false; + const std::size_t remainingLength = (2 + (LABEL_LENGTH * numberOfLabels)); + + if (message.get_data_length() >= remainingLength) + { + for (std::uint_fast8_t i = 0; i < numberOfLabels; i++) + { + char tempStringLabel[8] = { 0 }; + tempStringLabel[0] = message.get_uint8_at(2 + (LABEL_LENGTH * i)); + tempStringLabel[1] = message.get_uint8_at(3 + (LABEL_LENGTH * i)); + tempStringLabel[2] = message.get_uint8_at(4 + (LABEL_LENGTH * i)); + tempStringLabel[3] = message.get_uint8_at(5 + (LABEL_LENGTH * i)); + tempStringLabel[4] = message.get_uint8_at(6 + (LABEL_LENGTH * i)); + tempStringLabel[5] = message.get_uint8_at(7 + (LABEL_LENGTH * i)); + tempStringLabel[6] = message.get_uint8_at(8 + (LABEL_LENGTH * i)); + tempStringLabel[7] = '\0'; + std::string labelDecoded(tempStringLabel); + std::string tempActualLabel(parentVT->objectPools[0].versionLabel); + + // Check if we need to manipulate the passed in label by padding with spaces + while (tempActualLabel.size() < LABEL_LENGTH) + { + tempActualLabel.push_back(' '); + } + + if (tempActualLabel.size() > LABEL_LENGTH) + { + tempActualLabel.resize(LABEL_LENGTH); + } + + if (tempActualLabel == labelDecoded) + { + labelMatched = true; + parentVT->set_state(StateMachineState::SendLoadVersion); + CANStackLogger::CAN_stack_log(CANStackLogger::LoggingLevel::Info, "[VT]: VT Server has a matching label for " + isobus::to_string(labelDecoded) + ". It will be loaded and upload will be skipped."); + break; + } + else + { + CANStackLogger::CAN_stack_log(CANStackLogger::LoggingLevel::Info, "[VT]: VT Server has a label for " + isobus::to_string(labelDecoded) + ". This version will be deleted."); + const std::array deleteBuffer = { + static_cast(labelDecoded[0]), + static_cast(labelDecoded[1]), + static_cast(labelDecoded[2]), + static_cast(labelDecoded[3]), + static_cast(labelDecoded[4]), + static_cast(labelDecoded[5]), + static_cast(labelDecoded[6]) + }; + if (!parentVT->send_delete_version(deleteBuffer)) + { + CANStackLogger::CAN_stack_log(CANStackLogger::LoggingLevel::Warning, "[VT]: Failed to send the delete version message for label " + isobus::to_string(labelDecoded)); + } + } + } + if (!labelMatched) + { + CANStackLogger::CAN_stack_log(CANStackLogger::LoggingLevel::Info, "[VT]: No version label from the VT matched. Client will upload the pool and store it instead."); + parentVT->set_state(StateMachineState::UploadObjectPool); + } + } + else + { + CANStackLogger::CAN_stack_log(CANStackLogger::LoggingLevel::Warning, "[VT]: Get Versions Response length is not long enough. Message ignored."); + } + } + else + { + CANStackLogger::CAN_stack_log(CANStackLogger::LoggingLevel::Info, "[VT]: No version label from the VT matched. Client will upload the pool and store it instead."); + parentVT->set_state(StateMachineState::UploadObjectPool); + } + } + else + { + CANStackLogger::CAN_stack_log(CANStackLogger::LoggingLevel::Warning, "[VT]: Get Versions Response ignored!"); + } + } + break; + + case static_cast(Function::LoadVersionCommand): + { + if (StateMachineState::WaitForLoadVersionResponse == parentVT->state) + { + if (0 == message.get_uint8_at(5)) + { + CANStackLogger::CAN_stack_log(CANStackLogger::LoggingLevel::Info, "[VT]: Loaded object pool version from VT non-volatile memory with no errors."); + parentVT->set_state(StateMachineState::Connected); + + //! @todo maybe a better way available than relying on aux function callbacks registered? + if (parentVT->auxiliaryFunctionEventDispatcher.get_listener_count() > 0) + { + if (parentVT->send_auxiliary_functions_preferred_assignment()) + { + CANStackLogger::CAN_stack_log(CANStackLogger::LoggingLevel::Debug, "[AUX-N]: Sent preferred assignments after LoadVersionCommand."); + } + else + { + CANStackLogger::CAN_stack_log(CANStackLogger::LoggingLevel::Warning, "[AUX-N]: Failed to send preferred assignments after LoadVersionCommand."); + } + } + } + else + { + // At least one error is set + if (message.get_bool_at(5, 0)) + { + CANStackLogger::CAN_stack_log(CANStackLogger::LoggingLevel::Warning, "[VT]: Load Versions Response error: File system error or corruption."); + } + if (message.get_bool_at(5, 1)) + { + CANStackLogger::CAN_stack_log(CANStackLogger::LoggingLevel::Warning, "[VT]: Load Versions Response error: Insufficient memory."); + } + if (message.get_bool_at(5, 2)) + { + CANStackLogger::CAN_stack_log(CANStackLogger::LoggingLevel::Warning, "[VT]: Load Versions Response error: Any other error."); + } + + // Not sure what happened here... should be mostly impossible. Try to upload instead. + CANStackLogger::CAN_stack_log(CANStackLogger::LoggingLevel::Warning, "[VT]: Switching to pool upload instead."); + parentVT->set_state(StateMachineState::UploadObjectPool); + } + } + else + { + CANStackLogger::CAN_stack_log(CANStackLogger::LoggingLevel::Warning, "[VT]: Load Versions Response ignored!"); + } + } + break; + + case static_cast(Function::StoreVersionCommand): + { + if (StateMachineState::WaitForStoreVersionResponse == parentVT->state) + { + if (0 == message.get_uint8_at(5)) + { + // Stored with no error + parentVT->set_state(StateMachineState::Connected); + CANStackLogger::CAN_stack_log(CANStackLogger::LoggingLevel::Info, "[VT]: Stored object pool with no error."); + } + else + { + // At least one error is set + if (message.get_bool_at(5, 0)) + { + CANStackLogger::CAN_stack_log(CANStackLogger::LoggingLevel::Warning, "[VT]: Store Versions Response error: Version label is not correct."); + } + if (message.get_bool_at(5, 1)) + { + CANStackLogger::CAN_stack_log(CANStackLogger::LoggingLevel::Warning, "[VT]: Store Versions Response error: Insufficient memory."); + } + if (message.get_bool_at(5, 2)) + { + CANStackLogger::CAN_stack_log(CANStackLogger::LoggingLevel::Warning, "[VT]: Store Versions Response error: Any other error."); + } + } + } + else + { + CANStackLogger::CAN_stack_log(CANStackLogger::LoggingLevel::Warning, "[VT]: Store Versions Response ignored!"); + } + } + break; + + case static_cast(Function::DeleteVersionCommand): + { + if (0 == message.get_uint8_at(5)) + { + CANStackLogger::CAN_stack_log(CANStackLogger::LoggingLevel::Info, "[VT]: Delete Version Response OK!"); + } + else + { + if (message.get_bool_at(5, 1)) + { + CANStackLogger::CAN_stack_log(CANStackLogger::LoggingLevel::Warning, "[VT]: Delete Version Response error: Version label is not correct, or unknown."); + } + if (message.get_bool_at(5, 3)) + { + CANStackLogger::CAN_stack_log(CANStackLogger::LoggingLevel::Warning, "[VT]: Delete Version Response error: Any other error."); + } + } + } + break; + + case static_cast(Function::EndOfObjectPoolMessage): + { + if (StateMachineState::WaitForEndOfObjectPoolResponse == parentVT->state) + { + bool anyErrorInPool = message.get_bool_at(1, 0); + bool vtRanOutOfMemory = message.get_bool_at(1, 1); + bool otherErrors = message.get_bool_at(1, 3); + std::uint16_t parentObjectIDOfFaultyObject = message.get_uint16_at(2); + std::uint16_t objectIDOfFaultyObject = message.get_uint16_at(4); + std::uint8_t objectPoolErrorBitmask = message.get_uint8_at(6); + + if ((!anyErrorInPool) && + (0 == objectPoolErrorBitmask)) + { + // Clear scaling buffers + for (auto &objectPool : parentVT->objectPools) + { + objectPool.scaledObjectPool.clear(); + } + + // Check if we need to store this pool + if (!parentVT->objectPools[0].versionLabel.empty()) + { + parentVT->set_state(StateMachineState::SendStoreVersion); + } + else + { + parentVT->set_state(StateMachineState::Connected); + } + //! @todo maybe a better way available than relying on aux function callbacks registered? + if (parentVT->auxiliaryFunctionEventDispatcher.get_listener_count() > 0) + { + if (parentVT->send_auxiliary_functions_preferred_assignment()) + { + CANStackLogger::CAN_stack_log(CANStackLogger::LoggingLevel::Debug, "[AUX-N]: Sent preferred assignments after EndOfObjectPoolMessage."); + } + else + { + CANStackLogger::CAN_stack_log(CANStackLogger::LoggingLevel::Warning, "[AUX-N]: Failed to send preferred assignments after EndOfObjectPoolMessage."); + } + } + } + else + { + parentVT->set_state(StateMachineState::Failed); + CANStackLogger::CAN_stack_log(CANStackLogger::LoggingLevel::Error, "[VT]: Error in end of object pool message." + std::string("Faulty Object ") + isobus::to_string(static_cast(objectIDOfFaultyObject)) + std::string(" Faulty Object Parent ") + isobus::to_string(static_cast(parentObjectIDOfFaultyObject)) + std::string(" Pool error bitmask value ") + isobus::to_string(static_cast(objectPoolErrorBitmask))); + if (vtRanOutOfMemory) + { + CANStackLogger::CAN_stack_log(CANStackLogger::LoggingLevel::Error, "[VT]: Ran out of memory"); + } + if (otherErrors) + { + CANStackLogger::CAN_stack_log(CANStackLogger::LoggingLevel::Error, "[VT]: Reported other errors in EOM response"); + } + } + } + } + break; + } + } + break; + + case static_cast(CANLibParameterGroupNumber::ECUtoVirtualTerminal): + { + switch (message.get_uint8_at(0)) + { + case static_cast(Function::AuxiliaryInputTypeTwoMaintenanceMessage): + { + std::uint16_t modelIdentificationCode = message.get_uint16_at(1); + bool ready = message.get_uint8_at(3); + + if (ready) + { + auto result = std::find_if(parentVT->assignedAuxiliaryInputDevices.begin(), parentVT->assignedAuxiliaryInputDevices.end(), [&modelIdentificationCode](const AssignedAuxiliaryInputDevice &aux) { + return aux.modelIdentificationCode == modelIdentificationCode; + }); + if (result == std::end(parentVT->assignedAuxiliaryInputDevices)) + { + AssignedAuxiliaryInputDevice inputDevice{ message.get_source_control_function()->get_NAME().get_full_name(), modelIdentificationCode, {} }; + parentVT->assignedAuxiliaryInputDevices.push_back(inputDevice); + CANStackLogger::CAN_stack_log(CANStackLogger::LoggingLevel::Info, "[AUX-N]: New auxiliary input device with name: " + isobus::to_string(inputDevice.name) + " and model identification code: " + isobus::to_string(modelIdentificationCode)); + } + } + } + break; + } + } + break; + + default: + { + CANStackLogger::CAN_stack_log(CANStackLogger::LoggingLevel::Warning, "[VT]: Client unknown message: " + isobus::to_string(static_cast(message.get_identifier().get_parameter_group_number()))); + } + break; + } + } + else + { + CANStackLogger::CAN_stack_log(CANStackLogger::LoggingLevel::Warning, "[VT]: VT-ECU Client message invalid"); + } + } + + void VirtualTerminalClient::process_callback(std::uint32_t parameterGroupNumber, + std::uint32_t, + std::shared_ptr, + std::shared_ptr destinationControlFunction, + bool successful, + void *parentPointer) + { + if ((nullptr != parentPointer) && + (static_cast(CANLibParameterGroupNumber::ECUtoVirtualTerminal) == parameterGroupNumber) && + (nullptr != destinationControlFunction)) + { + VirtualTerminalClient *parent = static_cast(parentPointer); + + if (StateMachineState::UploadObjectPool == parent->state) + { + if (successful) + { + parent->currentObjectPoolState = CurrentObjectPoolUploadState::Success; + } + else + { + parent->currentObjectPoolState = CurrentObjectPoolUploadState::Failed; + } + } + } + } + + bool VirtualTerminalClient::process_internal_object_pool_upload_callback(std::uint32_t callbackIndex, + std::uint32_t bytesOffset, + std::uint32_t numberOfBytesNeeded, + std::uint8_t *chunkBuffer, + void *parentPointer) + { + bool retVal = false; + + if ((nullptr != parentPointer) && + (nullptr != chunkBuffer) && + (0 != numberOfBytesNeeded)) + { + VirtualTerminalClient *parentVTClient = static_cast(parentPointer); + std::uint32_t poolIndex = std::numeric_limits::max(); + bool usingExternalCallback = false; + + // Need to figure out which pool we're currently uploading + for (std::uint32_t i = 0; i < parentVTClient->objectPools.size(); i++) + { + if (!parentVTClient->objectPools[i].uploaded) + { + poolIndex = i; + usingExternalCallback = parentVTClient->objectPools[i].useDataCallback; + break; + } + } + + // If pool index is FFs, something is wrong with the state machine state, return false. + if ((std::numeric_limits::max() != poolIndex) && + (bytesOffset + numberOfBytesNeeded) <= parentVTClient->objectPools[poolIndex].objectPoolSize + 1) + { + // We've got more data to transfer + if ((0 != parentVTClient->objectPools[poolIndex].autoScaleDataMaskOriginalDimension) && (0 != parentVTClient->objectPools[poolIndex].autoScaleSoftKeyDesignatorOriginalHeight)) + { + // Object pool has been pre-scaled. Use the scaling buffer instead + retVal = true; + if (0 == bytesOffset) + { + chunkBuffer[0] = static_cast(Function::ObjectPoolTransferMessage); + memcpy(&chunkBuffer[1], &parentVTClient->objectPools[poolIndex].scaledObjectPool[bytesOffset], numberOfBytesNeeded - 1); + } + else + { + // Subtract off 1 to account for the mux in the first byte of the message + memcpy(chunkBuffer, &parentVTClient->objectPools[poolIndex].scaledObjectPool[bytesOffset - 1], numberOfBytesNeeded); + } + } + else + { + if (usingExternalCallback) + { + // We're using the user's supplied callback to get a chunk of info + if (0 == bytesOffset) + { + chunkBuffer[0] = static_cast(Function::ObjectPoolTransferMessage); + retVal = parentVTClient->objectPools[poolIndex].dataCallback(callbackIndex, bytesOffset, numberOfBytesNeeded - 1, &chunkBuffer[1], parentVTClient); + } + else + { + // Subtract off 1 to account for the mux in the first byte of the message + retVal = parentVTClient->objectPools[poolIndex].dataCallback(callbackIndex, bytesOffset - 1, numberOfBytesNeeded, chunkBuffer, parentVTClient); + } + } + else + { + // We already have the whole pool in RAM + retVal = true; + if (0 == bytesOffset) + { + chunkBuffer[0] = static_cast(Function::ObjectPoolTransferMessage); + memcpy(&chunkBuffer[1], &parentVTClient->objectPools[poolIndex].objectPoolDataPointer[bytesOffset], numberOfBytesNeeded - 1); + } + else + { + // Subtract off 1 to account for the mux in the first byte of the message + memcpy(chunkBuffer, &parentVTClient->objectPools[poolIndex].objectPoolDataPointer[bytesOffset - 1], numberOfBytesNeeded); + } + } + } + } + } + return retVal; + } + + bool VirtualTerminalClient::get_any_pool_needs_scaling() const + { + bool retVal = false; + + for (auto &objectPool : objectPools) + { + if ((0 != objectPool.autoScaleDataMaskOriginalDimension) && + (0 != objectPool.autoScaleSoftKeyDesignatorOriginalHeight)) + { + retVal = true; + break; + } + } + return retVal; + } + + bool VirtualTerminalClient::scale_object_pools() + { + bool retVal = true; + + for (auto &objectPool : objectPools) + { + // Step 1: Make a read/write copy of the pool + if (nullptr != objectPool.objectPoolDataPointer) + { + objectPool.scaledObjectPool.resize(objectPool.objectPoolSize); + memcpy(&objectPool.scaledObjectPool[0], objectPool.objectPoolDataPointer, objectPool.objectPoolSize); + } + else if (nullptr != objectPool.objectPoolVectorPointer) + { + objectPool.scaledObjectPool.resize(objectPool.objectPoolVectorPointer->size()); + std::copy(objectPool.objectPoolVectorPointer->begin(), objectPool.objectPoolVectorPointer->end(), objectPool.scaledObjectPool.begin()); + } + else if (objectPool.useDataCallback) + { + objectPool.scaledObjectPool.resize(objectPool.objectPoolSize); + + for (std::uint32_t i = 0; i < objectPool.objectPoolSize; i++) + { + retVal &= objectPool.dataCallback(i, i, 1, &objectPool.scaledObjectPool[i], this); + } + + if (!retVal) + { + break; + } + } + + // Step 2, Parse the pool and resize each object as we iterate through it + auto poolIterator = objectPool.scaledObjectPool.begin(); + + while ((poolIterator != objectPool.scaledObjectPool.end()) && + retVal) + { + if (VirtualTerminalObjectType::Key == static_cast(poolIterator[2])) + { + retVal &= resize_object(&poolIterator[0], + static_cast(get_softkey_x_axis_pixels()) / static_cast(objectPool.autoScaleSoftKeyDesignatorOriginalHeight), + static_cast(poolIterator[2])); + } + else + { + retVal &= resize_object(&poolIterator[0], + static_cast(get_number_x_pixels()) / static_cast(objectPool.autoScaleDataMaskOriginalDimension), + static_cast(poolIterator[2])); + } + + std::uint32_t objectSize = get_number_bytes_in_object(&poolIterator[0]); + if (retVal) + { + if (get_is_object_scalable(static_cast(*(poolIterator + 2)))) + { + CANStackLogger::debug("[VT]: Resized an object: " + + isobus::to_string(static_cast((*poolIterator)) | (static_cast((*poolIterator + 1))) << 8) + + " with type " + + isobus::to_string(static_cast((*(poolIterator + 2)))) + + " with size " + + isobus::to_string(static_cast(objectSize))); + } + } + else + { + CANStackLogger::error("[VT]: Failed to resize an object: " + + isobus::to_string(static_cast((*poolIterator)) | (static_cast((*poolIterator + 1))) << 8) + + " with type " + + isobus::to_string(static_cast((*poolIterator + 2))) + + " with size " + + isobus::to_string(static_cast(objectSize))); + } + poolIterator += objectSize; + } + } + return retVal; + } + + bool VirtualTerminalClient::get_is_object_scalable(VirtualTerminalObjectType type) + { + bool retVal = false; + + switch (type) + { + case VirtualTerminalObjectType::WorkingSet: + { + retVal = false; + } + break; + + case VirtualTerminalObjectType::DataMask: + case VirtualTerminalObjectType::AlarmMask: + case VirtualTerminalObjectType::Container: + case VirtualTerminalObjectType::Key: + case VirtualTerminalObjectType::Button: + case VirtualTerminalObjectType::InputBoolean: + case VirtualTerminalObjectType::InputString: + case VirtualTerminalObjectType::InputNumber: + case VirtualTerminalObjectType::InputList: + case VirtualTerminalObjectType::OutputString: + case VirtualTerminalObjectType::OutputNumber: + case VirtualTerminalObjectType::OutputList: + case VirtualTerminalObjectType::OutputLine: + case VirtualTerminalObjectType::OutputRectangle: + case VirtualTerminalObjectType::OutputEllipse: + case VirtualTerminalObjectType::OutputPolygon: + case VirtualTerminalObjectType::OutputMeter: + case VirtualTerminalObjectType::OutputLinearBarGraph: + case VirtualTerminalObjectType::OutputArchedBarGraph: + case VirtualTerminalObjectType::PictureGraphic: + case VirtualTerminalObjectType::AuxiliaryFunctionType1: + case VirtualTerminalObjectType::AuxiliaryInputType1: + case VirtualTerminalObjectType::AuxiliaryFunctionType2: + case VirtualTerminalObjectType::AuxiliaryInputType2: + case VirtualTerminalObjectType::FontAttributes: + { + retVal = true; + } + break; + + default: + { + retVal = false; + } + break; + } + return retVal; + } + + VirtualTerminalClient::FontSize VirtualTerminalClient::get_font_or_next_smallest_font(FontSize originalFont) const + { + FontSize retVal = originalFont; + + while ((!get_font_size_supported(retVal)) && (FontSize::Size6x8 != retVal)) + { + retVal = static_cast(static_cast(originalFont) - 1); + } + return retVal; + } + + VirtualTerminalClient::FontSize VirtualTerminalClient::remap_font_to_scale(FontSize originalFont, float scaleFactor) + { + static constexpr float SCALE_FACTOR_POSITIVE_FUDGE = 1.05f; + static constexpr float SCALE_FACTOR_NEGATIVE_FUDGE = 0.95f; + FontSize retVal = originalFont; + + if (scaleFactor > SCALE_FACTOR_POSITIVE_FUDGE || scaleFactor < SCALE_FACTOR_NEGATIVE_FUDGE) + { + const std::unordered_map>> FONT_SCALING_MAPPER{ + { FontSize::Size6x8, + { { 23.95f, FontSize::Size128x192 }, + { 21.30f, FontSize::Size128x128 }, + { 15.95f, FontSize::Size96x128 }, + { 11.95f, FontSize::Size64x96 }, + { 10.60f, FontSize::Size64x64 }, + { 7.95f, FontSize::Size48x64 }, + { 5.95f, FontSize::Size32x48 }, + { 5.30f, FontSize::Size32x32 }, + { 3.95f, FontSize::Size24x32 }, + { 2.95f, FontSize::Size16x24 }, + { 2.60f, FontSize::Size16x16 }, + { 1.95f, FontSize::Size12x16 }, + { 1.45f, FontSize::Size8x12 }, + { 1.30f, FontSize::Size8x8 } } }, + { FontSize::Size8x8, + { { 23.95f, FontSize::Size128x192 }, + { 15.95f, FontSize::Size128x128 }, + { 11.95f, FontSize::Size64x96 }, + { 7.95f, FontSize::Size64x64 }, + { 5.95f, FontSize::Size32x48 }, + { 3.95f, FontSize::Size32x32 }, + { 2.95f, FontSize::Size16x24 }, + { 1.95f, FontSize::Size16x16 }, + { 1.45f, FontSize::Size8x12 }, + { 0.0f, FontSize::Size6x8 } } }, + { FontSize::Size8x12, + { { 15.95f, FontSize::Size128x192 }, + { 11.95f, FontSize::Size96x128 }, + { 7.95f, FontSize::Size64x96 }, + { 5.95f, FontSize::Size48x64 }, + { 3.95f, FontSize::Size32x48 }, + { 2.95f, FontSize::Size24x32 }, + { 1.95f, FontSize::Size16x24 }, + { 1.45f, FontSize::Size12x16 }, + { 0.0f, FontSize::Size6x8 } } }, + { FontSize::Size12x16, + { { 11.95f, FontSize::Size128x192 }, + { 10.60f, FontSize::Size128x128 }, + { 7.95f, FontSize::Size96x128 }, + { 5.95f, FontSize::Size64x96 }, + { 5.30f, FontSize::Size64x64 }, + { 3.95f, FontSize::Size48x64 }, + { 2.95f, FontSize::Size32x48 }, + { 2.60f, FontSize::Size32x32 }, + { 1.95f, FontSize::Size24x32 }, + { 1.45f, FontSize::Size16x24 }, + { 1.30f, FontSize::Size16x16 }, + { 0.75f, FontSize::Size8x12 }, + { 0.67f, FontSize::Size8x8 }, + { 0.0f, FontSize::Size6x8 } } }, + { FontSize::Size16x16, + { { 11.95f, FontSize::Size128x192 }, + { 7.95f, FontSize::Size128x128 }, + { 5.95f, FontSize::Size64x96 }, + { 3.95f, FontSize::Size64x64 }, + { 2.95f, FontSize::Size32x48 }, + { 1.95f, FontSize::Size32x32 }, + { 1.45f, FontSize::Size16x24 }, + { 0.75f, FontSize::Size8x12 }, + { 0.50f, FontSize::Size8x8 }, + { 0.0f, FontSize::Size6x8 } } }, + { FontSize::Size16x24, + { { 7.95f, FontSize::Size128x128 }, + { 5.95f, FontSize::Size96x128 }, + { 3.95f, FontSize::Size64x96 }, + { 2.95f, FontSize::Size48x64 }, + { 1.95f, FontSize::Size32x48 }, + { 1.45f, FontSize::Size24x32 }, + { 0.75f, FontSize::Size12x16 }, + { 0.50f, FontSize::Size8x12 }, + { 0.0f, FontSize::Size6x8 } } }, + { FontSize::Size24x32, + { { 5.95f, FontSize::Size128x192 }, + { 5.30f, FontSize::Size128x128 }, + { 3.95f, FontSize::Size96x128 }, + { 2.95f, FontSize::Size64x96 }, + { 2.60f, FontSize::Size64x64 }, + { 1.95f, FontSize::Size48x64 }, + { 1.45f, FontSize::Size32x48 }, + { 1.30f, FontSize::Size32x32 }, + { 0.75f, FontSize::Size16x24 }, + { 0.60f, FontSize::Size16x16 }, + { 0.50f, FontSize::Size12x16 }, + { 0.37f, FontSize::Size8x12 }, + { 0.30f, FontSize::Size8x8 }, + { 0.0f, FontSize::Size6x8 } } }, + { FontSize::Size32x32, + { { 5.95f, FontSize::Size128x192 }, + { 3.95f, FontSize::Size128x128 }, + { 2.95f, FontSize::Size64x96 }, + { 1.95f, FontSize::Size64x64 }, + { 1.45f, FontSize::Size32x48 }, + { 0.75f, FontSize::Size16x24 }, + { 0.50f, FontSize::Size16x16 }, + { 0.37f, FontSize::Size8x12 }, + { 0.20f, FontSize::Size8x8 }, + { 0.0f, FontSize::Size6x8 } } }, + { FontSize::Size32x48, + { { 3.95f, FontSize::Size128x192 }, + { 2.95f, FontSize::Size96x128 }, + { 1.95f, FontSize::Size64x96 }, + { 1.45f, FontSize::Size48x64 }, + { 0.75f, FontSize::Size24x32 }, + { 0.50f, FontSize::Size16x24 }, + { 0.37f, FontSize::Size12x16 }, + { 0.20f, FontSize::Size8x12 }, + { 0.0f, FontSize::Size6x8 } } }, + { FontSize::Size48x64, + { { 2.95f, FontSize::Size128x192 }, + { 2.60f, FontSize::Size128x128 }, + { 1.95f, FontSize::Size96x128 }, + { 1.45f, FontSize::Size64x96 }, + { 1.30f, FontSize::Size64x64 }, + { 0.75f, FontSize::Size32x48 }, + { 0.60f, FontSize::Size32x32 }, + { 0.50f, FontSize::Size24x32 }, + { 0.37f, FontSize::Size16x24 }, + { 0.33f, FontSize::Size16x16 }, + { 0.25f, FontSize::Size12x16 }, + { 0.18f, FontSize::Size8x12 }, + { 0.16f, FontSize::Size8x8 }, + { 0.0f, FontSize::Size6x8 } } }, + { FontSize::Size64x64, + { { 2.95f, FontSize::Size128x192 }, + { 1.95f, FontSize::Size128x128 }, + { 1.45f, FontSize::Size64x96 }, + { 0.75f, FontSize::Size32x48 }, + { 0.50f, FontSize::Size32x32 }, + { 0.37f, FontSize::Size16x24 }, + { 0.25f, FontSize::Size16x16 }, + { 0.18f, FontSize::Size8x12 }, + { 0.12f, FontSize::Size8x8 }, + { 0.0f, FontSize::Size6x8 } } }, + { FontSize::Size64x96, + { { 1.95f, FontSize::Size128x192 }, + { 1.45f, FontSize::Size96x128 }, + { 0.75f, FontSize::Size48x64 }, + { 0.50f, FontSize::Size32x48 }, + { 0.37f, FontSize::Size24x32 }, + { 0.25f, FontSize::Size16x24 }, + { 0.18f, FontSize::Size12x16 }, + { 0.12f, FontSize::Size8x12 }, + { 0.0f, FontSize::Size6x8 } } }, + { FontSize::Size96x128, + { { 1.45f, FontSize::Size128x192 }, + { 1.30f, FontSize::Size128x128 }, + { 0.75f, FontSize::Size64x96 }, + { 0.60f, FontSize::Size64x64 }, + { 0.50f, FontSize::Size48x64 }, + { 0.37f, FontSize::Size32x48 }, + { 0.33f, FontSize::Size32x32 }, + { 0.25f, FontSize::Size24x32 }, + { 0.18f, FontSize::Size16x24 }, + { 0.16f, FontSize::Size16x16 }, + { 0.125f, FontSize::Size12x16 }, + { 0.09f, FontSize::Size8x12 }, + { 0.08f, FontSize::Size8x8 }, + { 0.0f, FontSize::Size6x8 } } }, + { FontSize::Size128x128, + { { 1.45f, FontSize::Size128x192 }, + { 0.75f, FontSize::Size64x96 }, + { 0.50f, FontSize::Size64x64 }, + { 0.37f, FontSize::Size32x48 }, + { 0.25f, FontSize::Size32x32 }, + { 0.18f, FontSize::Size16x24 }, + { 0.125f, FontSize::Size16x16 }, + { 0.09f, FontSize::Size8x12 }, + { 0.06f, FontSize::Size8x8 }, + { 0.0f, FontSize::Size6x8 } } }, + { FontSize::Size128x192, + { { 0.75f, FontSize::Size96x128 }, + { 0.50f, FontSize::Size64x96 }, + { 0.37f, FontSize::Size48x64 }, + { 0.25f, FontSize::Size32x48 }, + { 0.18f, FontSize::Size24x32 }, + { 0.125f, FontSize::Size16x24 }, + { 0.09f, FontSize::Size12x16 }, + { 0.06f, FontSize::Size8x12 }, + { 0.0f, FontSize::Size6x8 } } } + }; + + auto iterator = FONT_SCALING_MAPPER.find(originalFont); + if (iterator != FONT_SCALING_MAPPER.end()) + { + for (auto &pair : iterator->second) + { + if (scaleFactor >= pair.first) + { + retVal = pair.second; + break; + } + } + } + + if (retVal == originalFont) + { + // Unknown font? Newer version than we support of the ISO standard? Or scaling factor out of range? + CANStackLogger::error("[VT]: Unable to scale font type " + isobus::to_string(static_cast(originalFont)) + + " with scale factor " + isobus::to_string(scaleFactor) + ". Returning original font."); + } + } + return retVal; + } + + std::uint32_t VirtualTerminalClient::get_minimum_object_length(VirtualTerminalObjectType type) + { + std::uint32_t retVal = 0; + + switch (type) + { + case VirtualTerminalObjectType::WorkingSet: + { + retVal = 10; + } + break; + + case VirtualTerminalObjectType::OutputList: + case VirtualTerminalObjectType::ExternalReferenceNAME: + case VirtualTerminalObjectType::ObjectLabelRefrenceList: + { + retVal = 12; + } + break; + + case VirtualTerminalObjectType::AlarmMask: + case VirtualTerminalObjectType::Container: + case VirtualTerminalObjectType::KeyGroup: + { + retVal = 10; + } + break; + + case VirtualTerminalObjectType::ExternalObjectPointer: + { + retVal = 9; + } + break; + + case VirtualTerminalObjectType::SoftKeyMask: + case VirtualTerminalObjectType::ColourMap: + { + retVal = 6; + } + break; + + case VirtualTerminalObjectType::Key: + case VirtualTerminalObjectType::NumberVariable: + case VirtualTerminalObjectType::InputAttributes: + { + retVal = 7; + } + break; + + case VirtualTerminalObjectType::Button: + case VirtualTerminalObjectType::InputBoolean: + case VirtualTerminalObjectType::OutputRectangle: + case VirtualTerminalObjectType::InputList: + case VirtualTerminalObjectType::ExternalObjectDefinition: + { + retVal = 13; + } + break; + + case VirtualTerminalObjectType::InputString: + { + retVal = 19; + } + break; + + case VirtualTerminalObjectType::InputNumber: + { + retVal = 38; + } + break; + + case VirtualTerminalObjectType::OutputString: + { + retVal = 17; + } + break; + + case VirtualTerminalObjectType::OutputNumber: + { + retVal = 29; + } + break; + + case VirtualTerminalObjectType::OutputLine: + { + retVal = 11; + } + break; + + case VirtualTerminalObjectType::OutputEllipse: + { + retVal = 15; + } + break; + + case VirtualTerminalObjectType::OutputPolygon: + { + retVal = 14; + } + break; + + case VirtualTerminalObjectType::OutputMeter: + { + retVal = 21; + } + break; + + case VirtualTerminalObjectType::OutputLinearBarGraph: + { + retVal = 24; + } + break; + + case VirtualTerminalObjectType::OutputArchedBarGraph: + { + retVal = 27; + } + break; + + case VirtualTerminalObjectType::PictureGraphic: + case VirtualTerminalObjectType::Animation: + case VirtualTerminalObjectType::WindowMask: + { + retVal = 17; + } + break; + + case VirtualTerminalObjectType::StringVariable: + case VirtualTerminalObjectType::ExtendedInputAttributes: + case VirtualTerminalObjectType::ObjectPointer: + case VirtualTerminalObjectType::Macro: + { + retVal = 5; + } + break; + + case VirtualTerminalObjectType::FontAttributes: + case VirtualTerminalObjectType::LineAttributes: + case VirtualTerminalObjectType::FillAttributes: + case VirtualTerminalObjectType::DataMask: + { + retVal = 8; + } + break; + + case VirtualTerminalObjectType::GraphicsContext: + { + retVal = 34; + } + break; + + default: + { + CANStackLogger::error("[VT]: Cannot autoscale object pool due to unknown object minimum length - type " + isobus::to_string(static_cast(type))); + } + break; + } + return retVal; + } + + std::uint32_t VirtualTerminalClient::get_number_bytes_in_object(std::uint8_t *buffer) + { + auto currentObjectType = static_cast(buffer[2]); + std::uint32_t retVal = get_minimum_object_length(currentObjectType); + + switch (currentObjectType) + { + case VirtualTerminalObjectType::WorkingSet: + { + const std::uint32_t sizeOfChildObjects = (buffer[7] * 6); + const std::uint32_t sizeOfMacros = (buffer[8] * 2); + const std::uint32_t sizeOfLanguageCodes = (buffer[9] * 2); + retVal += (sizeOfLanguageCodes + sizeOfChildObjects + sizeOfMacros); + } + break; + + case VirtualTerminalObjectType::DataMask: + { + const std::uint32_t sizeOfChildObjects = (buffer[6] * 6); + const std::uint32_t sizeOfMacros = (buffer[7] * 2); + retVal += (sizeOfChildObjects + sizeOfMacros); + } + break; + + case VirtualTerminalObjectType::AlarmMask: + case VirtualTerminalObjectType::Container: + { + const std::uint32_t sizeOfChildObjects = (buffer[8] * 6); + const std::uint32_t sizeOfMacros = (buffer[9] * 2); + retVal += (sizeOfChildObjects + sizeOfMacros); + } + break; + + case VirtualTerminalObjectType::SoftKeyMask: + { + const std::uint32_t sizeOfChildObjects = (buffer[4] * 2); + const std::uint32_t sizeOfMacros = (buffer[5] * 2); + retVal += (sizeOfChildObjects + sizeOfMacros); + } + break; + + case VirtualTerminalObjectType::Key: + { + const std::uint32_t sizeOfChildObjects = (buffer[5] * 6); + const std::uint32_t sizeOfMacros = (buffer[6] * 2); + retVal += (sizeOfChildObjects + sizeOfMacros); + } + break; + + case VirtualTerminalObjectType::Button: + { + const std::uint32_t sizeOfChildObjects = (buffer[11] * 6); + const std::uint32_t sizeOfMacros = (buffer[12] * 2); + retVal += (sizeOfChildObjects + sizeOfMacros); + } + break; + + case VirtualTerminalObjectType::InputBoolean: + { + const std::uint32_t sizeOfMacros = (buffer[12] * 2); + retVal += sizeOfMacros; + } + break; + + case VirtualTerminalObjectType::InputString: + { + const std::uint32_t sizeOfValue = buffer[16]; + const std::uint32_t sizeOfMacros = (buffer[18 + sizeOfValue] * 2); + retVal += (sizeOfValue + sizeOfMacros); + } + break; + + case VirtualTerminalObjectType::InputNumber: + { + const std::uint32_t sizeOfMacros = (buffer[37] * 2); + retVal += sizeOfMacros; + } + break; + + case VirtualTerminalObjectType::InputList: + { + const std::uint32_t sizeOfMacros = (buffer[12] * 2); + const std::uint32_t sizeOfListObjectIDs = (buffer[10] * 2); + retVal += (sizeOfMacros + sizeOfListObjectIDs); + } + break; + + case VirtualTerminalObjectType::OutputString: + { + const std::uint32_t sizeOfValue = (static_cast(buffer[14]) | static_cast(buffer[15] << 8)); + const std::uint32_t sizeOfMacros = (buffer[16 + sizeOfValue] * 2); + retVal += (sizeOfMacros + sizeOfValue); + } + break; + + case VirtualTerminalObjectType::OutputNumber: + { + const std::uint32_t sizeOfMacros = (buffer[28] * 2); + retVal += sizeOfMacros; + } + break; + + case VirtualTerminalObjectType::OutputList: + { + const std::uint32_t sizeOfMacros = (buffer[11] * 2); + const std::uint32_t sizeOfListObjectIDs = (buffer[10] * 2); + retVal += (sizeOfMacros + sizeOfListObjectIDs); + } + break; + + case VirtualTerminalObjectType::OutputLine: + { + const std::uint32_t sizeOfMacros = (buffer[10] * 2); + retVal += sizeOfMacros; + } + break; + + case VirtualTerminalObjectType::OutputRectangle: + { + const std::uint32_t sizeOfMacros = (buffer[12] * 2); + retVal += sizeOfMacros; + } + break; + + case VirtualTerminalObjectType::OutputEllipse: + { + const std::uint32_t sizeOfMacros = (buffer[14] * 2); + retVal += sizeOfMacros; + } + break; + + case VirtualTerminalObjectType::OutputPolygon: + { + const std::uint32_t sizeOfPoints = (buffer[12] * 4); + const std::uint32_t sizeOfMacros = (buffer[13] * 2); + retVal += (sizeOfMacros + sizeOfPoints); + } + break; + + case VirtualTerminalObjectType::OutputMeter: + { + const std::uint32_t sizeOfMacros = (buffer[20] * 2); + retVal += sizeOfMacros; + } + break; + + case VirtualTerminalObjectType::OutputLinearBarGraph: + { + const std::uint32_t sizeOfMacros = (buffer[23] * 2); + retVal += sizeOfMacros; + } + break; + + case VirtualTerminalObjectType::OutputArchedBarGraph: + { + const std::uint32_t sizeOfMacros = (buffer[26] * 2); + retVal += sizeOfMacros; + } + break; + + case VirtualTerminalObjectType::PictureGraphic: + { + const std::uint32_t sizeOfMacros = (buffer[16] * 2); + const std::uint32_t sizeOfRawData = (static_cast(buffer[12]) | + (static_cast(buffer[13]) << 8) | + (static_cast(buffer[14]) << 16) | + (static_cast(buffer[15]) << 24)); + retVal += (sizeOfRawData + sizeOfMacros); + } + break; + + case VirtualTerminalObjectType::ObjectPointer: + case VirtualTerminalObjectType::NumberVariable: + case VirtualTerminalObjectType::GraphicsContext: + case VirtualTerminalObjectType::ExternalReferenceNAME: + case VirtualTerminalObjectType::ExternalObjectPointer: + case VirtualTerminalObjectType::AuxiliaryControlDesignatorType2: + { + // No additional length + } + break; + + case VirtualTerminalObjectType::StringVariable: + { + const std::uint32_t sizeOfValue = (static_cast(buffer[3]) | static_cast(buffer[4]) << 8); + retVal += sizeOfValue; + } + break; + + case VirtualTerminalObjectType::FontAttributes: + case VirtualTerminalObjectType::LineAttributes: + case VirtualTerminalObjectType::FillAttributes: + { + const std::uint32_t sizeOfMacros = (buffer[7] * 2); + retVal += sizeOfMacros; + } + break; + + case VirtualTerminalObjectType::InputAttributes: + { + const std::uint32_t sizeOfValidationString = buffer[4]; + const std::uint32_t sizeOfMacros = (buffer[5 + sizeOfValidationString] * 2); + retVal += (sizeOfMacros + sizeOfValidationString); + } + break; + + case VirtualTerminalObjectType::ExtendedInputAttributes: + { + const std::uint32_t numberOfCodePlanes = buffer[5]; + retVal += (numberOfCodePlanes * 2); // Doesn't include the character ranges, need to handle those externally + } + break; + + case VirtualTerminalObjectType::Macro: + { + const std::uint32_t numberOfMacroBytes = (static_cast(buffer[3]) | (static_cast(buffer[4]) << 8)); + retVal += numberOfMacroBytes; + } + break; + + case VirtualTerminalObjectType::ColourMap: + { + const std::uint32_t numberIndexes = (static_cast(buffer[3]) | (static_cast(buffer[4]) << 8)); + retVal += numberIndexes; + } + break; + + case VirtualTerminalObjectType::WindowMask: + { + const std::uint32_t sizeOfReferences = (buffer[14] * 2); + const std::uint32_t numberObjects = (buffer[15] * 6); + const std::uint32_t sizeOfMacros = (buffer[16] * 2); + retVal += (sizeOfMacros + numberObjects + sizeOfReferences); + } + break; + + case VirtualTerminalObjectType::KeyGroup: + { + const std::uint32_t numberObjects = (buffer[8] * 2); + const std::uint32_t sizeOfMacros = (buffer[9] * 2); + retVal += (sizeOfMacros + numberObjects); + } + break; + + case VirtualTerminalObjectType::ObjectLabelRefrenceList: + { + const std::uint32_t sizeOfLabeledObjects = ((static_cast(buffer[4]) | static_cast(buffer[5]) << 8) * 7); + retVal += sizeOfLabeledObjects; + } + break; + + case VirtualTerminalObjectType::ExternalObjectDefinition: + { + const std::uint32_t sizeOfObjects = (buffer[12] * 2); + retVal += sizeOfObjects; + } + break; + + case VirtualTerminalObjectType::Animation: + { + const std::uint32_t sizeOfObjects = (buffer[15] * 6); + const std::uint32_t sizeOfMacros = (buffer[16] * 2); + retVal += (sizeOfMacros + sizeOfObjects); + } + break; + + case VirtualTerminalObjectType::AuxiliaryFunctionType1: + case VirtualTerminalObjectType::AuxiliaryFunctionType2: + { + const std::uint32_t sizeOfObjects = (buffer[5] * 6); + retVal += sizeOfObjects; + } + break; + + case VirtualTerminalObjectType::AuxiliaryInputType1: + case VirtualTerminalObjectType::AuxiliaryInputType2: + { + const std::uint32_t sizeOfObjects = (buffer[6] * 6); + retVal += sizeOfObjects; + } + break; + + default: + { + CANStackLogger::error("[VT]: Cannot autoscale object pool due to unknown object total length - type " + isobus::to_string(static_cast(buffer[2]))); + } + break; + } + return retVal; + } + + void VirtualTerminalClient::process_standard_object_height_and_width(std::uint8_t *buffer, float scaleFactor) + { + auto width = static_cast(((static_cast(buffer[3]) | (static_cast(buffer[4]) << 8))) * scaleFactor); + auto height = static_cast(((static_cast(buffer[5]) | (static_cast(buffer[6]) << 8))) * scaleFactor); + buffer[3] = (width & 0xFF); + buffer[4] = (width >> 8); + buffer[5] = (height & 0xFF); + buffer[6] = (height >> 8); + } + + bool VirtualTerminalClient::resize_object(std::uint8_t *buffer, float scaleFactor, VirtualTerminalObjectType type) + { + bool retVal = false; + + if (get_is_object_scalable(type)) + { + switch (type) + { + case VirtualTerminalObjectType::DataMask: + { + const std::uint8_t childrenToFollow = buffer[6]; + + for (std::uint_fast8_t i = 0; i < childrenToFollow; i++) + { + auto childX = static_cast(((static_cast(buffer[10 + (6 * i)]) | (static_cast(buffer[11 + (6 * i)]) << 8))) * scaleFactor); + auto childY = static_cast(((static_cast(buffer[12 + (6 * i)]) | (static_cast(buffer[13 + (6 * i)]) << 8))) * scaleFactor); + buffer[10 + (6 * i)] = (childX & 0xFF); + buffer[11 + (6 * i)] = (childX >> 8); + buffer[12 + (6 * i)] = (childY & 0xFF); + buffer[13 + (6 * i)] = (childY >> 8); + } + retVal = true; + } + break; + + case VirtualTerminalObjectType::AlarmMask: + { + const std::uint8_t childrenToFollow = buffer[8]; + + for (std::uint_fast8_t i = 0; i < childrenToFollow; i++) + { + auto childX = static_cast(((static_cast(buffer[12 + (6 * i)]) | (static_cast(buffer[13 + (6 * i)]) << 8))) * scaleFactor); + auto childY = static_cast(((static_cast(buffer[14 + (6 * i)]) | (static_cast(buffer[15 + (6 * i)]) << 8))) * scaleFactor); + buffer[12 + (6 * i)] = (childX & 0xFF); + buffer[13 + (6 * i)] = (childX >> 8); + buffer[14 + (6 * i)] = (childY & 0xFF); + buffer[15 + (6 * i)] = (childY >> 8); + } + retVal = true; + } + break; + + case VirtualTerminalObjectType::Container: + { + std::uint8_t childrenToFollow = buffer[8]; + + // Modify the object in memory + process_standard_object_height_and_width(buffer, scaleFactor); + + // Iterate over the list of children and move them proportionally to the new size + // The container is 10 bytes, followed by children with 2 bytes of ID, 2 of X, and 2 of Y + for (std::uint_fast8_t i = 0; i < childrenToFollow; i++) + { + auto childX = static_cast(((static_cast(buffer[12 + (6 * i)]) | (static_cast(buffer[13 + (6 * i)]) << 8))) * scaleFactor); + auto childY = static_cast(((static_cast(buffer[14 + (6 * i)]) | (static_cast(buffer[15 + (6 * i)]) << 8))) * scaleFactor); + buffer[12 + (6 * i)] = (childX & 0xFF); + buffer[13 + (6 * i)] = (childX >> 8); + buffer[14 + (6 * i)] = (childY & 0xFF); + buffer[15 + (6 * i)] = (childY >> 8); + } + retVal = true; + } + break; + + case VirtualTerminalObjectType::Button: + { + std::uint8_t childrenToFollow = buffer[11]; + + // Modify the object in memory + process_standard_object_height_and_width(buffer, scaleFactor); + + // Iterate over the list of children and move them proportionally to the new size + for (std::uint_fast8_t i = 0; i < childrenToFollow; i++) + { + auto childWidth = static_cast(((static_cast(buffer[15 + (6 * i)]) | (static_cast(buffer[16 + (6 * i)]) << 8))) * scaleFactor); + auto childHeight = static_cast(((static_cast(buffer[17 + (6 * i)]) | (static_cast(buffer[18 + (6 * i)]) << 8))) * scaleFactor); + buffer[15 + (6 * i)] = (childWidth & 0xFF); + buffer[16 + (6 * i)] = (childWidth >> 8); + buffer[17 + (6 * i)] = (childHeight & 0xFF); + buffer[18 + (6 * i)] = (childHeight >> 8); + } + retVal = true; + } + break; + + case VirtualTerminalObjectType::InputBoolean: + { + auto width = static_cast((static_cast(buffer[4]) | (static_cast(buffer[5]) << 8))); + + // Modify the object in memory + buffer[4] = (width & 0xFF); + buffer[5] = (width >> 8); + retVal = true; + } + break; + + case VirtualTerminalObjectType::InputString: + case VirtualTerminalObjectType::InputNumber: + case VirtualTerminalObjectType::InputList: + case VirtualTerminalObjectType::OutputString: + case VirtualTerminalObjectType::OutputNumber: + case VirtualTerminalObjectType::OutputList: + case VirtualTerminalObjectType::OutputLinearBarGraph: + { + // Modify the object in memory + process_standard_object_height_and_width(buffer, scaleFactor); + retVal = true; + } + break; + + case VirtualTerminalObjectType::OutputLine: + case VirtualTerminalObjectType::OutputRectangle: + case VirtualTerminalObjectType::OutputEllipse: + { + // Modify the object in memory + auto width = static_cast(((static_cast(buffer[5]) | (static_cast(buffer[6]) << 8))) * scaleFactor); + auto height = static_cast(((static_cast(buffer[7]) | (static_cast(buffer[8]) << 8))) * scaleFactor); + buffer[5] = (width & 0xFF); + buffer[6] = (width >> 8); + buffer[7] = (height & 0xFF); + buffer[8] = (height >> 8); + retVal = true; + } + break; + + case VirtualTerminalObjectType::OutputPolygon: + { + const std::uint8_t numberOfPoints = buffer[12]; + + // Modify the object in memory + process_standard_object_height_and_width(buffer, scaleFactor); + + // Reposition the child points + for (std::uint_fast8_t i = 0; i < numberOfPoints; i++) + { + auto xPosition = static_cast(((static_cast(buffer[14 + (4 * i)]) | (static_cast(buffer[15 + (4 * i)]) << 8))) * scaleFactor); + auto yPosition = static_cast(((static_cast(buffer[16 + (4 * i)]) | (static_cast(buffer[17 + (4 * i)]) << 8))) * scaleFactor); + buffer[14 + (4 * i)] = (xPosition & 0xFF); + buffer[15 + (4 * i)] = (xPosition >> 8); + buffer[16 + (4 * i)] = (yPosition & 0xFF); + buffer[17 + (4 * i)] = (yPosition >> 8); + } + retVal = true; + } + break; + + case VirtualTerminalObjectType::OutputMeter: + case VirtualTerminalObjectType::PictureGraphic: + { + // Modify the object in memory + auto width = static_cast(((static_cast(buffer[3]) | (static_cast(buffer[4]) << 8))) * scaleFactor); + buffer[3] = (width & 0xFF); + buffer[4] = (width >> 8); + retVal = true; + } + break; + + case VirtualTerminalObjectType::OutputArchedBarGraph: + { + // Modify the object in memory + process_standard_object_height_and_width(buffer, scaleFactor); + + auto width = static_cast(((static_cast(buffer[12]) | (static_cast(buffer[13]) << 8))) * scaleFactor); + buffer[12] = (width & 0xFF); + buffer[13] = (width >> 8); + retVal = true; + } + break; + + case VirtualTerminalObjectType::Animation: + { + std::uint8_t childrenToFollow = buffer[15]; + + // Modify the object in memory + process_standard_object_height_and_width(buffer, scaleFactor); + + // Iterate over the list of children and move them proportionally to the new size + for (std::uint_fast8_t i = 0; i < childrenToFollow; i++) + { + auto childX = static_cast(((static_cast(buffer[20 + (6 * i)]) | (static_cast(buffer[21 + (6 * i)]) << 8))) * scaleFactor); + auto childY = static_cast(((static_cast(buffer[22 + (6 * i)]) | (static_cast(buffer[23 + (6 * i)]) << 8))) * scaleFactor); + buffer[20 + (6 * i)] = (childX & 0xFF); + buffer[21 + (6 * i)] = (childX >> 8); + buffer[22 + (6 * i)] = (childY & 0xFF); + buffer[23 + (6 * i)] = (childY >> 8); + } + retVal = true; + } + break; + + case VirtualTerminalObjectType::Key: + { + const std::uint8_t childrenToFollow = buffer[5]; + + for (std::uint_fast8_t i = 0; i < childrenToFollow; i++) + { + auto childX = static_cast(((static_cast(buffer[9 + (6 * i)]) | (static_cast(buffer[10 + (6 * i)]) << 8))) * scaleFactor); + auto childY = static_cast(((static_cast(buffer[11 + (6 * i)]) | (static_cast(buffer[12 + (6 * i)]) << 8))) * scaleFactor); + buffer[9 + (6 * i)] = (childX & 0xFF); + buffer[10 + (6 * i)] = (childX >> 8); + buffer[11 + (6 * i)] = (childY & 0xFF); + buffer[12 + (6 * i)] = (childY >> 8); + } + retVal = true; + } + break; + + case VirtualTerminalObjectType::FontAttributes: + { + buffer[4] = static_cast(get_font_or_next_smallest_font(remap_font_to_scale(static_cast(buffer[4]), scaleFactor))); + retVal = true; + } + break; + + case VirtualTerminalObjectType::AuxiliaryFunctionType1: + case VirtualTerminalObjectType::AuxiliaryFunctionType2: + case VirtualTerminalObjectType::AuxiliaryInputType2: + { + std::uint8_t childrenToFollow = buffer[5]; + + for (std::uint_fast8_t i = 0; i < childrenToFollow; i++) + { + auto childX = static_cast(((static_cast(buffer[8 + (6 * i)]) | (static_cast(buffer[9 + (6 * i)]) << 8))) * scaleFactor); + auto childY = static_cast(((static_cast(buffer[10 + (6 * i)]) | (static_cast(buffer[11 + (6 * i)]) << 8))) * scaleFactor); + buffer[8 + (6 * i)] = (childX & 0xFF); + buffer[9 + (6 * i)] = (childX >> 8); + buffer[10 + (6 * i)] = (childY & 0xFF); + buffer[11 + (6 * i)] = (childY >> 8); + } + retVal = true; + } + break; + + case VirtualTerminalObjectType::AuxiliaryInputType1: + { + std::uint8_t childrenToFollow = buffer[6]; + + for (std::uint_fast8_t i = 0; i < childrenToFollow; i++) + { + auto childX = static_cast(((static_cast(buffer[9 + (6 * i)]) | (static_cast(buffer[10 + (6 * i)]) << 8))) * scaleFactor); + auto childY = static_cast(((static_cast(buffer[11 + (6 * i)]) | (static_cast(buffer[12 + (6 * i)]) << 8))) * scaleFactor); + buffer[9 + (6 * i)] = (childX & 0xFF); + buffer[10 + (6 * i)] = (childX >> 8); + buffer[11 + (6 * i)] = (childY & 0xFF); + buffer[12 + (6 * i)] = (childY >> 8); + } + retVal = true; + } + break; + + default: + { + CANStackLogger::debug("[VT]: Skipping resize of non-resizable object type " + + isobus::to_string(static_cast(type))); + retVal = false; + } + break; + } + } + else + { + CANStackLogger::debug("[VT]: Skipping resize of non-resizable object type " + + isobus::to_string(static_cast(type))); + retVal = true; + } + return retVal; + } + + void VirtualTerminalClient::worker_thread_function() + { +#if !defined CAN_STACK_DISABLE_THREADS && !defined ARDUINO + for (;;) + { + if (shouldTerminate) + { + break; + } + update(); + std::this_thread::sleep_for(std::chrono::milliseconds(50)); + } +#endif + } + +} // namespace isobus diff --git a/src/isobus_virtual_terminal_client.hpp b/src/isobus_virtual_terminal_client.hpp new file mode 100644 index 0000000..247740d --- /dev/null +++ b/src/isobus_virtual_terminal_client.hpp @@ -0,0 +1,1641 @@ +//================================================================================================ +/// @file isobus_virtual_terminal_client.hpp +/// +/// @brief A class to manage a client connection to a ISOBUS virtual terminal display +/// @author Adrian Del Grosso +/// +/// @copyright 2022 Adrian Del Grosso +//================================================================================================ +#ifndef ISOBUS_VIRTUAL_TERMINAL_CLIENT_HPP +#define ISOBUS_VIRTUAL_TERMINAL_CLIENT_HPP + +#include "can_internal_control_function.hpp" +#include "can_partnered_control_function.hpp" +#include "isobus_language_command_interface.hpp" +#include "isobus_virtual_terminal_objects.hpp" +#include "event_dispatcher.hpp" +#include "processing_flags.hpp" + +#include +#include +#include +#include +#include + +#if !defined CAN_STACK_DISABLE_THREADS && !defined ARDUINO +#include +#endif + +namespace isobus +{ + //================================================================================================ + /// @class VirtualTerminalClient + /// + /// @brief An client interface for interacting with a virtual terminal (VT) server + /// @details This class is the main interface for working with a VT. To use it, + /// you must instantiate it with a source and partner control function, and set and one or more + /// object pools to this class to be uploaded to the VT server. Once this setup is done, call + /// the initialize function to start running the internal state machine. The stack will take care + /// of uploading the object pool, and then you will be able to interact with the pool using the + /// provided "send" functions from your application. + //================================================================================================ + class VirtualTerminalClient + { + public: + /// @brief Enumerates the states that can be sent with a hide/show object command + enum class HideShowObjectCommand : std::uint8_t + { + HideObject = 0, ///< Hides the object + ShowObject = 1 ///< Shows an object + }; + + /// @brief Enumerates the states that can be sent with an enable/disable object command + enum class EnableDisableObjectCommand : std::uint8_t + { + DisableObject = 0, ///< Disables a compatible object + EnableObject = 1 ///< Enables a compatible object + }; + + /// @brief Enumerates the states that can be sent with a select input object options command + enum class SelectInputObjectOptions : std::uint8_t + { + ActivateObjectForDataInput = 0x00, ///< Activates an object for data input + SetFocusToObject = 0xFF ///< Focuses the object (usually this draws a temporary box around it) + }; + + /// @brief The different VT versions that a client or server might support + enum class VTVersion + { + Version2OrOlder, ///< Client or server supports VT version 2 or lower + Version3, ///< Client or server supports all of VT version 3 + Version4, ///< Client or server supports all of VT version 4 + Version5, ///< Client or server supports all of VT version 5 + Version6, ///< Client or server supports all of VT version 6 + ReservedOrUnknown, ///< Reserved value, not to be used + }; + + /// @brief Enumerates the different line directions that can be used when changing an endpoint of an object + enum class LineDirection : std::uint8_t + { + TopLeftToBottomRightOfEnclosingVirtualRectangle = 0, ///< Draws the line from top left to bottom right of the enclosing virtual rectangle + BottomLeftToTopRightOfEnclosingVirtualRectangle = 1 ///< Draws the line from bottom left to top right of the enclosing virtual rectangle + }; + + /// @brief Enumerates the different font sizes + enum class FontSize : std::uint8_t + { + Size6x8 = 0, ///< 6x8 Font size + Size8x8 = 1, ///< 8x8 Font size + Size8x12 = 2, ///< 8x12 Font size + Size12x16 = 3, ///< 12x16 Font size + Size16x16 = 4, ///< 16x16 Font size + Size16x24 = 5, ///< 16x24 Font size + Size24x32 = 6, ///< 24x32 Font size + Size32x32 = 7, ///< 32x32 Font size + Size32x48 = 8, ///< 32x48 Font size + Size48x64 = 9, ///< 48x64 Font size + Size64x64 = 10, ///< 64x64 Font size + Size64x96 = 11, ///< 64x96 Font size + Size96x128 = 12, ///< 96x128 Font size + Size128x128 = 13, ///< 128x128 Font size + Size128x192 = 14 ///< 128x192 Font size + }; + + /// @brief Enumerates the font style options that can be encoded in a font style bitfield + enum class FontStyleBits : std::uint8_t + { + Bold = 0, ///< Bold font style + CrossedOut = 1, ///< Crossed-out font style (strikethrough) + Underlined = 2, ///< Underlined font style + Italic = 3, ///< Italic font style + Inverted = 4, ///< Inverted font style (upside down) + Flashing = 5, ///< Flashing font style + FlashingHidden = 6, ///< Flashing between hidden and shown font style + ProportionalFontRendering = 7 ///< Enables proportional font rendering if supported by the server + }; + + /// @brief Enumerates the different font types + enum class FontType : std::uint8_t + { + ISO8859_1 = 0, ///< ISO Latin 1 + ISO8859_15 = 1, ///< ISO Latin 9 + ISO8859_2 = 2, ///< ISO Latin 2 + Reserved_1 = 3, ///< Reserved + ISO8859_4 = 4, ///< ISO Latin 4 + ISO8859_5 = 5, ///< Cyrillic + Reserved_2 = 6, ///< Reserved + ISO8859_7 = 7, ///< Greek + ReservedEnd = 239, ///< Reserved from ISO8859_7 to this value + ProprietaryBegin = 240, ///< The beginning of the proprietary range + ProprietaryEnd = 255 ///< The end of the proprietary region + }; + + /// @brief Enumerates the different fill types for an object + enum class FillType : std::uint8_t + { + NoFill = 0, ///< No fill will be applied + FillWithLineColour = 1, ///< Fill with the colour of the outline of the shape + FillWithSpecifiedColourInFillColourAttribute = 2, ///< Fill with the colour specified by a fill attribute + FillWithPatternGivenByFillPatternAttribute = 3 ///< Fill with a patter provided by a fill pattern attribute + }; + + /// @brief The types of object pool masks + enum class MaskType : std::uint8_t + { + DataMask = 1, ///< A data mask, used in normal circumstances + AlarmMask = 2 ///< An alarm mask, which has different metadata related to popping up alarms, like priority + }; + + /// @brief The allowable priorities of an alarm mask + enum class AlarmMaskPriority : std::uint8_t + { + High = 0, ///< Overrides lower priority alarm masks + Medium = 1, ///< Overrides low priority alarm masks + Low = 2 ///< Overrides data masks + }; + + /// @brief Denotes the lock/unlock state of a mask. Used to freeze/unfreeze rendering of a mask. + enum class MaskLockState : std::uint8_t + { + UnlockMask = 0, ///< Renders the mask normally + LockMask = 1 ///< Locks the mask so rendering of it is not updated until it is unlocked or a timeout occurs + }; + + /// @brief The different key activation codes that a button press can generate + enum class KeyActivationCode : std::uint8_t + { + ButtonUnlatchedOrReleased = 0, ///< Button is released + ButtonPressedOrLatched = 1, ///< Button is pressed + ButtonStillHeld = 2, ///< Button is being held down (sent cyclically) + ButtonPressAborted = 3 ///< Press was aborted (user navigated away from the button and did not release it) + }; + + /// @brief Enumerates the errors that can be present in an ESC message + enum class ESCMessageErrorCode : std::uint8_t + { + NoError = 0, ///< No error occurred + NoInputFieldOpen = 1, ///< No input field is open + OtherError = 5 ///< Error is not one of the above + }; + + /// @brief Enumerates the different events that can be associated with a macro + enum class MacroEventID : std::uint8_t + { + Reserved = 0, ///< Reserved + OnActivate = 1, ///< Event on activation of an object (such as for data input) + OnDeactivate = 2, ///< Event on deactivation of an object + OnShow = 3, ///< Event on an object being shown + OnHide = 4, ///< Event on an object being hidden + OnEnable = 5, ///< Event on enable of an object + OnDisable = 6, ///< Event on disabling an object + OnChangeActiveMask = 7, ///< Event on changing the active mask + OnChangeSoftKeyMask = 8, ///< Event on change of the soft key mask + OnChangeAttribute = 9, ///< Event on change of an attribute value + OnChangeBackgroundColour = 10, ///< Event on change of a background colour + OnChangeFontAttributes = 11, ///< Event on change of a font attribute + OnChangeLineAttributes = 12, ///< Event on change of a line attribute + OnChangeFillAttributes = 13, ///< Event on change of a fill attribute + OnChangeChildLocation = 14, ///< Event on change of a child objects location + OnChangeSize = 15, ///< Event on change of an object size + OnChangeValue = 16, ///< Event on change of an object value (like via `change numeric value`) + OnChangePriority = 17, ///< Event on change of a mask's priority + OnChangeEndPoint = 18, ///< Event on change of an object endpoint + OnInputFieldSelection = 19, ///< Event when an input field is selected + OnInputFieldDeselection = 20, ///< Event on deselection of an input field + OnESC = 21, ///< Event on ESC (escape) + OnEntryOfValue = 22, ///< Event on entry of a value + OnEntryOfNewValue = 23, ///< Event on entry of a *new* value + OnKeyPress = 24, ///< Event on the press of a key + OnKeyRelease = 25, ///< Event on the release of a key + OnChangeChildPosition = 26, ///< Event on changing a child object's position + OnPointingEventPress = 27, ///< Event on a pointing event press + OnPointingEventRelease = 28, ///< Event on a pointing event release + ReservedBegin = 29, ///< Beginning of the reserved range + ReservedEnd = 254, ///< End of the reserved range + UseExtendedMacroReference = 255 ///< Use extended macro reference + }; + + /// @brief Enumerates the various VT server graphics modes + enum class GraphicMode : std::uint8_t + { + Monochrome = 0, ///< Monochromatic graphics mode (1 bit) + SixteenColour = 1, ///< 16 Colour mode (4 bit) + TwoHundredFiftySixColour = 2 ///< 256 Colour mode (8 bit) + }; + + /// @brief Enumerates the various auxiliary input function types + enum class AuxiliaryTypeTwoFunctionType : std::uint8_t + { + BooleanLatching = 0, ///< Two-position switch (maintains position) (Single Pole, Double Throw) + AnalogueLatching = 1, ///< Two-way analogue (Maintains position setting) + BooleanMomentary = 2, ///< Two-position switch (returns to off) (Momentary Single Pole, Single Throw) + AnalogueMomentaryTwoWay = 3, ///< Two-way analogue (returns to centre position - 50%) + AnalogueMomentaryOneWay = 4, ///< One-way analogue (returns to 0%) + DualBooleanLatching = 5, ///< Three-position switch (maintains position) (Single Pole, Three Positions, Centre Off) + DualBooleanMomentary = 6, ///< Three-position switch (returns to off/centre position) (Momentary Single Pole, Three Positions, Centre Off) + DualBooleanLatchingUpOnly = 7, ///< Three-position switch (maintains position only in up position) (Single Pole, Three Positions, Centre Off) + DualBooleanLatchingDownpOnly = 8, ///< Three-position switch (maintains position only in down position) (Momentary Single Pole, Three Positions, Centre Off) + AnalogueMomentaryBooleanLatching = 9, ///< two-way analogue (returns to centre position) with latching Boolean at 0% and 100% positions + AnalogueLatchingBooleanLatching = 10, ///< two-way analogue (maintains position setting) with momentary Boolean at 0% and 100% positions + QuadratureBooleanMomentary = 11, ///< Two Quadrature mounted Three-position switches (returns to centre position) (Momentary Single Pole, Three Position Single Throw, Centre Off) + QuadratureAnalogueLatching = 12, ///< Two Quadrature mounted Two-way analogue (maintains position) + QuadratureAnalogueMomentary = 13, ///< Two Quadrature mounted Two-way analogue (returns to centre position - 50%) + BidirectionalEncoder = 14, ///< Count increases when turning in the encoders "increase" direction, and decreases when turning in the opposite direction + Reserved = 30, ///< 15-30 Reserved + ReservedRemoveAssignment = 31 ///< Used for Remove assignment command + }; + + /// @brief The internal state machine state of the VT client, mostly just public so tests can access it + enum class StateMachineState : std::uint8_t + { + Disconnected, ///< VT is not connected, and is not trying to connect yet + WaitForPartnerVTStatusMessage, ///< VT client is initialized, waiting for a VT server to come online + SendWorkingSetMasterMessage, ///< Client is sending the working state master message + ReadyForObjectPool, ///< Client needs an object pool before connection can continue + SendGetMemory, ///< Client is sending the "get memory" message to see if VT has enough memory available + WaitForGetMemoryResponse, ///< Client is waiting for a response to the "get memory" message + SendGetNumberSoftkeys, ///< Client is sending the "get number of soft keys" message + WaitForGetNumberSoftKeysResponse, ///< Client is waiting for a response to the "get number of soft keys" message + SendGetTextFontData, ///< Client is sending the "get text font data" message + WaitForGetTextFontDataResponse, ///< Client is waiting for a response to the "get text font data" message + SendGetHardware, ///< Client is sending the "get hardware" message + WaitForGetHardwareResponse, ///< Client is waiting for a response to the "get hardware" message + SendGetVersions, ///< If a version label was specified, check to see if the VT has that version already + WaitForGetVersionsResponse, ///< Client is waiting for a response to the "get versions" message + SendStoreVersion, ///< Sending the store version command + WaitForStoreVersionResponse, ///< Client is waiting for a response to the store version command + SendLoadVersion, ///< Sending the load version command + WaitForLoadVersionResponse, ///< Client is waiting for the VT to respond to the "Load Version" command + UploadObjectPool, ///< Client is uploading the object pool + SendEndOfObjectPool, ///< Client is sending the end of object pool message + WaitForEndOfObjectPoolResponse, ///< Client is waiting for the end of object pool response message + Connected, ///< Client is connected to the VT server and the application layer is in control + Failed ///< Client could not connect to the VT due to an error + }; + + /// @brief A struct for storing information of a function assigned to an auxiliary input + class AssignedAuxiliaryFunction + { + public: + /// @brief Constructs a `AssignedAuxiliaryFunction`, sets default values + /// @param[in] functionObjectID the object ID of the function present in our object pool + /// @param[in] inputObjectID the object ID assigned on the auxiliary inputs end + /// @param[in] functionType the type of function + AssignedAuxiliaryFunction(std::uint16_t functionObjectID, std::uint16_t inputObjectID, AuxiliaryTypeTwoFunctionType functionType); + + /// @brief Allows easy comparison of two `AssignedAuxiliaryFunction` objects + /// @param[in] other the object to compare against + bool operator==(const AssignedAuxiliaryFunction &other) const; + + std::uint16_t functionObjectID; ///< The object ID of the function present in our object pool + std::uint16_t inputObjectID; ///< The object ID assigned on the auxiliary inputs end + AuxiliaryTypeTwoFunctionType functionType; ///< The type of function + }; + + static constexpr std::uint16_t NULL_OBJECT_ID = 0xFFFF; ///< The NULL Object ID, usually drawn as blank space + + /// @brief The constructor for a VirtualTerminalClient + /// @param[in] partner The VT server control function + /// @param[in] clientSource The internal control function to communicate from + VirtualTerminalClient(std::shared_ptr partner, std::shared_ptr clientSource); + + /// @brief Deleted copy constructor for VirtualTerminalClient + VirtualTerminalClient(VirtualTerminalClient &) = delete; + + /// @brief The destructor for the VirtualTerminalClient + ~VirtualTerminalClient(); + + // Setup Functions + /// @brief This function starts the state machine. Call this once you have supplied 1 or more object pool and are ready to connect. + /// @param[in] spawnThread The client will start a thread to manage itself if this parameter is true. Otherwise you must update it cyclically. + void initialize(bool spawnThread); + + /// @brief Returns if the client has been initialized + /// @note This does not mean that the client is connected to the VT server + /// @returns true if the client has been initialized + bool get_is_initialized() const; + + /// @brief Check whether the client is connected to the VT server + /// @returns true if cconnected, false otherwise + bool get_is_connected() const; + + /// @brief Terminates the client and joins the worker thread if applicable + void terminate(); + + /// @brief Halts communication with the VT gracefully and restarts it. + void restart_communication(); + + /// @brief Returns the control function of the VT server with which this VT client communicates. + /// @returns The partner control function for the VT server + std::shared_ptr get_partner_control_function() const; + + /// @brief Returns the internal control function being used by the client + /// @returns The internal control function being used by the client + std::shared_ptr get_internal_control_function() const; + + /// @brief Returns the active working set master's address + /// @returns The active working set master's address, or 0xFE (NULL_CAN_ADDRESS) if none or unknown + std::uint8_t get_active_working_set_master_address() const; + + /// @brief A struct for storing information of a VT key input event + struct VTKeyEvent + { + VirtualTerminalClient *parentPointer; ///< A pointer to the parent VT client + std::uint16_t objectID; ///< The object ID + std::uint16_t parentObjectID; ///< The parent object ID + std::uint8_t keyNumber; ///< The key number + KeyActivationCode keyEvent; ///< The key event + }; + + /// @brief A struct for storing information of a VT pointing event + struct VTPointingEvent + { + VirtualTerminalClient *parentPointer; ///< A pointer to the parent VT client + std::uint16_t xPosition; ///< The x position + std::uint16_t yPosition; ///< The y position + std::uint16_t parentObjectID; ///< The parent object ID + KeyActivationCode keyEvent; ///< The key event + }; + + /// @brief A struct for storing information of a VT input object selection event + struct VTSelectInputObjectEvent + { + VirtualTerminalClient *parentPointer; ///< A pointer to the parent VT client + std::uint16_t objectID; ///< The object ID + bool objectSelected; ///< Whether the object is selected + bool objectOpenForInput; ///< Whether the object is open for input + }; + + /// @brief A struct for storing information of a VT ESC message event + struct VTESCMessageEvent + { + VirtualTerminalClient *parentPointer; ///< A pointer to the parent VT client + std::uint16_t objectID; ///< The object ID + ESCMessageErrorCode errorCode; ///< The error code + }; + + /// @brief A struct for storing information of a VT change numeric value event + struct VTChangeNumericValueEvent + { + VirtualTerminalClient *parentPointer; ///< A pointer to the parent VT client + std::uint32_t value; ///< The value + std::uint16_t objectID; ///< The object ID + }; + + /// @brief A struct for storing information of a VT change active mask event + struct VTChangeActiveMaskEvent + { + VirtualTerminalClient *parentPointer; ///< A pointer to the parent VT client + std::uint16_t maskObjectID; ///< The mask object ID + std::uint16_t errorObjectID; ///< The error object ID + std::uint16_t parentObjectID; ///< The parent object ID + bool missingObjects; ///< Whether there are missing objects + bool maskOrChildHasErrors; ///< Whether the mask or child has errors + bool anyOtherError; ///< Whether there are any other errors + bool poolDeleted; ///< Whether the pool has been deleted + }; + + /// @brief A struct for storing information of a VT change soft key mask event + struct VTChangeSoftKeyMaskEvent + { + VirtualTerminalClient *parentPointer; ///< A pointer to the parent VT client + std::uint16_t dataOrAlarmMaskObjectID; ///< The data or alarm mask object ID + std::uint16_t softKeyMaskObjectID; ///< The soft key mask object ID + bool missingObjects; ///< Whether there are missing objects + bool maskOrChildHasErrors; ///< Whether the mask or child has errors + bool anyOtherError; ///< Whether there are any other errors + bool poolDeleted; ///< Whether the pool has been deleted + }; + + /// @brief A struct for storing information of a VT change string value event + struct VTChangeStringValueEvent + { + std::string value; ///< The value + VirtualTerminalClient *parentPointer; ///< A pointer to the parent VT client + std::uint16_t objectID; ///< The object ID + }; + + /// @brief A struct for storing information of a VT on user-layout hide/show event + struct VTUserLayoutHideShowEvent + { + VirtualTerminalClient *parentPointer; ///< A pointer to the parent VT client + std::uint16_t objectID; ///< The object ID + bool isHidden; ///< Whether the object is hidden + }; + + /// @brief A struct for storing information of a VT control audio signal termination event + struct VTAudioSignalTerminationEvent + { + VirtualTerminalClient *parentPointer; ///< A pointer to the parent VT client + bool isTerminated; ///< Whether the audio signal is terminated + }; + + /// @brief A struct for storing information of an auxilary function event + struct AuxiliaryFunctionEvent + { + AssignedAuxiliaryFunction function; ///< The function + VirtualTerminalClient *parentPointer; ///< A pointer to the parent VT client + std::uint16_t value1; ///< The first value + std::uint16_t value2; ///< The second value + }; + + /// @brief Add a listener for when a soft key is pressed or released + /// @param[in] callback The callback to be invoked + /// @returns A shared pointer to the callback, which must be kept alive for as long as the callback is needed + std::shared_ptr add_vt_soft_key_event_listener(std::function callback); + + /// @brief Add a listener for when a button is pressed or released + /// @param[in] callback The callback to be invoked + /// @returns A shared pointer to the callback, which must be kept alive for as long as the callback is needed + std::shared_ptr add_vt_button_event_listener(std::function callback); + + /// @brief Add a listener for when a pointing event is "pressed or released" + /// @param[in] callback The callback to be invoked + /// @returns A shared pointer to the callback, which must be kept alive for as long as the callback is needed + std::shared_ptr add_vt_pointing_event_listener(std::function callback); + + /// @brief Add a listener for when an input object event is triggered + /// @param[in] callback The callback to be invoked + /// @returns A shared pointer to the callback, which must be kept alive for as long as the callback is needed + std::shared_ptr add_vt_select_input_object_event_listener(std::function callback); + + /// @brief Add a listener for when an ESC message is received, e.g. an open object input is closed + /// @param[in] callback The callback to be invoked + /// @returns A shared pointer to the callback, which must be kept alive for as long as the callback is needed + std::shared_ptr add_vt_esc_message_event_listener(std::function callback); + + /// @brief Add a listener for when a numeric value is changed in an input object + /// @param[in] callback The callback to be invoked + /// @returns A shared pointer to the callback, which must be kept alive for as long as the callback is needed + std::shared_ptr add_vt_change_numeric_value_event_listener(std::function callback); + + /// @brief Add a listener for when the active mask is changed + /// @details The VT sends this whenever there are missing object references or errors in the mask. + /// @param[in] callback The callback to be invoked + /// @returns A shared pointer to the callback, which must be kept alive for as long as the callback is needed + std::shared_ptr add_vt_change_active_mask_event_listener(std::function callback); + + /// @brief Add a listener for when the soft key mask is changed + /// @details The VT sends this whenever there are missing object references or errors in the mask. + /// @param[in] callback The callback to be invoked + /// @returns A shared pointer to the callback, which must be kept alive for as long as the callback is needed + std::shared_ptr add_vt_change_soft_key_mask_event_listener(std::function callback); + + /// @brief Add a listener for when a string value is changed + /// @details The object could be either the input string object or the referenced string variable object. + /// @param[in] callback The callback to be invoked + /// @returns A shared pointer to the callback, which must be kept alive for as long as the callback is needed + std::shared_ptr add_vt_change_string_value_event_listener(std::function callback); + + /// @brief Add a listener for when a user-layout object is hidden or shown + /// @param[in] callback The callback to be invoked + /// @returns A shared pointer to the callback, which must be kept alive for as long as the callback is needed + std::shared_ptr add_vt_user_layout_hide_show_event_listener(std::function callback); + + /// @brief Add a listener for when an audio signal is terminated + /// @param[in] callback The callback to be invoked + /// @returns A shared pointer to the callback, which must be kept alive for as long as the callback is needed + std::shared_ptr add_vt_control_audio_signal_termination_event_listener(std::function callback); + + /// @brief Add a listener for for when a change in auxiliary input for a function is received + /// @param[in] callback The callback to be invoked + /// @returns A shared pointer to the callback, which must be kept alive for as long as the callback is needed + std::shared_ptr add_auxiliary_function_event_listener(std::function callback); + + /// @brief Set the model identification code of our auxiliary input device. + /// @details The model identification code is used to allow other devices identify + /// whether our device differs from a previous versions. If the model identification code + /// is different, the preferred assignments are reset. + /// @param[in] modelIdentificationCode The model identification code + void set_auxiliary_input_model_identification_code(std::uint16_t modelIdentificationCode); + + /// @brief Get whether the VT has enabled the learn mode for the auxiliary input + /// @returns true if the VT has enabled the learn mode for the auxiliary input + bool get_auxiliary_input_learn_mode_enabled() const; + + /// @brief Add a new auxiliary input to be managed by this virtual terminal object. + /// @details This function should be called for each auxiliary input that is available in the pool, + /// and will receive updates using update_auxiliary_input(). + /// @param[in] auxiliaryInputID The ID of the auxiliary input + void add_auxiliary_input_object_id(const std::uint16_t auxiliaryInputID); + + /// @brief Remove an auxiliary input from the pool of managed auxiliary inputs. + /// @param[in] auxiliaryInputID The ID of the auxiliary input + void remove_auxiliary_input_object_id(const std::uint16_t auxiliaryInputID); + + /// @brief Update the state of an auxiliary input. This should be called when + /// the value of an auxiliary input changes. + /// @param[in] auxiliaryInputID The ID of the auxiliary input + /// @param[in] value1 The first value of the auxiliary input. See Table J.5 of Part 6 of the standard for details. + /// @param[in] value2 The second value of the auxiliary input. See Table J.5 of Part 6 of the standard for details. + /// @param[in] controlLocked Whether the auxiliary input is locked + void update_auxiliary_input(const std::uint16_t auxiliaryInputID, const std::uint16_t value1, const std::uint16_t value2, const bool controlLocked = false); + + // Command Messages + /// @brief Sends a hide/show object command + /// @details This command is used to hide or show a Container object. + /// This pertains to the visibility of the object as well as its + /// remembered state.If the object cannot be displayed due to references to missing + /// objects, the VT generates an error in the response. + /// @param[in] objectID The ID of the target object + /// @param[in] command The target hide/show state of the object + /// @returns true if the message was sent successfully + bool send_hide_show_object(std::uint16_t objectID, HideShowObjectCommand command) const; + + /// @brief Sends an enable/disable object command + /// @details This command is used to enable or disable an input field object + /// or a Button object and pertains to the accessibility of an input field + /// object or Button object.This command is also used to enable or disable an Animation object. + /// It is allowed to enable already enabled objects and to disable already disabled objects. + /// @param[in] objectID The ID of the target object + /// @param[in] command The target enable/disable state of the object + /// @returns true if the message was sent successfully + bool send_enable_disable_object(std::uint16_t objectID, EnableDisableObjectCommand command) const; + + /// @brief Sends a select input object command + /// @details This command is used to force the selection of an input field, Button, or Key object. + /// @param[in] objectID The ID of the target object + /// @param[in] option The method by which the object will be selected + /// @returns true if the message was sent successfully + bool send_select_input_object(std::uint16_t objectID, SelectInputObjectOptions option) const; + + /// @brief Sends the ESC message (Escape) + /// @returns true if the message was sent successfully + bool send_ESC() const; + + /// @brief Sends the control audio signal command + /// @details This command may be used to control the audio on the VT. + /// When received this message shall terminate any audio in process from + /// the originating ECU and replace the previous command with the new command. + /// @param[in] activations Number of times to activate the signal + /// @param[in] frequency_hz The audio frequency to command in Hz + /// @param[in] duration_ms Duration of the signal activation + /// @param[in] offTimeDuration_ms The amount of silent time in the signal + /// @returns true if the message was sent successfully + bool send_control_audio_signal(std::uint8_t activations, std::uint16_t frequency_hz, std::uint16_t duration_ms, std::uint16_t offTimeDuration_ms) const; + + /// @brief Sends the set audio volume command + /// @details This command applies to subsequent Control Audio Signal commands. + /// VTs that are not able to modify the volume of the currently playing tone shall set + /// the Audio device is busy bit in the response.This command should not affect in any way + /// the volume settings of other Working Sets and shall not affect the volume of Alarm Masks. + /// @param[in] volume_percent The volume percentage to set the VT server to + /// @returns true if the message was sent successfully + bool send_set_audio_volume(std::uint8_t volume_percent) const; + + /// @brief Sends the change child location command + /// @details The Change Child Location command is used to change the position of an object. The new position is set + /// relative to the object's current position. Since the object can be included in many + /// parent objects, the parent Object ID is also included. If a parent object includes + /// the child object multiple times, then each instance will be moved. + /// The position attributes given in the message have an offset of -127, so + /// a value of 255 results in a +128 px move. + /// Positive values indicate a position change down or to the right. Negative values + /// indicate a position change up or to the left. + /// @param[in] objectID The ID of the target object + /// @param[in] parentObjectID The ID of the object's parent object + /// @param[in] relativeXPositionChange The amount to change the X position by (px) + /// @param[in] relativeYPositionChange The amount to change the Y position by (px) + /// @returns true if the message was sent successfully + bool send_change_child_location(std::uint16_t objectID, std::uint16_t parentObjectID, std::uint8_t relativeXPositionChange, std::uint8_t relativeYPositionChange) const; + + /// @brief Sends the change child position command + /// @details The new position is set relative to the parent object's position. + /// Since the object can be included in many parent objects, the parent Object ID + /// is also included.If a parent object includes the child object multiples times, + /// then each instance will be moved to the same location(the designer may want to + /// use Change Child Location command to move all instances in a relative motion). + /// When the object is moved, the parent object shall be refreshed. + /// The position attributes given in the message are signed integer. + /// Positive values indicate a position below(Y) or to the right of(X) the top left + /// corner of the parent object.Negative values indicate a position above(Y) or to the + /// left of(X) the top left corner of the parent object. + /// @param[in] objectID The ID of the target object + /// @param[in] parentObjectID The ID of the object's parent object + /// @param[in] xPosition The new X position of the object (px) + /// @param[in] yPosition The new Y position of the object (px) + /// @returns true if the message was sent successfully + bool send_change_child_position(std::uint16_t objectID, std::uint16_t parentObjectID, std::uint16_t xPosition, std::uint16_t yPosition) const; + + /// @brief Sends the change size command + /// @details A value of 0 for width or height or both + /// means that the object size is 0 and the object is not drawn. + /// @param[in] objectID The ID of the target object + /// @param[in] newWidth The new width of the object + /// @param[in] newHeight The new height of the object + /// @returns true if the message was sent successfully + bool send_change_size_command(std::uint16_t objectID, std::uint16_t newWidth, std::uint16_t newHeight) const; + + /// @brief Sends the change background colour command + /// @param[in] objectID The ID of the target object + /// @param[in] colour The new background colour of the object + /// @returns true if the message was sent successfully + bool send_change_background_colour(std::uint16_t objectID, std::uint8_t colour) const; + + /// @brief Sends the change numeric value command + /// @details The size of the object shall not be changed by this command. Only the object indicated in the + /// command is to be changed, variables referenced by the object are not changed. + /// @param[in] objectID The ID of the target object + /// @param[in] value The new numeric value of the object + /// @returns true if the message was sent successfully + bool send_change_numeric_value(std::uint16_t objectID, std::uint32_t value) const; + + /// @brief Sends the change string value command + /// @details The size of the object shall not be changed by this command. Only the object indicated in the + /// command is to be changed, variables referenced by the object are not changed. + /// The transferred string is allowed to be smaller than the length of the value attribute of the target object and in + /// this case the VT shall pad the value attribute with space characters. + /// @param[in] objectID The ID of the target object + /// @param[in] stringLength The length of the string to be sent + /// @param[in] value The string to be sent + /// @returns true if the message was sent successfully + bool send_change_string_value(std::uint16_t objectID, uint16_t stringLength, const char *value) const; + + /// @brief Sends the change string value command (with a c++ string instead of buffer + length) + /// @details The size of the object shall not be changed by this command. Only the object indicated in the + /// command is to be changed, variables referenced by the object are not changed. + /// The transferred string is allowed to be smaller than the length of the value attribute of the target object and in + /// this case the VT shall pad the value attribute with space characters. + /// @param[in] objectID The ID of the target object + /// @param[in] value The string to be sent + /// @returns true if the message was sent successfully + bool send_change_string_value(std::uint16_t objectID, const std::string &value) const; + + /// @brief Sends the change endpoint command, which changes the end of an output line + /// @param[in] objectID The ID of the target object + /// @param[in] width_px The width to change the output line to + /// @param[in] height_px The height to change the output line to + /// @param[in] direction The line direction (refer to output line object attributes) + /// @returns true if the message was sent successfully + bool send_change_endpoint(std::uint16_t objectID, std::uint16_t width_px, std::uint16_t height_px, LineDirection direction) const; + + /// @brief Sends the change font attributes command + /// @details This command is used to change the Font Attributes in a Font Attributes object. + /// @param[in] objectID The ID of the target object + /// @param[in] colour See the standard VT colour palette for more details + /// @param[in] size Font size + /// @param[in] type Font Type + /// @param[in] styleBitfield The font style encoded as a bitfield + /// @returns true if the message was sent successfully + bool send_change_font_attributes(std::uint16_t objectID, std::uint8_t colour, FontSize size, std::uint8_t type, std::uint8_t styleBitfield) const; + + /// @brief Sends the change line attributes command + /// @details This command is used to change the Line Attributes in a Line Attributes object. + /// @param[in] objectID The ID of the target object + /// @param[in] colour See the standard VT colour palette for more details + /// @param[in] width The line width + /// @param[in] lineArtBitmask The line art, encoded as a bitfield (See ISO11783-6 for details) + /// @returns true if the message was sent successfully + bool send_change_line_attributes(std::uint16_t objectID, std::uint8_t colour, std::uint8_t width, std::uint16_t lineArtBitmask) const; + + /// @brief Sends the change fill attributes command + /// @details This command is used to change the Fill Attributes in a Fill Attributes object. + /// @param[in] objectID The ID of the target object + /// @param[in] fillType The fill type + /// @param[in] colour See the standard VT colour palette for more details + /// @param[in] fillPatternObjectID Object ID to a fill pattern or NULL_OBJECT_ID + /// @returns true if the message was sent successfully + bool send_change_fill_attributes(std::uint16_t objectID, FillType fillType, std::uint8_t colour, std::uint16_t fillPatternObjectID) const; + + /// @brief Sends the change active mask command + /// @details This command is used to change the active mask of a Working Set + /// to either a Data Mask object or an Alarm Mask object. + /// @param[in] workingSetObjectID The ID of the working set + /// @param[in] newActiveMaskObjectID The object ID of the new active mask + /// @returns true if the message was sent successfully + bool send_change_active_mask(std::uint16_t workingSetObjectID, std::uint16_t newActiveMaskObjectID) const; + + /// @brief Sends the change softkey mask command + /// @details This command is used to change the Soft Key Mask associated with a + /// Data Mask object or an Alarm Mask object. + /// @param[in] type The mask type, alarm or data + /// @param[in] dataOrAlarmMaskObjectID The object ID of the target mask + /// @param[in] newSoftKeyMaskObjectID The object ID of the new softkey mask + /// @returns true if the message was sent successfully + bool send_change_softkey_mask(MaskType type, std::uint16_t dataOrAlarmMaskObjectID, std::uint16_t newSoftKeyMaskObjectID) const; + + /// @brief Sends the change attribute command + /// @details This command is used to change any attribute with an assigned Attribute ID. + /// This message cannot be used to change strings. + /// @param[in] objectID The ID of the target object + /// @param[in] attributeID The attribute ID of the attribute being changed + /// @param[in] value The new attribute value + /// @returns true if the message was sent successfully + bool send_change_attribute(std::uint16_t objectID, std::uint8_t attributeID, std::uint32_t value) const; + + /// @brief Sends the change attribute command (for float values) + /// @details This command is used to change a float attribute with an assigned Attribute ID. + /// This is most useful for changing output number scale factors. + /// @param[in] objectID The ID of the target object + /// @param[in] attributeID The attribute ID of the attribute being changed + /// @param[in] value The new attribute value + /// @returns true if the message was sent successfully + bool send_change_attribute(std::uint16_t objectID, std::uint8_t attributeID, float value) const; + + /// @brief Sends the change priority command + /// @details This command is used to change the priority of an Alarm Mask. + /// This command causes the VT to evaluate the priority of all active masks and + /// may cause a change to a different mask if the Alarm Mask being changed + /// should either become the active Working Set and mask, + /// or should no longer be the active Working Set and mask. + /// @param[in] alarmMaskObjectID The object ID of the target alarm mask + /// @param[in] priority The new priority for the mask + /// @returns true if the message was sent successfully + bool send_change_priority(std::uint16_t alarmMaskObjectID, AlarmMaskPriority priority) const; + + /// @brief Sends the change list item command + /// @details This command is used to change a list item in an Input List object, + /// Output List object, animation object, or external object definition object. + /// NULL_OBJECT_ID will result in the list item being removed, but will not change the index + /// order of the other list items. + /// @param[in] objectID The object ID of the list + /// @param[in] listIndex The index in the list to edit + /// @param[in] newObjectID The new object ID for the specified list index, or NULL_OBJECT_ID. + /// @returns true if the message was sent successfully + bool send_change_list_item(std::uint16_t objectID, std::uint8_t listIndex, std::uint16_t newObjectID) const; + + /// @brief Sends the lock unlock mask command + /// @details This command is used by a Working Set to disallow or allow + /// screen refreshes at the VT for the visible Data Mask or User Layout Data Mask + /// owned by the requesting Working Set. + /// This message would be used when a series of changes need to be synchronized or made visually atomic. + /// The mask may be unlocked if a a timeout occurs based on the timeout attribute of this message, or by + /// several other methods outlined in ISO11783-6, such as "proprietary reasons". + /// @param[in] state The target lock/unlock state + /// @param[in] objectID The object ID of the target mask + /// @param[in] timeout_ms The max time to lock the mask, or 0 for no timeout. Does not apply to unlock commands. + /// @returns true if the message was sent successfully + bool send_lock_unlock_mask(MaskLockState state, std::uint16_t objectID, std::uint16_t timeout_ms) const; + + /// @brief Sends the execute macro command + /// @details This command is used to execute a Macro. + /// @param[in] objectID The ID of the target object + /// @returns true if the message was sent successfully + bool send_execute_macro(std::uint16_t objectID) const; + + /// @brief Sends the change object label command + /// @details This command is used by an ECU to change a label of an object. + /// @param[in] objectID The ID of the target object + /// @param[in] labelStringObjectID The label's object ID + /// @param[in] fontType The font type or NULL_OBJECT_ID + /// @param[in] graphicalDesignatorObjectID Object ID of an object to be used as a graphic representation of the object label or NULL_OBJECT_ID + /// @returns true if the message was sent successfully + bool send_change_object_label(std::uint16_t objectID, std::uint16_t labelStringObjectID, std::uint8_t fontType, std::uint16_t graphicalDesignatorObjectID) const; + + /// @brief Sends change polygon point command + /// @details This command is used by a Working Set to modify a point in an Output Polygon object. + /// @param[in] objectID The ID of the target object + /// @param[in] pointIndex The index of the point in the polygon to edit + /// @param[in] newXValue The new X axis value (px) + /// @param[in] newYValue The new Y axis value (px) + /// @returns true if the message was sent successfully + bool send_change_polygon_point(std::uint16_t objectID, std::uint8_t pointIndex, std::uint16_t newXValue, std::uint16_t newYValue) const; + + /// @brief Sends the change polygon scale command + /// @details This command is used by a Working Set to change the scale of a complete Output Polygon object. This + /// message causes the value of the polygon points to be changed. + /// @param[in] objectID The ID of the target object + /// @param[in] widthAttribute New width attribute + /// @param[in] heightAttribute New height attribute + /// @returns true if the message was sent successfully + bool send_change_polygon_scale(std::uint16_t objectID, std::uint16_t widthAttribute, std::uint16_t heightAttribute) const; + + /// @brief Sends the select colour map or palette command + /// @param[in] objectID The object to select + /// @returns true if the message was sent successfully + bool send_select_colour_map_or_palette(std::uint16_t objectID) const; + + /// @brief Sends the execute extended macro command + /// @details Executes an extended macro + /// @param[in] objectID The object ID of the extended macro to execute + /// @returns true if the message was sent successfully + bool send_execute_extended_macro(std::uint16_t objectID) const; + + /// @brief Sends the select active working set command + /// @param[in] NAMEofWorkingSetMasterForDesiredWorkingSet The NAME of the target working set master + /// @returns true if the message was sent successfully + bool send_select_active_working_set(std::uint64_t NAMEofWorkingSetMasterForDesiredWorkingSet) const; + + // Graphics Context Commands: + /// @brief Sends the set graphics cursor command + /// @details This command sets the graphics cursor X/Y attributes of the object. + /// @param[in] objectID The ID of the target object + /// @param[in] xPosition The new X position (px) + /// @param[in] yPosition The new Y position (px) + /// @returns true if the message was sent successfully + bool send_set_graphics_cursor(std::uint16_t objectID, std::int16_t xPosition, std::int16_t yPosition) const; + + /// @brief Sends the move graphics cursor command + /// @details This command alters the graphics cursor x/y attributes of the object + /// by moving it relative to its current position. + /// @param[in] objectID The ID of the target object + /// @param[in] xOffset The new relative X offset of the cursor + /// @param[in] yOffset The new relative Y offset of the cursor + /// @returns true if the message was sent successfully + bool send_move_graphics_cursor(std::uint16_t objectID, std::int16_t xOffset, std::int16_t yOffset) const; + + /// @brief Sends the set foreground colour command + /// @details This command modifies the foreground colour + /// attribute.The graphics cursor is not moved. + /// @param[in] objectID The ID of the target object + /// @param[in] colour See standard colour palette, 0-255 + /// @returns true if the message was sent successfully + bool send_set_foreground_colour(std::uint16_t objectID, std::uint8_t colour) const; + + /// @brief Sends the set background colour command + /// @details This command modifies the background colour + /// attribute.The graphics cursor is not moved. + /// @param[in] objectID The ID of the target object + /// @param[in] colour See standard colour palette, 0-255 + /// @returns true if the message was sent successfully + bool send_set_background_colour(std::uint16_t objectID, std::uint8_t colour) const; + + /// @brief Sends the set line attributes object id + /// @details This command modifies the Output Line object + /// attribute. All drawing commands that follow use the new attribute value. + /// For line suppression, set the Object ID to NULL. + /// The graphics cursor is not moved. + /// @param[in] objectID The ID of the target object + /// @param[in] lineAttributeobjectID The object ID of the line attribute + /// @returns true if the message was sent successfully + bool send_set_line_attributes_object_id(std::uint16_t objectID, std::uint16_t lineAttributeobjectID) const; + + /// @brief Sends the fill attributes object id + /// @details This command modifies the fill object attribute. All + /// drawing commands that follow use the new attribute value. + /// For no filling, set the Object ID to NULL. The + /// graphics cursor is not moved. + /// @param[in] objectID The ID of the target object + /// @param[in] fillAttributeobjectID The object ID of the fill attribute + /// @returns true if the message was sent successfully + bool send_set_fill_attributes_object_id(std::uint16_t objectID, std::uint16_t fillAttributeobjectID) const; + + /// @brief Sends the set fill attributes object ID command + /// @details This command modifies the font object attribute. All + /// drawing commands that follow use the new attribute value. + /// If text is not being used, the object can be set to NULL. + /// The graphics cursor is not moved. + /// @param[in] objectID The ID of the target object + /// @param[in] fontAttributesObjectID The object ID of the font attribute + /// @returns true if the message was sent successfully + bool send_set_font_attributes_object_id(std::uint16_t objectID, std::uint16_t fontAttributesObjectID) const; + + /// @brief Sends the erase rectangle command + /// @details Fills the rectangle at the graphics cursor using the + /// current background colour.For this command, the Fill Attributes Object is + /// not used regardless of the state of Options bit 1 The graphics cursor is + /// moved to the bottom right pixel inside of the rectangle. + /// @param[in] objectID The ID of the target object + /// @param[in] width The width of the rectangle + /// @param[in] height The height of the rectangle + /// @returns true if the message was sent successfully + bool send_erase_rectangle(std::uint16_t objectID, std::uint16_t width, std::uint16_t height) const; + + /// @brief Sends the draw point command + /// @details Sets the pixel to the foreground colour. The graphics + /// cursor is moved to the defined point. + /// @param[in] objectID The ID of the target object + /// @param[in] xOffset The pixel X offset relative to the cursor + /// @param[in] yOffset The pixel Y offset relative to the cursor + /// @returns true if the message was sent successfully + bool send_draw_point(std::uint16_t objectID, std::int16_t xOffset, std::int16_t yOffset) const; + + /// @brief Sends the draw line command + /// @details Draws a line from the graphics cursor to the specified + /// end pixel using the foreground colour. The Output Line + /// Object drawing rules apply with respect to the end + /// pixel location and Line Attributes.The graphics cursor + /// is moved to the specified end pixel. + /// @param[in] objectID The ID of the target object + /// @param[in] xOffset The pixel X offset relative to the cursor + /// @param[in] yOffset The pixel Y offset relative to the cursor + /// @returns true if the message was sent successfully + bool send_draw_line(std::uint16_t objectID, std::int16_t xOffset, std::int16_t yOffset) const; + + /// @brief Sends the draw rectangle command + /// @details Draws a rectangle at the graphics cursor. The + /// Rectangle Object drawing rules apply.If a Line + /// Attributes object is currently defined, the border is + /// drawn. If a fill attribute object is currently defined, + /// the rectangle is filled.The graphics cursor is moved to the + /// bottom right pixel inside of the rectangle. + /// @param[in] objectID The ID of the target object + /// @param[in] width The width of the rectangle (px) + /// @param[in] height The height of the rectangle (px) + /// @returns true if the message was sent successfully + bool send_draw_rectangle(std::uint16_t objectID, std::uint16_t width, std::uint16_t height) const; + + /// @brief Sends the draw closed ellipse message + /// @details Draws a closed ellipse bounded by the rectangle + /// defined by the current graphics cursor location and the + /// width and height given.The Output Ellipse object + /// drawing rules apply.If a Line Attributes object is currently defined, + /// the border is drawn.If a fill attribute object is currently defined, + /// the ellipse is filled. The graphics cursor is moved to the bottom right pixel + /// inside of the bounding rectangle. + /// @param[in] objectID The ID of the target object + /// @param[in] width The width of the ellipse (px) + /// @param[in] height The height of the ellipse (px) + /// @returns true if the message was sent successfully + bool send_draw_closed_ellipse(std::uint16_t objectID, std::uint16_t width, std::uint16_t height) const; + + /// @brief Sends the draw polygon command + /// @details Draws a polygon from the graphics cursor to the first + /// point, then to the second point and so on. The polygon + /// is closed if the last point has the offset 0,0. This is + /// because offset 0,0 gives the coordinates of the original + /// graphics cursor which was used as the first point in the + /// polygon. If the data does not close the polygon, no + /// automatic closing is performed and filling is ignored. + /// Foreground colour is used for the border colour. The + /// Output Polygon object drawing rules apply. If a Line + /// Attributes object is currently defined, the border is + /// drawn. If a fill object is currently defined and the + /// polygon is closed, the polygon is filled. The graphics + /// cursor is moved to the last point in the list. + /// @param[in] objectID The ID of the target object + /// @param[in] numberOfPoints Number of points in the polygon + /// @param[in] listOfXOffsetsRelativeToCursor A list of X offsets for the points, relative to the cursor + /// @param[in] listOfYOffsetsRelativeToCursor A list of Y offsets for the points, relative to the cursor + /// @returns true if the message was sent successfully + bool send_draw_polygon(std::uint16_t objectID, std::uint8_t numberOfPoints, std::int16_t *listOfXOffsetsRelativeToCursor, std::int16_t *listOfYOffsetsRelativeToCursor) const; + + /// @brief Sends the draw text command + /// @details Draws the given text using the Font Attributes object. + /// Any flashing bits in the Font style of the Font Attributes + /// object are ignored If opaque, the background colour + /// attribute is used.The graphics cursor is moved to the + /// bottom right corner of the extent of the text. + /// @param[in] objectID The ID of the target object + /// @param[in] transparent Denotes if the text background is transparent + /// @param[in] textLength String length + /// @param[in] value A buffer to the text to draw with length `textLength` + /// @returns true if the message was sent successfully + bool send_draw_text(std::uint16_t objectID, bool transparent, std::uint8_t textLength, const char *value) const; + + /// @brief Sends the pan viewport command + /// @details This command modifies the viewport X and Y + /// attributes and forces a redraw of the object.This + /// allows �panning� of the underlying object contents. + /// The graphics cursor is not moved. + /// @param[in] objectID The ID of the target object + /// @param[in] xAttribute The viewport X attribute + /// @param[in] yAttribute The viewport Y attribute + /// @returns true if the message was sent successfully + bool send_pan_viewport(std::uint16_t objectID, std::int16_t xAttribute, std::int16_t yAttribute) const; + + /// @brief Sends the zoom viewport command + /// @details This command allows magnification of the viewport + /// contents. See section on zooming for meaning of the + /// zoom value. The graphics cursor is not moved. + /// @param[in] objectID The ID of the target object + /// @param[in] zoom Zoom value, -32.0 to 32.0 + /// @returns true if the message was sent successfully + bool send_zoom_viewport(std::uint16_t objectID, float zoom) const; + + /// @brief Sends the pan and zoom viewport command + /// @details This command allows both panning and zooming at the same time. + /// @param[in] objectID The ID of the target object + /// @param[in] xAttribute The viewport X attribute + /// @param[in] yAttribute The viewport Y attribute + /// @param[in] zoom Zoom value, -32.0 to 32.0 + /// @returns true if the message was sent successfully + bool send_pan_and_zoom_viewport(std::uint16_t objectID, std::int16_t xAttribute, std::int16_t yAttribute, float zoom) const; + + /// @brief Sends the change viewport size command + /// @details This command changes the size of the viewport and + /// can be compared to the normal Change Size + /// command.The graphics cursor is not moved. + /// @param[in] objectID The ID of the target object + /// @param[in] width The width of the viewport + /// @param[in] height The height of the viewport + /// @returns true if the message was sent successfully + bool send_change_viewport_size(std::uint16_t objectID, std::uint16_t width, std::uint16_t height) const; + + /// @brief Sends the draw VT object command + /// @details his command draws the VT Object specified by the Object ID + /// at the current graphics cursor location. + /// @param[in] graphicsContextObjectID The ID of the target graphics context object + /// @param[in] VTObjectID The object ID to draw + /// @returns true if the message was sent successfully + bool send_draw_vt_object(std::uint16_t graphicsContextObjectID, std::uint16_t VTObjectID) const; + + /// @brief Sends the copy canvas to picture graphic command + /// @details This command copies the current canvas of the + /// Graphics Context Object into the Picture Graphic object specified. + /// @param[in] graphicsContextObjectID The ID of the target graphics context object + /// @param[in] objectID The picture graphic's object ID to copy to + /// @returns true if the message was sent successfully + bool send_copy_canvas_to_picture_graphic(std::uint16_t graphicsContextObjectID, std::uint16_t objectID) const; + + /// @brief Sends the copy viewport to picture graphic command + /// @details This command copies the current Viewport of the GCO into the + /// specified picture graphic. + /// @param[in] graphicsContextObjectID The ID of the target graphics context object + /// @param[in] objectID The picture graphic's object ID to copy to + /// @returns true if the message was sent successfully + bool send_copy_viewport_to_picture_graphic(std::uint16_t graphicsContextObjectID, std::uint16_t objectID) const; + + // VT Querying + /// @brief Sends the get attribute value message + /// @param[in] objectID The object ID to query + /// @param[in] attributeID The attribute object to query + /// @returns true if the message was sent successfully + bool send_get_attribute_value(std::uint16_t objectID, std::uint8_t attributeID) const; + + // Get Softkeys Response + /// @brief Returns the number of X axis pixels in a softkey + /// @returns The number of X axis pixels in a softkey + std::uint8_t get_softkey_x_axis_pixels() const; + + /// @brief Returns the number of Y axis pixels in a softkey + /// @returns The number of Y axis pixels in a softkey + std::uint8_t get_softkey_y_axis_pixels() const; + + /// @brief Returns the number of virtual softkeys reported by the VT server + /// @returns The number of virtual softkeys reported by the VT server + std::uint8_t get_number_virtual_softkeys() const; + + /// @brief Returns the number of physical softkeys reported by the VT server + /// @returns The number of physical softkeys reported by the VT server + std::uint8_t get_number_physical_softkeys() const; + + // Get Text Font Data Response + /// @brief Returns if the selected font is supported + /// @param[in] value The font to check against + /// @returns true if the font is supported by the VT server + bool get_font_size_supported(FontSize value) const; + + /// @brief Returns if the selected font style is supported + /// @param[in] value The font style to check against + /// @returns true if the font style is supported by the VT server + bool get_font_style_supported(FontStyleBits value) const; + + // Get Hardware Responses + /// @brief Returns the graphics mode supported by the VT server + /// @returns The graphics mode supported by the VT server + GraphicMode get_graphic_mode() const; + + /// @brief Returns if the VT server supports a touchscreen with pointing message + /// @returns true if the VT server supports a touchscreen with pointing message + bool get_support_touchscreen_with_pointing_message() const; + + /// @brief Returns if the VT server supports a pointing device with pointing message + /// @returns true if the VT server supports a pointing device with pointing message + bool get_support_pointing_device_with_pointing_message() const; + + /// @brief Returns if the VT server supports multiple frequency audio output + /// @returns true if the VT server supports multiple frequency audio output + bool get_multiple_frequency_audio_output() const; + + /// @brief Returns if the VT server supports adjustable volume output + /// @returns true if the VT server supports adjustable volume output + bool get_has_adjustable_volume_output() const; + + /// @brief Returns if the VT server supports simultaneous activation of physical keys + /// @returns true if the VT server supports simultaneous activation of physical keys + bool get_support_simultaneous_activation_physical_keys() const; + + /// @brief Returns if the VT server supports simultaneous activation of buttons and softkeys + /// @returns true if the VT server supports simultaneous activation of buttons and softkeys + bool get_support_simultaneous_activation_buttons_and_softkeys() const; + + /// @brief Returns if the VT supports the drag operation + /// @returns true if the VT supports the drag operation + bool get_support_drag_operation() const; + + /// @brief Returns if the VT supports the intermediate coordinates during a drag operation + /// @returns true if the VT supports the intermediate coordinates during a drag operation + bool get_support_intermediate_coordinates_during_drag_operations() const; + + /// @brief Returns the number of x pixels in the data mask area + /// @returns the number of x pixels in the data mask area + std::uint16_t get_number_x_pixels() const; + + /// @brief Returns the number of y pixels in the data mask area + /// @returns the number of y pixels in the data mask area + std::uint16_t get_number_y_pixels() const; + + /// @brief Returns the VT version supported supported by the VT server + /// @returns The VT version supported supported by the VT server + VTVersion get_connected_vt_version() const; + + /// @brief Returns if the VT version is supported by the VT server + /// @param[in] value The VT version to check against + /// @returns true if the VT version is supported by the VT server + bool get_vt_version_supported(VTVersion value) const; + + /// @brief Returns the current data mask displayed by the VT server + /// @returns The object ID of the data mask visible + std::uint16_t get_visible_data_mask() const; + + /// @brief Returns the current soft key mask displayed by the VT server + /// @returns The object ID of the soft key mask visible + std::uint16_t get_visible_soft_key_mask() const; + + // ************************************************ + // Object Pool Interface + // ************************************************ + // These are the functions for specifying your pool to upload. + // You have a few options: + // 1. Upload in one blob of contigious memory + // This is good for small pools or pools where you have all the data in memory. + // 2. Get a callback at some inteval to provide data in chunks + // This is probably better for huge pools if you are RAM constrained, or if your + // pool is stored on some external device that you need to get data from in pages. + // This is also the best way to load from IOP files! + // If using callbacks, The object pool and pointer MUST NOT be deleted or leave scope until upload is done. + // Version must be the same for all pools uploaded to this VT server!!! + + /// @brief Assigns an object pool to the client using a buffer and size. + /// @details This is good for small pools or pools where you have all the data in memory. + /// @param[in] poolIndex The index of the pool you are assigning + /// @param[in] poolSupportedVTVersion The VT version of the object pool + /// @param[in] pool A pointer to the object pool. Must remain valid until client is connected! + /// @param[in] size The object pool size + /// @param[in] version An optional version string. The stack will automatically store/load your pool from the VT if this is provided. + void set_object_pool(std::uint8_t poolIndex, + VTVersion poolSupportedVTVersion, + const std::uint8_t *pool, + std::uint32_t size, + std::string version = ""); + + /// @brief Assigns an object pool to the client using a vector. + /// @details This is good for small pools or pools where you have all the data in memory. + /// @param[in] poolIndex The index of the pool you are assigning + /// @param[in] poolSupportedVTVersion The VT version of the object pool + /// @param[in] pool A pointer to the object pool. Must remain valid until client is connected! + /// @param[in] version An optional version string. The stack will automatically store/load your pool from the VT if this is provided. + void set_object_pool(std::uint8_t poolIndex, + VTVersion poolSupportedVTVersion, + const std::vector *pool, + std::string version = ""); + + /// @brief Configures an object pool to be automatically scaled to match the target VT server + /// @param[in] poolIndex The index of the pool you want to auto-scale + /// @param[in] originalDataMaskDimensions_px The data mask width that your object pool was originally designed for + /// @param[in] originalSoftKyeDesignatorHeight_px The soft key designator width that your object pool was originally designed for + void set_object_pool_scaling(std::uint8_t poolIndex, + std::uint32_t originalDataMaskDimensions_px, + std::uint32_t originalSoftKyeDesignatorHeight_px); + + /// @brief Assigns an object pool to the client where the client will get data in chunks during upload. + /// @details This is probably better for huge pools if you are RAM constrained, or if your + /// pool is stored on some external device that you need to get data from in pages. + /// This is also the best way to load from IOP files, as you can read the data in piece by piece. + /// @param[in] poolIndex The index of the pool you are assigning + /// @param[in] poolSupportedVTVersion The VT version of the object pool + /// @param[in] poolTotalSize The object pool size + /// @param[in] value The data callback that will be used to get object pool data to upload. + /// @param[in] version An optional version string. The stack will automatically store/load your pool from the VT if this is provided. + void register_object_pool_data_chunk_callback(std::uint8_t poolIndex, VTVersion poolSupportedVTVersion, std::uint32_t poolTotalSize, DataChunkCallback value, std::string version = ""); + + /// @brief Periodic Update Function (worker thread may call this) + /// @details This class can spawn a thread, or you can supply your own to run this function. + /// To configure that behavior, see the initialize function. + void update(); + + /// @brief Used to determine the language and unit systems in use by the VT server + LanguageCommandInterface languageCommandInterface; + + protected: + /// @brief Enumerates the multiplexor byte values for VT commands + enum class Function : std::uint8_t + { + SoftKeyActivationMessage = 0x00, + ButtonActivationMessage = 0x01, + PointingEventMessage = 0x02, + VTSelectInputObjectMessage = 0x03, + VTESCMessage = 0x04, + VTChangeNumericValueMessage = 0x05, + VTChangeActiveMaskMessage = 0x06, + VTChangeSoftKeyMaskMessage = 0x07, + VTChangeStringValueMessage = 0x08, + VTOnUserLayoutHideShowMessage = 0x09, + VTControlAudioSignalTerminationMessage = 0x0A, + ObjectPoolTransferMessage = 0x11, + EndOfObjectPoolMessage = 0x12, + AuxiliaryAssignmentTypeOneCommand = 0x20, + AuxiliaryInputTypeOneStatus = 0x21, + PreferredAssignmentCommand = 0x22, + AuxiliaryInputTypeTwoMaintenanceMessage = 0x23, + AuxiliaryAssignmentTypeTwoCommand = 0x24, + AuxiliaryInputStatusTypeTwoEnableCommand = 0x25, + AuxiliaryInputTypeTwoStatusMessage = 0x26, + AuxiliaryCapabilitiesRequest = 0x27, + SelectActiveWorkingSet = 0x90, + ESCCommand = 0x92, + HideShowObjectCommand = 0xA0, + EnableDisableObjectCommand = 0xA1, + SelectInputObjectCommand = 0xA2, + ControlAudioSignalCommand = 0xA3, + SetAudioVolumeCommand = 0xA4, + ChangeChildLocationCommand = 0xA5, + ChangeSizeCommand = 0xA6, + ChangeBackgroundColourCommand = 0xA7, + ChangeNumericValueCommand = 0xA8, + ChangeEndPointCommand = 0xA9, + ChangeFontAttributesCommand = 0xAA, + ChangeLineAttributesCommand = 0xAB, + ChangeFillAttributesCommand = 0xAC, + ChangeActiveMaskCommand = 0xAD, + ChangeSoftKeyMaskCommand = 0xAE, + ChangeAttributeCommand = 0xAF, + ChangePriorityCommand = 0xB0, + ChangeListItemCommand = 0xB1, + DeleteObjectPoolCommand = 0xB2, + ChangeStringValueCommand = 0xB3, + ChangeChildPositionCommand = 0xB4, + ChangeObjectLabelCommand = 0xB5, + ChangePolygonPointCommand = 0xB6, + ChangePolygonScaleCommand = 0xB7, + GraphicsContextCommand = 0xB8, + GetAttributeValueMessage = 0xB9, + SelectColourMapCommand = 0xBA, + IdentifyVTMessage = 0xBB, + ExecuteExtendedMacroCommand = 0xBC, + LockUnlockMaskCommand = 0xBD, + ExecuteMacroCommand = 0xBE, + GetMemoryMessage = 0xC0, + GetSupportedWidecharsMessage = 0xC1, + GetNumberOfSoftKeysMessage = 0xC2, + GetTextFontDataMessage = 0xC3, + GetWindowMaskDataMessage = 0xC4, + GetSupportedObjectsMessage = 0xC5, + GetHardwareMessage = 0xC7, + StoreVersionCommand = 0xD0, + LoadVersionCommand = 0xD1, + DeleteVersionCommand = 0xD2, + ExtendedGetVersionsMessage = 0xD3, + ExtendedStoreVersionCommand = 0xD4, + ExtendedLoadVersionCommand = 0xD5, + ExtendedDeleteVersionCommand = 0xD6, + GetVersionsMessage = 0xDF, + GetVersionsResponse = 0xE0, + UnsupportedVTFunctionMessage = 0xFD, + VTStatusMessage = 0xFE, + WorkingSetMaintenanceMessage = 0xFF + }; + + /// @brief Enumerates the command types for graphics context objects + enum class GraphicsContextSubCommandID : std::uint8_t + { + SetGraphicsCursor = 0x00, ///< Sets the graphics cursor x/y attributes + MoveGraphicsCursor = 0x01, ///< Moves the cursor relative to current location + SetForegroundColour = 0x02, ///< Sets the foreground colour + SetBackgroundColour = 0x03, ///< Sets the background colour + SetLineAttributesObjectID = 0x04, ///< Sets the line attribute object ID + SetFillAttributesObjectID = 0x05, ///< Sets the fill attribute object ID + SetFontAttributesObjectID = 0x06, ///< Sets the font attribute object ID + EraseRectangle = 0x07, ///< Erases a rectangle + DrawPoint = 0x08, ///< Draws a point + DrawLine = 0x09, ///< Draws a line + DrawRectangle = 0x0A, ///< Draws a rectangle + DrawClosedEllipse = 0x0B, ///< Draws a closed ellipse + DrawPolygon = 0x0C, ///< Draws polygon + DrawText = 0x0D, ///< Draws text + PanViewport = 0x0E, ///< Pans viewport + ZoomViewport = 0x0F, ///< Zooms the viewport + PanAndZoomViewport = 0x10, ///< Pan and zooms the viewport + ChangeViewportSize = 0x11, ///< Changes the viewport size + DrawVTObject = 0x12, ///< Draws a VT object + CopyCanvasToPictureGraphic = 0x13, ///< Copies the canvas to picture graphic object + CopyViewportToPictureGraphic = 0x14 ///< Copies the viewport to picture graphic object + }; + + /// @brief Flags used as a retry mechanism for sending important messages + enum class TransmitFlags : std::uint32_t + { + SendWorkingSetMaintenance = 0, ///< Flag to send the working set maintenenace message + SendAuxiliaryMaintenance = 1, ///< Flag to send the auxiliary maintenance message + + NumberFlags ///< The number of flags in this enum + }; + + /// @brief The different states of an object pool upload process + enum class CurrentObjectPoolUploadState : std::uint8_t + { + Uninitialized, ///< The object pool upload has not been started + InProgress, ///< The object pool upload is in progress + Success, ///< The object pool was uploaded + Failed ///< The pool upload has failed + }; + + /// @brief An object for storing information regarding an object pool upload + struct ObjectPoolDataStruct + { + const std::uint8_t *objectPoolDataPointer; ///< A pointer to an object pool + const std::vector *objectPoolVectorPointer; ///< A pointer to an object pool (vector format) + std::vector scaledObjectPool; ///< Stores a copy of a pool to auto-scale in RAM before uploading it + DataChunkCallback dataCallback; ///< A callback used to get data in chunks as an alternative to loading the whole pool at once + std::string versionLabel; ///< An optional version label that will be used to load/store the pool to the VT. 7 character max! + std::uint32_t objectPoolSize; ///< The size of the object pool + std::uint32_t autoScaleDataMaskOriginalDimension; ///< The original length or width of this object pool's data mask area (in pixels) + std::uint32_t autoScaleSoftKeyDesignatorOriginalHeight; ///< The original height of a soft key designator as designed in the pool (in pixels) + VTVersion version; ///< The version of the object pool. Must be the same for all pools! + bool useDataCallback; ///< Determines if the client will use callbacks to get the data in chunks. + bool uploaded; ///< The upload state of this pool + }; + + /// @brief A struct for storing information about an auxiliary input device + struct AssignedAuxiliaryInputDevice + { + const std::uint64_t name; ///< The NAME of the unit + const std::uint16_t modelIdentificationCode; ///< The model identification code + std::vector functions; ///< The functions assigned to this auxiliary input device (only applicable for listeners of input) + }; + + /// @brief Struct for storing the state of an auxiliary input on our device + struct AuxiliaryInputState + { + std::uint64_t lastStatusUpdate; ///< The time of the last status update, in milliseconds + bool enabled; ///< Whether the auxiliary input is enabled by the VT + bool hasInteraction; ///< Whether the auxiliary input is currently interacted with + bool controlLocked; ///< Whether the auxiliary input is currently locked + std::uint16_t value1; ///< The first value of the auxiliary input. See Table J.5 of Part 6 of the standard for details + std::uint16_t value2; ///< The second value of the auxiliary input. See Table J.5 of Part 6 of the standard for details + }; + + static constexpr std::uint64_t AUXILIARY_INPUT_STATUS_DELAY = 1000; ///< The delay between the auxiliary input status messages, in milliseconds + static constexpr std::uint64_t AUXILIARY_INPUT_STATUS_DELAY_INTERACTION = 50; ///< The delay between the auxiliary input status messages when the input is interacted with, in milliseconds + + // Object Pool Managment + /// @brief Sends the delete object pool message + /// @returns true if the message was sent + bool send_delete_object_pool() const; + + /// @brief Sends the working set maintenance message + /// @param[in] initializing Used to set the initializing bit + /// @param[in] workingSetVersion The version supported by the working set + /// @returns true if the message was sent + bool send_working_set_maintenance(bool initializing, VTVersion workingSetVersion) const; + + /// @brief Sends the get memory message + /// @details This message checks to see if the VT has enough memory available to store your object pool(s) + /// @param[in] requiredMemory Memory in bytes to check for on the VT server + /// @returns true if the message was sent + bool send_get_memory(std::uint32_t requiredMemory) const; + + /// @brief Sends the get number of softkeys message + /// @returns true if the message was sent + bool send_get_number_of_softkeys() const; + + /// @brief Sends the get text font data message + /// @returns true if the message was sent + bool send_get_text_font_data() const; + + /// @brief Sends the get hardware message + /// @returns true if the message was sent + bool send_get_hardware() const; + + /// @brief Sends the get supported widechars message + /// @returns true if the message was sent + bool send_get_supported_widechars() const; + + /// @brief Sends the get window mask data message + /// @returns true if the message was sent + bool send_get_window_mask_data() const; + + /// @brief Sends the get supported objects message + /// @returns true if the message was sent + bool send_get_supported_objects() const; + + /// @brief Sends the get versions message + /// @returns true if the message was sent + bool send_get_versions() const; + + /// @brief Sends the store version message + /// @param[in] versionLabel The version label to store + /// @returns true if the message was sent + bool send_store_version(std::array versionLabel) const; + + /// @brief Sends the load version message + /// @param[in] versionLabel The version label to load + /// @returns true if the message was sent + bool send_load_version(std::array versionLabel) const; + + /// @brief Sends the delete version message + /// @param[in] versionLabel The version label to delete + /// @returns true if the message was sent + bool send_delete_version(std::array versionLabel) const; + + /// @brief Sends the get extended versions message + /// @returns true if the message was sent + bool send_extended_get_versions() const; + + /// @brief Sends the extended store version message + /// @param[in] versionLabel The version label to store + /// @returns true if the message was sent + bool send_extended_store_version(std::array versionLabel) const; + + /// @brief Sends the extended load version message + /// @param[in] versionLabel The version label to load + /// @returns true if the message was sent + bool send_extended_load_version(std::array versionLabel) const; + + /// @brief Sends the extended delete version message + /// @param[in] versionLabel The version label to delete + /// @returns true if the message was sent + bool send_extended_delete_version(std::array versionLabel) const; + + /// @brief Sends the end of object pool message + /// @returns true if the message was sent + bool send_end_of_object_pool() const; + + /// @brief Sends the working set master message + /// @returns true if the message was sent + bool send_working_set_master() const; + + /// @brief Send the preferred auxiliary control type 2 assignment command + /// @returns true if the message was sent successfully + bool send_auxiliary_functions_preferred_assignment() const; + + /// @brief Send the auxiliary control type 2 assignment reponse message + /// @param[in] functionObjectID The object ID of the function + /// @param[in] hasError true if the assignment failed + /// @param[in] isAlreadyAssigned true if the function is already assigned + /// @returns true if the message was sent successfully + bool send_auxiliary_function_assignment_response(std::uint16_t functionObjectID, bool hasError, bool isAlreadyAssigned) const; + + /// @brief Send the auxiliary control type 2 maintenance message + /// @returns true if the message was sent successfully + bool send_auxiliary_input_maintenance() const; + + /// @brief Send the auxiliary input status type 2 enable response + /// @param[in] objectID The object ID of the input + /// @param[in] isEnabled true if the input is enabled + /// @param[in] hasError true if the enable failed + /// @returns true if the message was sent successfully + bool send_auxiliary_input_status_enable_response(std::uint16_t objectID, bool isEnabled, bool hasError) const; + + /// @brief Send the auxiliary control type 2 status message for all inputs if applicable + void update_auxiliary_input_status(); + + /// @brief Send the auxiliary control type 2 status message for a specific input if applicable + /// @param[in] objectID The object ID of the input + /// @returns true if the status message was sent + bool update_auxiliary_input_status(std::uint16_t objectID); + + /// @brief Sets the state machine state and updates the associated timestamp + /// @param[in] value The new state for the state machine + void set_state(StateMachineState value); + + /// @brief Processes the internal Tx flags + /// @param[in] flag The flag to process + /// @param[in] parent A context variable to find the relevant VT client class + static void process_flags(std::uint32_t flag, void *parent); + + /// @brief Processes a CAN message destined for any VT client + /// @param[in] message The CAN message being received + /// @param[in] parentPointer A context variable to find the relevant VT client class + static void process_rx_message(const CANMessage &message, void *parentPointer); + + /// @brief The callback passed to the network manager's send function to know when a Tx is completed + static void process_callback(std::uint32_t parameterGroupNumber, + std::uint32_t dataLength, + std::shared_ptr sourceControlFunction, + std::shared_ptr destinationControlFunction, + bool successful, + void *parentPointer); + + /// @brief The data callback passed to the network manger's send function for the transport layer messages + /// @details We upload the data with callbacks to avoid making a complete copy of the pool to + /// accommodate the multiplexor that needs to get passed to the transport layer message's first byte. + /// @param[in] callbackIndex The number of times the callback has been called + /// @param[in] bytesOffset The byte offset at which to get pool data + /// @param[in] numberOfBytesNeeded The number of bytes the protocol needs to send another frame (usually 7) + /// @param[out] chunkBuffer A pointer through which the data should be returned to the protocol + /// @param[in] parentPointer A context variable that is passed back through the callback + /// @returns true if the data was successfully returned via the callback + static bool process_internal_object_pool_upload_callback(std::uint32_t callbackIndex, + std::uint32_t bytesOffset, + std::uint32_t numberOfBytesNeeded, + std::uint8_t *chunkBuffer, + void *parentPointer); + + /// @brief Returns if any object pool had scaling configured + /// @returns true if any pool has both data mask and softkey scaling configured + bool get_any_pool_needs_scaling() const; + + /// @brief Iterates through each object pool and scales each object in the pool automatically + /// @returns true if all object pools scaled with no error + bool scale_object_pools(); + + /// @brief Returns if the specified object type can be scaled + /// @returns true if the object is inherently scalable + static bool get_is_object_scalable(VirtualTerminalObjectType type); + + /// @brief Returns the closest font to the one you passed in, in decending order + /// @param[in] originalFont The original font that you want to scale + /// @returns The best available font that the VT reported it has available + FontSize get_font_or_next_smallest_font(FontSize originalFont) const; + + /// @brief Remaps a font to some other font based on a scale factor + /// This is not a one-size-fits-all solution, but should usually result in a compatible font + /// @param[in] originalFont The font to scale + /// @param[in] scaleFactor The factor by which to attempt scaling the font + /// @returns A scaled font, depending on what the VT has available + static FontSize remap_font_to_scale(FontSize originalFont, float scaleFactor); + + /// @brief Returns the minimum length that the specified object could possibly require in bytes + /// @param[in] type The VT object type to check + /// @returns The minimum number of bytes that the specified object might use + static std::uint32_t get_minimum_object_length(VirtualTerminalObjectType type); + + /// @brief Returns the total number of bytes in the VT object located at the specified memory location + /// @param[in] buffer A pointer to the start of the VT object + /// @returns The total number of bytes present in the VT object at the specified location + static std::uint32_t get_number_bytes_in_object(std::uint8_t *buffer); + + /// @brief Resizes the most common VT object format by some scale factor + /// @param[in] buffer A pointer to the start of the VT object + /// @param[in] scaleFactor The scale factor to use when scaling the object, with 1.0 being the original scale + static void process_standard_object_height_and_width(std::uint8_t *buffer, float scaleFactor); + + /// @brief Resizes a single VT object by some scale factor + /// @param[in] buffer A pointer to the start of the VT object + /// @param[in] scaleFactor The scale factor to scale the object by + /// @param[in] type The type of the object to resize. Must match the object located at `buffer` + /// @returns true if the object was resized, otherwise false + bool resize_object(std::uint8_t *buffer, float scaleFactor, VirtualTerminalObjectType type); + + /// @brief The worker thread will execute this function when it runs, if applicable + void worker_thread_function(); + + static constexpr std::uint32_t VT_STATUS_TIMEOUT_MS = 3000; ///< The max allowable time between VT status messages before its considered offline + static constexpr std::uint32_t WORKING_SET_MAINTENANCE_TIMEOUT_MS = 1000; ///< The delay between working set maintenance messages + static constexpr std::uint32_t AUXILIARY_MAINTENANCE_TIMEOUT_MS = 100; ///< The delay between auxiliary maintenance messages + + std::shared_ptr partnerControlFunction; ///< The partner control function this client will send to + std::shared_ptr myControlFunction; ///< The internal control function the client uses to send from + + ProcessingFlags txFlags; ///< A retry mechanism for internal Tx messages + + // Status message contents from the VT + std::uint32_t lastVTStatusTimestamp_ms = 0; ///< The timestamp of the last VT status message + std::uint16_t activeWorkingSetDataMaskObjectID = NULL_OBJECT_ID; ///< The active working set data mask object ID + std::uint16_t activeWorkingSetSoftKeyMaskObjectID = NULL_OBJECT_ID; ///< The active working set's softkey mask object ID + std::uint8_t activeWorkingSetMasterAddress = NULL_CAN_ADDRESS; ///< The active working set master address + std::uint8_t busyCodesBitfield = 0; ///< The VT server's busy codes + std::uint8_t currentCommandFunctionCode = 0; ///< The VT server's current command function code + + std::uint8_t connectedVTVersion = 0; ///< The VT server's supported max version + + // Softkey capabilities + std::uint8_t softKeyXAxisPixels = 0; ///< The size of a soft key X dimension as reported by the VT server + std::uint8_t softKeyYAxisPixels = 0; ///< The size of a soft key Y dimension as reported by the VT server + std::uint8_t numberVirtualSoftkeysPerSoftkeyMask = 0; ///< The number of virtual softkeys per softkey mask as reported by the VT server + std::uint8_t numberPhysicalSoftkeys = 0; ///< The number of physical softkeys supported by the VT server + + // Text Font Capabilities + std::uint8_t smallFontSizesBitfield = 0; ///< The small font sizes supported by the VT server + std::uint8_t largeFontSizesBitfield = 0; ///< The large font sizes supported by the VT server + std::uint8_t fontStylesBitfield = 0; ///< The text font capabilities supported by the VT server + + // Hardware Capabilities, from the get hardware message + GraphicMode supportedGraphicsMode = GraphicMode::TwoHundredFiftySixColour; ///< The graphics mode reported by the VT server + std::uint16_t xPixels = 0; ///< The x pixel dimension as reported by the VT server + std::uint16_t yPixels = 0; ///< The y pixel dimension as reported by the VT server + std::uint8_t hardwareFeaturesBitfield = 0; ///< The reported hardware features from the VT server + + // Internal client state variables + StateMachineState state = StateMachineState::Disconnected; ///< The current client state machine state + CurrentObjectPoolUploadState currentObjectPoolState = CurrentObjectPoolUploadState::Uninitialized; ///< The current upload state of the object pool being processed + std::uint32_t stateMachineTimestamp_ms = 0; ///< Timestamp from the last state machine update + std::uint32_t lastWorkingSetMaintenanceTimestamp_ms = 0; ///< The timestamp from the last time we sent the maintenance message + std::uint32_t lastAuxiliaryMaintenanceTimestamp_ms = 0; ///< The timestamp from the last time we sent the maintenance message + std::vector objectPools; ///< A container to hold all object pools that have been assigned to the interface + std::vector assignedAuxiliaryInputDevices; ///< A container to hold all auxiliary input devices known + std::uint16_t ourModelIdentificationCode = 1; ///< The model identification code of this input device + std::map ourAuxiliaryInputs; ///< The inputs on this auxiliary input device +#if !defined CAN_STACK_DISABLE_THREADS && !defined ARDUINO + std::thread *workerThread = nullptr; ///< The worker thread that updates this interface +#endif + bool firstTimeInState = false; ///< Stores if the current update cycle is the first time a state machine state has been processed + bool initialized = false; ///< Stores the client initialization state + bool sendWorkingSetMaintenance = false; ///< Used internally to enable and disable cyclic sending of the working set maintenance message + bool sendAuxiliaryMaintenance = false; ///< Used internally to enable and disable cyclic sending of the auxiliary maintenance message + bool shouldTerminate = false; ///< Used to determine if the client should exit and join the worker thread + + // Activation event callbacks + EventDispatcher softKeyEventDispatcher; ///< A list of all soft key event callbacks + EventDispatcher buttonEventDispatcher; ///< A list of all button event callbacks + EventDispatcher pointingEventDispatcher; ///< A list of all pointing event callbacks + EventDispatcher selectInputObjectEventDispatcher; ///< A list of all select input object callbacks + EventDispatcher escMessageEventDispatcher; ///< A list of all ESC event callbacks + EventDispatcher changeNumericValueEventDispatcher; ///< A list of all change numeric value callbacks + EventDispatcher changeActiveMaskEventDispatcher; ///< A list of all change active mask callbacks + EventDispatcher changeSoftKeyMaskEventDispatcher; ///< A list of all change soft key mask callbacks + EventDispatcher changeStringValueEventDispatcher; ///< A list of all change string value callbacks + EventDispatcher userLayoutHideShowEventDispatcher; ///< A list of all user layout hide/show callbacks + EventDispatcher audioSignalTerminationEventDispatcher; ///< A list of all control audio signal termination callbacks + EventDispatcher auxiliaryFunctionEventDispatcher; ///< A list of all auxiliary function callbacks + + // Object Pool info + DataChunkCallback objectPoolDataCallback = nullptr; ///< The callback to use to get pool data + std::uint32_t lastObjectPoolIndex = 0; ///< The last object pool index that was processed + }; + +} // namespace isobus + +#endif // ISOBUS_VIRTUAL_TERMINAL_CLIENT_HPP diff --git a/src/isobus_virtual_terminal_objects.hpp b/src/isobus_virtual_terminal_objects.hpp new file mode 100644 index 0000000..9f2a1dd --- /dev/null +++ b/src/isobus_virtual_terminal_objects.hpp @@ -0,0 +1,81 @@ +//================================================================================================ +/// @file isobus_virtual_terminal_objects.hpp +/// +/// @brief Enumerates the different VT object types that can comprise a VT object pool +/// @author Adrian Del Grosso +/// +/// @copyright 2023 Adrian Del Grosso +//================================================================================================ +#ifndef ISOBUS_VIRTUAL_TERMINAL_OBJECTS_HPP +#define ISOBUS_VIRTUAL_TERMINAL_OBJECTS_HPP + +namespace isobus +{ + /// @brief The types of objects in an object pool by object type byte value + enum class VirtualTerminalObjectType + { + WorkingSet = 0, ///< Top level object that describes an implement’s ECU or group of ECUs + DataMask = 1, ///< Top level object that contains other objects. A Data Mask is activated by a Working Set to become the active set of objects on the VT display. + AlarmMask = 2, ///< Top level object that contains other objects. Describes an alarm display. + Container = 3, ///< Used to group objects. + WindowMask = 34, ///< Top level object that contains other objects. The Window Mask is activated by the VT. + SoftKeyMask = 4, ///< Top level object that contains Key objects. + Key = 5, ///< Used to describe a Soft Key. + Button = 6, ///< Used to describe a Button control. + KeyGroup = 35, ///< Top level object that contains Key objects. + InputBoolean = 7, ///< Used to input a TRUE/FALSE type input. + InputString = 8, ///< Used to input a character string + InputNumber = 9, ///< Used to input an integer or float numeric. + InputList = 10, ///< Used to select an item from a pre-defined list. + OutputString = 11, ///< Used to output a character string. + OutputNumber = 12, ///< Used to output an integer or float numeric. + OutputList = 37, ///< Used to output a list item. + OutputLine = 13, ///< Used to output a line. + OutputRectangle = 14, ///< Used to output a rectangle or square. + OutputEllipse = 15, ///< Used to output an ellipse or circle. + OutputPolygon = 16, ///< Used to output a polygon. + OutputMeter = 17, ///< Used to output a meter. + OutputLinearBarGraph = 18, ///< Used to output a linear bar graph. + OutputArchedBarGraph = 19, ///< Used to output an arched bar graph. + GraphicsContext = 36, ///< Used to output a graphics context. + Animation = 44, ///< The Animation object is used to display simple animations + PictureGraphic = 20, ///< Used to output a picture graphic (bitmap). + NumberVariable = 21, ///< Used to store a 32-bit unsigned integer value. + StringVariable = 22, ///< Used to store a fixed length string value. + FontAttributes = 23, ///< Used to group font based attributes. Can only be referenced by other objects. + LineAttributes = 24, ///< Used to group line based attributes. Can only be referenced by other objects. + FillAttributes = 25, ///< Used to group fill based attributes. Can only be referenced by other objects + InputAttributes = 26, ///< Used to specify a list of valid characters. Can only be referenced by input field objects. + ExtendedInputAttributes = 38, ///< Used to specify a list of valid WideChars. Can only be referenced by Input Field Objects. + ColourMap = 39, ///< Used to specify a colour table object. + ObjectLabelRefrenceList = 40, ///< Used to specify an object label. + ObjectPointer = 27, ///< Used to reference another object. + ExternalObjectDefinition = 41, ///< Used to list the objects that may be referenced from another Working Set + ExternalReferenceNAME = 42, ///< Used to identify the WS Master of a Working Set that can be referenced + ExternalObjectPointer = 43, ///< Used to reference an object in another Working Set + Macro = 28, ///< Special object that contains a list of commands that can be executed in response to an event. + AuxiliaryFunctionType1 = 29, ///< The Auxiliary Function Type 1 object defines the designator and function type for an Auxiliary Function. + AuxiliaryInputType1 = 30, ///< The Auxiliary Input Type 1 object defines the designator, key number, and function type for an auxiliary input. + AuxiliaryFunctionType2 = 31, ///< The Auxiliary Function Type 2 object defines the designator and function type for an Auxiliary Function. + AuxiliaryInputType2 = 32, ///< The Auxiliary Input Type 2 object defines the designator, key number, and function type for an Auxiliary Input. + AuxiliaryControlDesignatorType2 = 33, ///< Used to reference Auxiliary Input Type 2 object or Auxiliary Function Type 2 object. + ManufacturerDefined1 = 240, ///< Manufacturer defined objects should not be sent to any other Vendors VT + ManufacturerDefined2 = 241, ///< Manufacturer defined objects should not be sent to any other Vendors VT + ManufacturerDefined3 = 242, ///< Manufacturer defined objects should not be sent to any other Vendors VT + ManufacturerDefined4 = 243, ///< Manufacturer defined objects should not be sent to any other Vendors VT + ManufacturerDefined5 = 244, ///< Manufacturer defined objects should not be sent to any other Vendors VT + ManufacturerDefined6 = 245, ///< Manufacturer defined objects should not be sent to any other Vendors VT + ManufacturerDefined7 = 246, ///< Manufacturer defined objects should not be sent to any other Vendors VT + ManufacturerDefined8 = 247, ///< Manufacturer defined objects should not be sent to any other Vendors VT + ManufacturerDefined9 = 248, ///< Manufacturer defined objects should not be sent to any other Vendors VT + ManufacturerDefined10 = 249, ///< Manufacturer defined objects should not be sent to any other Vendors VT + ManufacturerDefined11 = 250, ///< Manufacturer defined objects should not be sent to any other Vendors VT + ManufacturerDefined12 = 251, ///< Manufacturer defined objects should not be sent to any other Vendors VT + ManufacturerDefined13 = 252, ///< Manufacturer defined objects should not be sent to any other Vendors VT + ManufacturerDefined14 = 253, ///< Manufacturer defined objects should not be sent to any other Vendors VT + ManufacturerDefined15 = 254, ///< Manufacturer defined objects should not be sent to any other Vendors VT + Reserved = 255 ///< Reserved for future use. (See Clause D.14 Get Supported Objects message) + }; +} // namespace isobus + +#endif // ISOBUS_VIRTUAL_TERMINAL_OBJECTS_HPP diff --git a/src/kinetis_flexcan.hpp b/src/kinetis_flexcan.hpp new file mode 100644 index 0000000..5ddb2a0 --- /dev/null +++ b/src/kinetis_flexcan.hpp @@ -0,0 +1,1141 @@ +/* + * File: kinetis_flexcan.h + * Purpose: Register and bit definitions + */ + +#ifndef __KINETIS_FLEXCAN_H +#define __KINETIS_FLEXCAN_H + +#include + +/* Common bit definition */ +#define BIT0 (1L) +#define BIT1 (BIT0 << 1) +#define BIT2 (BIT0 << 2) +#define BIT3 (BIT0 << 3) +#define BIT4 (BIT0 << 4) +#define BIT5 (BIT0 << 5) +#define BIT6 (BIT0 << 6) +#define BIT7 (BIT0 << 7) +#define BIT8 (BIT0 << 8) +#define BIT9 (BIT0 << 9) +#define BIT10 (BIT0 << 10) +#define BIT11 (BIT0 << 11) +#define BIT12 (0x00001000L) +#define BIT13 (0x00002000L) +#define BIT14 (0x00004000L) +#define BIT15 (0x00008000L) +#define BIT16 (0x00010000L) +#define BIT17 (0x00020000L) +#define BIT18 (0x00040000L) +#define BIT19 (0x00080000L) +#define BIT20 (0x00100000L) +#define BIT21 (0x00200000L) +#define BIT22 (0x00400000L) +#define BIT23 (0x00800000L) +#define BIT24 (0x01000000L) +#define BIT25 (0x02000000L) +#define BIT26 (0x04000000L) +#define BIT27 (0x08000000L) +#define BIT28 (0x10000000L) +#define BIT29 (0x20000000L) +#define BIT30 (0x40000000L) +#define BIT31 (0x80000000L) + +/* FlexCAN module I/O Base Addresss */ +#define FLEXCAN0_BASE (0x40024000L) +#define FLEXCAN1_BASE (0x400A4000L) + +typedef volatile uint32_t vuint32_t; + +/********************************************************************* +* +* FlexCAN0 (FLEXCAN0) +* +*********************************************************************/ + +/* Register read/write macros */ +#define FLEXCAN0_MCR (*(vuint32_t *)(FLEXCAN0_BASE)) +#define FLEXCAN0_CTRL1 (*(vuint32_t *)(FLEXCAN0_BASE + 4)) +#define FLEXCAN0_TIMER (*(vuint32_t *)(FLEXCAN0_BASE + 8)) +#define FLEXCAN0_TCR (*(vuint32_t *)(FLEXCAN0_BASE + 0x0C)) +#define FLEXCAN0_RXMGMASK (*(vuint32_t *)(FLEXCAN0_BASE + 0x10)) +#define FLEXCAN0_RX14MASK (*(vuint32_t *)(FLEXCAN0_BASE + 0x14)) +#define FLEXCAN0_RX15MASK (*(vuint32_t *)(FLEXCAN0_BASE + 0x18)) +#define FLEXCAN0_ECR (*(vuint32_t *)(FLEXCAN0_BASE + 0x1C)) +#define FLEXCAN0_ESR1 (*(vuint32_t *)(FLEXCAN0_BASE + 0x20)) +#define FLEXCAN0_IMASK2 (*(vuint32_t *)(FLEXCAN0_BASE + 0x24)) +#define FLEXCAN0_IMASK1 (*(vuint32_t *)(FLEXCAN0_BASE + 0x28)) +#define FLEXCAN0_IFLAG2 (*(vuint32_t *)(FLEXCAN0_BASE + 0x2C)) +#define FLEXCAN0_IFLAG1 (*(vuint32_t *)(FLEXCAN0_BASE + 0x30)) +#define FLEXCAN0_CTRL2 (*(vuint32_t *)(FLEXCAN0_BASE + 0x34)) +#define FLEXCAN0_ESR2 (*(vuint32_t *)(FLEXCAN0_BASE + 0x38)) +#define FLEXCAN0_FUREQ (*(vuint32_t *)(FLEXCAN0_BASE + 0x3C)) +#define FLEXCAN0_FUACK (*(vuint32_t *)(FLEXCAN0_BASE + 0x40)) +#define FLEXCAN0_CRCR (*(vuint32_t *)(FLEXCAN0_BASE + 0x44)) +#define FLEXCAN0_RXFGMASK (*(vuint32_t *)(FLEXCAN0_BASE + 0x48)) +#define FLEXCAN0_RXFIR (*(vuint32_t *)(FLEXCAN0_BASE + 0x4C)) +#define FLEXCAN0_DBG1 (*(vuint32_t *)(FLEXCAN0_BASE + 0x58)) +#define FLEXCAN0_DBG2 (*(vuint32_t *)(FLEXCAN0_BASE + 0x5C)) + +#define FLEXCAN0_IMEUR FLEXCAN0_FUREQ +#define FLEXCAN0_LRFR FLEXCAN0_FUACK + +/* Message Buffers */ +#define FLEXCAN0_MB0_CS (*(vuint32_t *)(FLEXCAN0_BASE + 0x80)) +#define FLEXCAN0_MB0_ID (*(vuint32_t *)(FLEXCAN0_BASE + 0x84)) +#define FLEXCAN0_MB0_WORD0 (*(vuint32_t *)(FLEXCAN0_BASE + 0x88)) +#define FLEXCAN0_MB0_WORD1 (*(vuint32_t *)(FLEXCAN0_BASE + 0x8C)) + +#define FLEXCAN0_MBn_CS(n) (*(vuint32_t *)(FLEXCAN0_BASE + 0x80 + n * 0x10)) +#define FLEXCAN0_MBn_ID(n) (*(vuint32_t *)(FLEXCAN0_BASE + 0x84 + n * 0x10)) +#define FLEXCAN0_MBn_WORD0(n) (*(vuint32_t *)(FLEXCAN0_BASE + 0x88 + n * 0x10)) +#define FLEXCAN0_MBn_WORD1(n) (*(vuint32_t *)(FLEXCAN0_BASE + 0x8C + n * 0x10)) + +/* Rx Individual Mask Registers */ +#define FLEXCAN0_RXIMR0 (*(vuint32_t *)(FLEXCAN0_BASE + 0x880)) +#define FLEXCAN0_RXIMRn(n) (*(vuint32_t *)(FLEXCAN0_BASE + 0x880 + n * 4)) + +/* Rx FIFO ID Filter Table Element 0 to 127 */ +#define FLEXCAN0_IDFLT_TAB0 (*(vuint32_t *)(FLEXCAN0_BASE + 0xE0)) +#define FLEXCAN0_IDFLT_TAB(n) (*(vuint32_t *)(FLEXCAN0_BASE + 0xE0 + (n * 4))) +//#define FLEXCAN0_IDFLT_TAB(n) (*(vuint32_t*)(FLEXCAN0_BASE+0xE0+(n<<2))) + +/* Memory Error Control Register */ +#define FLEXCAN0_MECR *(vuint32_t*)(FLEXCAN0_BASE+0x3B70)) + +/* Error Injection Address Register */ +#define FLEXCAN0_ERRIAR *(vuint32_t*)(FLEXCAN0_BASE+0x3B74)) + +/* Error Injection Data Pattern Register */ +#define FLEXCAN0_ERRIDPR *(vuint32_t*)(FLEXCAN0_BASE+0x3B78)) + +/* Error Injection Parity Pattern Register */ +#define FLEXCAN0_ERRIPPR *(vuint32_t*)(FLEXCAN0_BASE+0x3B7C)) + +/* Error Report Address Register */ +#define FLEXCAN0_RERRAR *(vuint32_t*)(FLEXCAN0_BASE+0x3B80)) + +/* Error Report Data Register */ +#define FLEXCAN0_RERRDR *(vuint32_t*)(FLEXCAN0_BASE+0x3B84)) + +/* Error Report Syndrome Register */ +#define FLEXCAN0_RERRSYNR *(vuint32_t*)(FLEXCAN0_BASE+0x3B88)) + +/* Error Status Register */ +#define FLEXCAN0_ERRSR *(vuint32_t*)(FLEXCAN0_BASE+0x3B8C)) + +/********************************************************************* +* +* FlexCAN1 (FLEXCAN1) +* +*********************************************************************/ +/* Register read/write macros */ +#define FLEXCAN1_MCR (*(vuint32_t *)(FLEXCAN1_BASE)) +#define FLEXCAN1_CTRL1 (*(vuint32_t *)(FLEXCAN1_BASE + 4)) +#define FLEXCAN1_TIMER (*(vuint32_t *)(FLEXCAN1_BASE + 8)) +#define FLEXCAN1_TCR (*(vuint32_t *)(FLEXCAN1_BASE + 0x0C)) +#define FLEXCAN1_RXMGMASK (*(vuint32_t *)(FLEXCAN1_BASE + 0x10)) +#define FLEXCAN1_RX14MASK (*(vuint32_t *)(FLEXCAN1_BASE + 0x14)) +#define FLEXCAN1_RX15MASK (*(vuint32_t *)(FLEXCAN1_BASE + 0x18)) +#define FLEXCAN1_ECR (*(vuint32_t *)(FLEXCAN1_BASE + 0x1C)) +#define FLEXCAN1_ESR1 (*(vuint32_t *)(FLEXCAN1_BASE + 0x20)) +#define FLEXCAN1_IMASK2 (*(vuint32_t *)(FLEXCAN1_BASE + 0x24)) +#define FLEXCAN1_IMASK1 (*(vuint32_t *)(FLEXCAN1_BASE + 0x28)) +#define FLEXCAN1_IFLAG2 (*(vuint32_t *)(FLEXCAN1_BASE + 0x2C)) +#define FLEXCAN1_IFLAG1 (*(vuint32_t *)(FLEXCAN1_BASE + 0x30)) +#define FLEXCAN1_CTRL2 (*(vuint32_t *)(FLEXCAN1_BASE + 0x34)) +#define FLEXCAN1_ESR2 (*(vuint32_t *)(FLEXCAN1_BASE + 0x38)) +#define FLEXCAN1_FUREQ (*(vuint32_t *)(FLEXCAN1_BASE + 0x3C)) +#define FLEXCAN1_FUACK (*(vuint32_t *)(FLEXCAN1_BASE + 0x40)) +#define FLEXCAN1_CRCR (*(vuint32_t *)(FLEXCAN1_BASE + 0x44)) +#define FLEXCAN1_RXFGMASK (*(vuint32_t *)(FLEXCAN1_BASE + 0x48)) +#define FLEXCAN1_RXFIR (*(vuint32_t *)(FLEXCAN1_BASE + 0x4C)) +#define FLEXCAN1_DBG1 (*(vuint32_t *)(FLEXCAN1_BASE + 0x58)) +#define FLEXCAN1_DBG2 (*(vuint32_t *)(FLEXCAN1_BASE + 0x5C)) + +#define FLEXCAN1_IMEUR FLEXCAN1_FUREQ +#define FLEXCAN1_LRFR FLEXCAN1_FUACK + +/* Message Buffers */ +#define FLEXCAN1_MB0_CS (*(vuint32_t *)(FLEXCAN1_BASE + 0x80)) +#define FLEXCAN1_MB0_ID (*(vuint32_t *)(FLEXCAN1_BASE + 0x84)) +#define FLEXCAN1_MB0_WORD0 (*(vuint32_t *)(FLEXCAN1_BASE + 0x88)) +#define FLEXCAN1_MB0_WORD1 (*(vuint32_t *)(FLEXCAN1_BASE + 0x8C)) + +#define FLEXCAN1_MBn_CS(n) (*(vuint32_t *)(FLEXCAN1_BASE + 0x80 + n * 0x10)) +#define FLEXCAN1_MBn_ID(n) (*(vuint32_t *)(FLEXCAN1_BASE + 0x84 + n * 0x10)) +#define FLEXCAN1_MBn_WORD0(n) (*(vuint32_t *)(FLEXCAN1_BASE + 0x88 + n * 0x10)) +#define FLEXCAN1_MBn_WORD1(n) (*(vuint32_t *)(FLEXCAN1_BASE + 0x8C + n * 0x10)) + +/* Rx Individual Mask Registers */ +#define FLEXCAN1_RXIMR0 (*(vuint32_t *)(FLEXCAN1_BASE + 0x880)) +#define FLEXCAN1_RXIMRn(n) (*(vuint32_t *)(FLEXCAN1_BASE + 0x880 + n * 4)) + +/* Rx FIFO ID Filter Table Element 0 to 127 */ +#define FLEXCAN1_IDFLT_TAB0 (*(vuint32_t *)(FLEXCAN1_BASE + 0xE0)) +#define FLEXCAN1_IDFLT_TAB(n) (*(vuint32_t *)(FLEXCAN1_BASE + 0xE0 + (n << 2))) + +/* Memory Error Control Register */ +#define FLEXCAN1_MECR *(vuint32_t*)(FLEXCAN1_BASE+0x7B70)) + +/* Error Injection Address Register */ +#define FLEXCAN1_ERRIAR *(vuint32_t*)(FLEXCAN1_BASE+0x3B74)) + +/* Error Injection Data Pattern Register */ +#define FLEXCAN1_ERRIDPR *(vuint32_t*)(FLEXCAN1_BASE+0x3B78)) + +/* Error Injection Parity Pattern Register */ +#define FLEXCAN1_ERRIPPR *(vuint32_t*)(FLEXCAN1_BASE+0x3B7C)) + +/* Error Report Address Register */ +#define FLEXCAN1_RERRAR *(vuint32_t*)(FLEXCAN1_BASE+0x3B80)) + +/* Error Report Data Register */ +#define FLEXCAN1_RERRDR *(vuint32_t*)(FLEXCAN1_BASE+0x3B84)) + +/* Error Report Syndrome Register */ +#define FLEXCAN1_RERRSYNR *(vuint32_t*)(FLEXCAN1_BASE+0x3B88)) + +/* Error Status Register */ +#define FLEXCAN1_ERRSR *(vuint32_t*)(FLEXCAN1_BASE+0x3B8C)) + +/* Bit definitions and macros for FLEXCAN_MCR */ +#define FLEXCAN_MCR_MAXMB(x) (((x)&0x0000007F) << 0) +#define FLEXCAN_MCR_IDAM(x) (((x)&0x00000003) << 8) +#define FLEXCAN_MCR_MAXMB_MASK (0x0000007F) +#define FLEXCAN_MCR_IDAM_MASK (0x00000300) +#define FLEXCAN_MCR_IDAM_BIT_NO (8) +#define FLEXCAN_MCR_AEN (0x00001000) +#define FLEXCAN_MCR_LPRIO_EN (0x00002000) +#define FLEXCAN_MCR_IRMQ (0x00010000) +#define FLEXCAN_MCR_SRX_DIS (0x00020000) +#define FLEXCAN_MCR_DOZE (0x00040000) +#define FLEXCAN_MCR_WAK_SRC (0x00080000) +#define FLEXCAN_MCR_LPM_ACK (0x00100000) +#define FLEXCAN_MCR_WRN_EN (0x00200000) +#define FLEXCAN_MCR_SLF_WAK (0x00400000) +#define FLEXCAN_MCR_SUPV (0x00800000) +#define FLEXCAN_MCR_FRZ_ACK (0x01000000) +#define FLEXCAN_MCR_SOFT_RST (0x02000000) +#define FLEXCAN_MCR_WAK_MSK (0x04000000) +#define FLEXCAN_MCR_NOT_RDY (0x08000000) +#define FLEXCAN_MCR_HALT (0x10000000) +#define FLEXCAN_MCR_FEN (0x20000000) +#define FLEXCAN_MCR_FRZ (0x40000000) +#define FLEXCAN_MCR_MDIS (0x80000000) + +/* Bit definitions and macros for FLEXCAN_CTRL */ +#define FLEXCAN_CTRL_PROPSEG(x) (((x)&0x00000007L) << 0) +#define FLEXCAN_CTRL_LOM (0x00000008) +#define FLEXCAN_CTRL_LBUF (0x00000010) +#define FLEXCAN_CTRL_TSYNC (0x00000020) +#define FLEXCAN_CTRL_BOFF_REC (0x00000040) +#define FLEXCAN_CTRL_SMP (0x00000080) +#define FLEXCAN_CTRL_RWRN_MSK (0x00000400) +#define FLEXCAN_CTRL_TWRN_MSK (0x00000800) +#define FLEXCAN_CTRL_LPB (0x00001000L) +#define FLEXCAN_CTRL_CLK_SRC (0x00002000) +#define FLEXCAN_CTRL_ERR_MSK (0x00004000) +#define FLEXCAN_CTRL_BOFF_MSK (0x00008000) +#define FLEXCAN_CTRL_PSEG2(x) (((x)&0x00000007L) << 16) +#define FLEXCAN_CTRL_PSEG1(x) (((x)&0x00000007L) << 19) +#define FLEXCAN_CTRL_RJW(x) (((x)&0x00000003L) << 22) +#define FLEXCAN_CTRL_PRESDIV(x) (((x)&0x000000FFL) << 24) + +/* Bit definitions and macros for FLEXCAN_CTRL2 */ +#define FLEXCAN_CTRL2_IMEUEN (BIT31) +#define FLEXCAN_CTRL2_RFFN (0x0F000000L) +#define FLEXCAN_CTRL2_RFFN_BIT_NO (24) +#define FLEXCAN_CTRL2_TASD (0x00F80000L) +#define FLEXCAN_CTRL2_TASD_BIT_NO (19) +#define FLEXCAN_CTRL2_MRP (BIT18) +#define FLEXCAN_CTRL2_RRS (BIT17) +#define FLEXCAN_CTRL2_EACEN (BIT16) +#define FLEXCAN_CTRL2_MUMASK (BIT1) +#define FLEXCAN_CTRL2_FUMASK (BIT0) +#define FLEXCAN_CTRL2_LOSTRLMSK (BIT2) +#define FLEXCAN_CTRL2_LOSTRMMSK (BIT1) +#define FLEXCAN_CTRL2_IMEUMASK (BIT0) +#define FLEXCAN_set_rffn(ctrl2, rffn) ctrl2 = ((ctrl2) & ~FLEXCAN_CTRL2_RFFN) | ((rffn & 0xF) << FLEXCAN_CTRL2_RFFN_BIT_NO) + +/* Bit definitions and macros for FLEXCAN_TIMER */ +#define FLEXCAN_TIMER_TIMER(x) (((x)&0x0000FFFF) << 0) + +/* Bit definitions and macros for FLEXCAN_TCR */ +#define FLEXCAN_TCR_DSCACK (0x00000100) +#define FLEXCAN_TCR_BIT_CLS (0x00000200) +#define FLEXCAN_TCR_TRD (0x00000400) + +/* Bit definitions and macros for FLEXCAN_RXGMASK */ +#define FLEXCAN_RXGMASK_MI0 (0x00000001) +#define FLEXCAN_RXGMASK_MI1 (0x00000002) +#define FLEXCAN_RXGMASK_MI2 (0x00000004) +#define FLEXCAN_RXGMASK_MI3 (0x00000008) +#define FLEXCAN_RXGMASK_MI4 (0x00000010) +#define FLEXCAN_RXGMASK_MI5 (0x00000020) +#define FLEXCAN_RXGMASK_MI6 (0x00000040) +#define FLEXCAN_RXGMASK_MI7 (0x00000080) +#define FLEXCAN_RXGMASK_MI8 (0x00000100) +#define FLEXCAN_RXGMASK_MI9 (0x00000200) +#define FLEXCAN_RXGMASK_MI10 (0x00000400) +#define FLEXCAN_RXGMASK_MI11 (0x00000800) +#define FLEXCAN_RXGMASK_MI12 (0x00001000) +#define FLEXCAN_RXGMASK_MI13 (0x00002000) +#define FLEXCAN_RXGMASK_MI14 (0x00004000) +#define FLEXCAN_RXGMASK_MI15 (0x00008000) +#define FLEXCAN_RXGMASK_MI16 (0x00010000) +#define FLEXCAN_RXGMASK_MI17 (0x00020000) +#define FLEXCAN_RXGMASK_MI18 (0x00040000) +#define FLEXCAN_RXGMASK_MI19 (0x00080000) +#define FLEXCAN_RXGMASK_MI20 (0x00100000) +#define FLEXCAN_RXGMASK_MI21 (0x00200000) +#define FLEXCAN_RXGMASK_MI22 (0x00400000) +#define FLEXCAN_RXGMASK_MI23 (0x00800000) +#define FLEXCAN_RXGMASK_MI24 (0x01000000) +#define FLEXCAN_RXGMASK_MI25 (0x02000000) +#define FLEXCAN_RXGMASK_MI26 (0x04000000) +#define FLEXCAN_RXGMASK_MI27 (0x08000000) +#define FLEXCAN_RXGMASK_MI28 (0x10000000) +#define FLEXCAN_RXGMASK_MI29 (0x20000000) +#define FLEXCAN_RXGMASK_MI30 (0x40000000) +#define FLEXCAN_RXGMASK_MI31 (0x80000000) + +/* Bit definitions and macros for FLEXCAN_RX14MASK */ +#define FLEXCAN_RX14MASK_MI0 (0x00000001) +#define FLEXCAN_RX14MASK_MI1 (0x00000002) +#define FLEXCAN_RX14MASK_MI2 (0x00000004) +#define FLEXCAN_RX14MASK_MI3 (0x00000008) +#define FLEXCAN_RX14MASK_MI4 (0x00000010) +#define FLEXCAN_RX14MASK_MI5 (0x00000020) +#define FLEXCAN_RX14MASK_MI6 (0x00000040) +#define FLEXCAN_RX14MASK_MI7 (0x00000080) +#define FLEXCAN_RX14MASK_MI8 (0x00000100) +#define FLEXCAN_RX14MASK_MI9 (0x00000200) +#define FLEXCAN_RX14MASK_MI10 (0x00000400) +#define FLEXCAN_RX14MASK_MI11 (0x00000800) +#define FLEXCAN_RX14MASK_MI12 (0x00001000) +#define FLEXCAN_RX14MASK_MI13 (0x00002000) +#define FLEXCAN_RX14MASK_MI14 (0x00004000) +#define FLEXCAN_RX14MASK_MI15 (0x00008000) +#define FLEXCAN_RX14MASK_MI16 (0x00010000) +#define FLEXCAN_RX14MASK_MI17 (0x00020000) +#define FLEXCAN_RX14MASK_MI18 (0x00040000) +#define FLEXCAN_RX14MASK_MI19 (0x00080000) +#define FLEXCAN_RX14MASK_MI20 (0x00100000) +#define FLEXCAN_RX14MASK_MI21 (0x00200000) +#define FLEXCAN_RX14MASK_MI22 (0x00400000) +#define FLEXCAN_RX14MASK_MI23 (0x00800000) +#define FLEXCAN_RX14MASK_MI24 (0x01000000) +#define FLEXCAN_RX14MASK_MI25 (0x02000000) +#define FLEXCAN_RX14MASK_MI26 (0x04000000) +#define FLEXCAN_RX14MASK_MI27 (0x08000000) +#define FLEXCAN_RX14MASK_MI28 (0x10000000) +#define FLEXCAN_RX14MASK_MI29 (0x20000000) +#define FLEXCAN_RX14MASK_MI30 (0x40000000) +#define FLEXCAN_RX14MASK_MI31 (0x80000000) + +/* Bit definitions and macros for FLEXCAN_RX15MASK */ +#define FLEXCAN_RX15MASK_MI0 (0x00000001) +#define FLEXCAN_RX15MASK_MI1 (0x00000002) +#define FLEXCAN_RX15MASK_MI2 (0x00000004) +#define FLEXCAN_RX15MASK_MI3 (0x00000008) +#define FLEXCAN_RX15MASK_MI4 (0x00000010) +#define FLEXCAN_RX15MASK_MI5 (0x00000020) +#define FLEXCAN_RX15MASK_MI6 (0x00000040) +#define FLEXCAN_RX15MASK_MI7 (0x00000080) +#define FLEXCAN_RX15MASK_MI8 (0x00000100) +#define FLEXCAN_RX15MASK_MI9 (0x00000200) +#define FLEXCAN_RX15MASK_MI10 (0x00000400) +#define FLEXCAN_RX15MASK_MI11 (0x00000800) +#define FLEXCAN_RX15MASK_MI12 (0x00001000) +#define FLEXCAN_RX15MASK_MI13 (0x00002000) +#define FLEXCAN_RX15MASK_MI14 (0x00004000) +#define FLEXCAN_RX15MASK_MI15 (0x00008000) +#define FLEXCAN_RX15MASK_MI16 (0x00010000) +#define FLEXCAN_RX15MASK_MI17 (0x00020000) +#define FLEXCAN_RX15MASK_MI18 (0x00040000) +#define FLEXCAN_RX15MASK_MI19 (0x00080000) +#define FLEXCAN_RX15MASK_MI20 (0x00100000) +#define FLEXCAN_RX15MASK_MI21 (0x00200000) +#define FLEXCAN_RX15MASK_MI22 (0x00400000) +#define FLEXCAN_RX15MASK_MI23 (0x00800000) +#define FLEXCAN_RX15MASK_MI24 (0x01000000) +#define FLEXCAN_RX15MASK_MI25 (0x02000000) +#define FLEXCAN_RX15MASK_MI26 (0x04000000) +#define FLEXCAN_RX15MASK_MI27 (0x08000000) +#define FLEXCAN_RX15MASK_MI28 (0x10000000) +#define FLEXCAN_RX15MASK_MI29 (0x20000000) +#define FLEXCAN_RX15MASK_MI30 (0x40000000) +#define FLEXCAN_RX15MASK_MI31 (0x80000000) + +/* Bit definitions and macros for FLEXCAN_ECR */ +#define FLEXCAN_ECR_TX_ERR_COUNTER(x) (((x)&0x000000FF) << 0) +#define FLEXCAN_ECR_RX_ERR_COUNTER(x) (((x)&0x000000FF) << 8) + +/* Bit definitions and macros for FLEXCAN_ESR1 */ +#define FLEXCAN_ESR_WAK_INT (0x00000001) +#define FLEXCAN_ESR_ERR_INT (0x00000002) +#define FLEXCAN_ESR_BOFF_INT (0x00000004) +#define FLEXCAN_ESR_RX (0x00000008) +#define FLEXCAN_ESR_FLT_CONF(x) (((x)&0x00000003) << 4) +#define FLEXCAN_ESR_FLT_CONF_MASK (0x00000030) +#define FLEXCAN_ESR_TX (0x00000040) +#define FLEXCAN_ESR_IDLE (0x00000080) +#define FLEXCAN_ESR_RX_WRN (0x00000100) +#define FLEXCAN_ESR_TX_WRN (0x00000200) +#define FLEXCAN_ESR_STF_ERR (0x00000400) +#define FLEXCAN_ESR_FRM_ERR (0x00000800) +#define FLEXCAN_ESR_CRC_ERR (0x00001000) +#define FLEXCAN_ESR_ACK_ERR (0x00002000) +#define FLEXCAN_ESR_BIT0_ERR (0x00004000) +#define FLEXCAN_ESR_BIT1_ERR (0x00008000) +#define FLEXCAN_ESR_RWRN_INT (0x00010000) +#define FLEXCAN_ESR_TWRN_INT (0x00020000) +#define FLEXCAN_ESR_get_fault_code(esr) (((esr)&FLEXCAN_ESR_FLT_CONF_MASK) >> 4) +#define CAN_ERROR_ACTIVE 0 +#define CAN_ERROR_PASSIVE 1 +#define CAN_ERROR_BUS_OFF 2 + +/* Bit definition for FLEXCAN_ESR2 */ +#define FLEXCAN_ESR2_IMB (0x00002000) +#define FLEXCAN_ESR2_VPS (0x00004000) +#define FLEXCAN_ESR2_LTM (0x007F0000L) +#define FLEXCAN_ESR2_LTM_BIT_NO (16) +#define FLEXCAN_ESR2_LOSTRLF (0x00000004) +#define FLEXCAN_ESR2_LOSTRMF (0x00000002) +#define FLEXCAN_ESR2_IMEUF (0x00000001) +#define FLEXCAN_get_LTM(esr2_value) (((esr2_value) & (FLEXCAN_ESR2_LTM)) >> (FLEXCAN_ESR2_LTM_BIT_NO)) + +/* Bit definitions and macros for FLEXCAN_IMASK1 */ +#define FLEXCAN_IMASK1_BUF0M (0x00000001) +#define FLEXCAN_IMASK1_BUF1M (0x00000002) +#define FLEXCAN_IMASK1_BUF2M (0x00000004) +#define FLEXCAN_IMASK1_BUF3M (0x00000008) +#define FLEXCAN_IMASK1_BUF4M (0x00000010) +#define FLEXCAN_IMASK1_BUF5M (0x00000020) +#define FLEXCAN_IMASK1_BUF6M (0x00000040) +#define FLEXCAN_IMASK1_BUF7M (0x00000080) +#define FLEXCAN_IMASK1_BUF8M (0x00000100) +#define FLEXCAN_IMASK1_BUF9M (0x00000200) +#define FLEXCAN_IMASK1_BUF10M (0x00000400) +#define FLEXCAN_IMASK1_BUF11M (0x00000800) +#define FLEXCAN_IMASK1_BUF12M (0x00001000) +#define FLEXCAN_IMASK1_BUF13M (0x00002000) +#define FLEXCAN_IMASK1_BUF14M (0x00004000) +#define FLEXCAN_IMASK1_BUF15M (0x00008000) +#define FLEXCAN_IMASK1_BUF16M (0x00010000) +#define FLEXCAN_IMASK1_BUF17M (0x00020000) +#define FLEXCAN_IMASK1_BUF18M (0x00040000) +#define FLEXCAN_IMASK1_BUF19M (0x00080000) +#define FLEXCAN_IMASK1_BUF20M (0x00100000) +#define FLEXCAN_IMASK1_BUF21M (0x00200000) +#define FLEXCAN_IMASK1_BUF22M (0x00400000) +#define FLEXCAN_IMASK1_BUF23M (0x00800000) +#define FLEXCAN_IMASK1_BUF24M (0x01000000) +#define FLEXCAN_IMASK1_BUF25M (0x02000000) +#define FLEXCAN_IMASK1_BUF26M (0x04000000) +#define FLEXCAN_IMASK1_BUF27M (0x08000000) +#define FLEXCAN_IMASK1_BUF28M (0x10000000) +#define FLEXCAN_IMASK1_BUF29M (0x20000000) +#define FLEXCAN_IMASK1_BUF30M (0x40000000) +#define FLEXCAN_IMASK1_BUF31M (0x80000000) + +/* Bit definitions and macros for FLEXCAN_IFLAG1 */ +#define FLEXCAN_IFLAG1_BUF0I (0x00000001) +#define FLEXCAN_IFLAG1_BUF1I (0x00000002) +#define FLEXCAN_IFLAG1_BUF2I (0x00000004) +#define FLEXCAN_IFLAG1_BUF3I (0x00000008) +#define FLEXCAN_IFLAG1_BUF4I (0x00000010) +#define FLEXCAN_IFLAG1_BUF5I (0x00000020) +#define FLEXCAN_IFLAG1_BUF6I (0x00000040) +#define FLEXCAN_IFLAG1_BUF7I (0x00000080) +#define FLEXCAN_IFLAG1_BUF8I (0x00000100) +#define FLEXCAN_IFLAG1_BUF9I (0x00000200) +#define FLEXCAN_IFLAG1_BUF10I (0x00000400) +#define FLEXCAN_IFLAG1_BUF11I (0x00000800) +#define FLEXCAN_IFLAG1_BUF12I (0x00001000) +#define FLEXCAN_IFLAG1_BUF13I (0x00002000) +#define FLEXCAN_IFLAG1_BUF14I (0x00004000) +#define FLEXCAN_IFLAG1_BUF15I (0x00008000) +#define FLEXCAN_IFLAG1_BUF16I (0x00010000) +#define FLEXCAN_IFLAG1_BUF17I (0x00020000) +#define FLEXCAN_IFLAG1_BUF18I (0x00040000) +#define FLEXCAN_IFLAG1_BUF19I (0x00080000) +#define FLEXCAN_IFLAG1_BUF20I (0x00100000) +#define FLEXCAN_IFLAG1_BUF21I (0x00200000) +#define FLEXCAN_IFLAG1_BUF22I (0x00400000) +#define FLEXCAN_IFLAG1_BUF23I (0x00800000) +#define FLEXCAN_IFLAG1_BUF24I (0x01000000) +#define FLEXCAN_IFLAG1_BUF25I (0x02000000) +#define FLEXCAN_IFLAG1_BUF26I (0x04000000) +#define FLEXCAN_IFLAG1_BUF27I (0x08000000) +#define FLEXCAN_IFLAG1_BUF28I (0x10000000) +#define FLEXCAN_IFLAG1_BUF29I (0x20000000) +#define FLEXCAN_IFLAG1_BUF30I (0x40000000) +#define FLEXCAN_IFLAG1_BUF31I (0x80000000) + +/* Bit definitions and macros for FLEXCAN_MB_CS */ +#define FLEXCAN_MB_CS_TIMESTAMP(x) (((x)&0x0000FFFF) << 0) +#define FLEXCAN_MB_CS_TIMESTAMP_MASK (0x0000FFFFL) +#define FLEXCAN_MB_CS_LENGTH(x) (((x)&0x0000000F) << 16) +#define FLEXCAN_MB_CS_RTR (0x00100000) +#define FLEXCAN_MB_CS_IDE (0x00200000) +#define FLEXCAN_MB_CS_SRR (0x00400000) +#define FLEXCAN_MB_CS_CODE(x) (((x)&0x0000000F) << 24) +#define FLEXCAN_MB_CS_CODE_MASK (0x0F000000L) +#define FLEXCAN_MB_CS_DLC_MASK (0x000F0000L) +#define FLEXCAN_MB_CODE_RX_INACTIVE (0) +#define FLEXCAN_MB_CODE_RX_EMPTY (4) +#define FLEXCAN_MB_CODE_RX_FULL (2) +#define FLEXCAN_MB_CODE_RX_OVERRUN (6) +#define FLEXCAN_MB_CODE_RX_BUSY (1) + +#define FLEXCAN_MB_CS_IDE_BIT_NO (21) +#define FLEXCAN_MB_CS_RTR_BIT_NO (20) +#define FLEXCAN_MB_CS_DLC_BIT_NO (16) + +#define FLEXCAN_MB_CODE_TX_INACTIVE (8) +#define FLEXCAN_MB_CODE_TX_ABORT (9) +#define FLEXCAN_MB_CODE_TX_ONCE (0x0C) +#define FLEXCAN_MB_CODE_TX_RESPONSE (0x0A) +#define FLEXCAN_MB_CODE_TX_RESPONSE_TEMPO (0x0E) +#define FLEXCAN_get_code(cs) (((cs)&FLEXCAN_MB_CS_CODE_MASK) >> 24) +#define FLEXCAN_get_length(cs) (((cs)&FLEXCAN_MB_CS_DLC_MASK) >> 16) +#define FLEXCAN_get_timestamp(cs) (((cs)&FLEXCAN_MB_CS_TIMESTAMP_MASK) >> 0) + +/* Bit definitions and macros for FLEXCAN_MB_ID */ +#define FLEXCAN_MB_ID_STD_MASK (0x1FFC0000UL) +#define FLEXCAN_MB_ID_EXT_MASK (0x1FFFFFFFUL) +#define FLEXCAN_MB_ID_IDEXT(x) (((x)&0x0003FFFF) << 0) +#define FLEXCAN_MB_ID_IDSTD(x) (((x)&0x000007FF) << 18) +#define FLEXCAN_MB_ID_PRIO(x) (((x)&0x00000007) << 29) +#define FLEXCAN_MB_ID_PRIO_BIT_NO (29) +#define FLEXCAN_MB_ID_STD_BIT_NO (18) +#define FLEXCAN_MB_ID_EXT_BIT_NO (0) + +/* Bit definitions and macros for FLEXCAN_MB_WORD0 */ +#define FLEXCAN_MB_WORD0_DATA3(x) (((x)&0x000000FF) << 0) +#define FLEXCAN_MB_WORD0_DATA2(x) (((x)&0x000000FF) << 8) +#define FLEXCAN_MB_WORD0_DATA1(x) (((x)&0x000000FF) << 16) +#define FLEXCAN_MB_WORD0_DATA0(x) (((x)&0x000000FF) << 24) + +/* Bit definitions and macros for FLEXCAN_MB_WORD1 */ +#define FLEXCAN_MB_WORD1_DATA7(x) (((x)&0x000000FF) << 0) +#define FLEXCAN_MB_WORD1_DATA6(x) (((x)&0x000000FF) << 8) +#define FLEXCAN_MB_WORD1_DATA5(x) (((x)&0x000000FF) << 16) +#define FLEXCAN_MB_WORD1_DATA4(x) (((x)&0x000000FF) << 24) + +/* Bit definitions and macros for FLEXCAN_RXIMR0 */ +#define FLEXCAN_RXIMR0_MI0 (0x00000001) +#define FLEXCAN_RXIMR0_MI1 (0x00000002) +#define FLEXCAN_RXIMR0_MI2 (0x00000004) +#define FLEXCAN_RXIMR0_MI3 (0x00000008) +#define FLEXCAN_RXIMR0_MI4 (0x00000010) +#define FLEXCAN_RXIMR0_MI5 (0x00000020) +#define FLEXCAN_RXIMR0_MI6 (0x00000040) +#define FLEXCAN_RXIMR0_MI7 (0x00000080) +#define FLEXCAN_RXIMR0_MI8 (0x00000100) +#define FLEXCAN_RXIMR0_MI9 (0x00000200) +#define FLEXCAN_RXIMR0_MI10 (0x00000400) +#define FLEXCAN_RXIMR0_MI11 (0x00000800) +#define FLEXCAN_RXIMR0_MI12 (0x00001000) +#define FLEXCAN_RXIMR0_MI13 (0x00002000) +#define FLEXCAN_RXIMR0_MI14 (0x00004000) +#define FLEXCAN_RXIMR0_MI15 (0x00008000) +#define FLEXCAN_RXIMR0_MI16 (0x00010000) +#define FLEXCAN_RXIMR0_MI17 (0x00020000) +#define FLEXCAN_RXIMR0_MI18 (0x00040000) +#define FLEXCAN_RXIMR0_MI19 (0x00080000) +#define FLEXCAN_RXIMR0_MI20 (0x00100000) +#define FLEXCAN_RXIMR0_MI21 (0x00200000) +#define FLEXCAN_RXIMR0_MI22 (0x00400000) +#define FLEXCAN_RXIMR0_MI23 (0x00800000) +#define FLEXCAN_RXIMR0_MI24 (0x01000000) +#define FLEXCAN_RXIMR0_MI25 (0x02000000) +#define FLEXCAN_RXIMR0_MI26 (0x04000000) +#define FLEXCAN_RXIMR0_MI27 (0x08000000) +#define FLEXCAN_RXIMR0_MI28 (0x10000000) +#define FLEXCAN_RXIMR0_MI29 (0x20000000) +#define FLEXCAN_RXIMR0_MI30 (0x40000000) +#define FLEXCAN_RXIMR0_MI31 (0x80000000) + +/* Bit definitions and macros for FLEXCAN_RXIMR1 */ +#define FLEXCAN_RXIMR1_MI0 (0x00000001) +#define FLEXCAN_RXIMR1_MI1 (0x00000002) +#define FLEXCAN_RXIMR1_MI2 (0x00000004) +#define FLEXCAN_RXIMR1_MI3 (0x00000008) +#define FLEXCAN_RXIMR1_MI4 (0x00000010) +#define FLEXCAN_RXIMR1_MI5 (0x00000020) +#define FLEXCAN_RXIMR1_MI6 (0x00000040) +#define FLEXCAN_RXIMR1_MI7 (0x00000080) +#define FLEXCAN_RXIMR1_MI8 (0x00000100) +#define FLEXCAN_RXIMR1_MI9 (0x00000200) +#define FLEXCAN_RXIMR1_MI10 (0x00000400) +#define FLEXCAN_RXIMR1_MI11 (0x00000800) +#define FLEXCAN_RXIMR1_MI12 (0x00001000) +#define FLEXCAN_RXIMR1_MI13 (0x00002000) +#define FLEXCAN_RXIMR1_MI14 (0x00004000) +#define FLEXCAN_RXIMR1_MI15 (0x00008000) +#define FLEXCAN_RXIMR1_MI16 (0x00010000) +#define FLEXCAN_RXIMR1_MI17 (0x00020000) +#define FLEXCAN_RXIMR1_MI18 (0x00040000) +#define FLEXCAN_RXIMR1_MI19 (0x00080000) +#define FLEXCAN_RXIMR1_MI20 (0x00100000) +#define FLEXCAN_RXIMR1_MI21 (0x00200000) +#define FLEXCAN_RXIMR1_MI22 (0x00400000) +#define FLEXCAN_RXIMR1_MI23 (0x00800000) +#define FLEXCAN_RXIMR1_MI24 (0x01000000) +#define FLEXCAN_RXIMR1_MI25 (0x02000000) +#define FLEXCAN_RXIMR1_MI26 (0x04000000) +#define FLEXCAN_RXIMR1_MI27 (0x08000000) +#define FLEXCAN_RXIMR1_MI28 (0x10000000) +#define FLEXCAN_RXIMR1_MI29 (0x20000000) +#define FLEXCAN_RXIMR1_MI30 (0x40000000) +#define FLEXCAN_RXIMR1_MI31 (0x80000000) + +/* Bit definitions and macros for FLEXCAN_RXIMR2 */ +#define FLEXCAN_RXIMR2_MI0 (0x00000001) +#define FLEXCAN_RXIMR2_MI1 (0x00000002) +#define FLEXCAN_RXIMR2_MI2 (0x00000004) +#define FLEXCAN_RXIMR2_MI3 (0x00000008) +#define FLEXCAN_RXIMR2_MI4 (0x00000010) +#define FLEXCAN_RXIMR2_MI5 (0x00000020) +#define FLEXCAN_RXIMR2_MI6 (0x00000040) +#define FLEXCAN_RXIMR2_MI7 (0x00000080) +#define FLEXCAN_RXIMR2_MI8 (0x00000100) +#define FLEXCAN_RXIMR2_MI9 (0x00000200) +#define FLEXCAN_RXIMR2_MI10 (0x00000400) +#define FLEXCAN_RXIMR2_MI11 (0x00000800) +#define FLEXCAN_RXIMR2_MI12 (0x00001000) +#define FLEXCAN_RXIMR2_MI13 (0x00002000) +#define FLEXCAN_RXIMR2_MI14 (0x00004000) +#define FLEXCAN_RXIMR2_MI15 (0x00008000) +#define FLEXCAN_RXIMR2_MI16 (0x00010000) +#define FLEXCAN_RXIMR2_MI17 (0x00020000) +#define FLEXCAN_RXIMR2_MI18 (0x00040000) +#define FLEXCAN_RXIMR2_MI19 (0x00080000) +#define FLEXCAN_RXIMR2_MI20 (0x00100000) +#define FLEXCAN_RXIMR2_MI21 (0x00200000) +#define FLEXCAN_RXIMR2_MI22 (0x00400000) +#define FLEXCAN_RXIMR2_MI23 (0x00800000) +#define FLEXCAN_RXIMR2_MI24 (0x01000000) +#define FLEXCAN_RXIMR2_MI25 (0x02000000) +#define FLEXCAN_RXIMR2_MI26 (0x04000000) +#define FLEXCAN_RXIMR2_MI27 (0x08000000) +#define FLEXCAN_RXIMR2_MI28 (0x10000000) +#define FLEXCAN_RXIMR2_MI29 (0x20000000) +#define FLEXCAN_RXIMR2_MI30 (0x40000000) +#define FLEXCAN_RXIMR2_MI31 (0x80000000) + +/* Bit definitions and macros for FLEXCAN_RXIMR3 */ +#define FLEXCAN_RXIMR3_MI0 (0x00000001) +#define FLEXCAN_RXIMR3_MI1 (0x00000002) +#define FLEXCAN_RXIMR3_MI2 (0x00000004) +#define FLEXCAN_RXIMR3_MI3 (0x00000008) +#define FLEXCAN_RXIMR3_MI4 (0x00000010) +#define FLEXCAN_RXIMR3_MI5 (0x00000020) +#define FLEXCAN_RXIMR3_MI6 (0x00000040) +#define FLEXCAN_RXIMR3_MI7 (0x00000080) +#define FLEXCAN_RXIMR3_MI8 (0x00000100) +#define FLEXCAN_RXIMR3_MI9 (0x00000200) +#define FLEXCAN_RXIMR3_MI10 (0x00000400) +#define FLEXCAN_RXIMR3_MI11 (0x00000800) +#define FLEXCAN_RXIMR3_MI12 (0x00001000) +#define FLEXCAN_RXIMR3_MI13 (0x00002000) +#define FLEXCAN_RXIMR3_MI14 (0x00004000) +#define FLEXCAN_RXIMR3_MI15 (0x00008000) +#define FLEXCAN_RXIMR3_MI16 (0x00010000) +#define FLEXCAN_RXIMR3_MI17 (0x00020000) +#define FLEXCAN_RXIMR3_MI18 (0x00040000) +#define FLEXCAN_RXIMR3_MI19 (0x00080000) +#define FLEXCAN_RXIMR3_MI20 (0x00100000) +#define FLEXCAN_RXIMR3_MI21 (0x00200000) +#define FLEXCAN_RXIMR3_MI22 (0x00400000) +#define FLEXCAN_RXIMR3_MI23 (0x00800000) +#define FLEXCAN_RXIMR3_MI24 (0x01000000) +#define FLEXCAN_RXIMR3_MI25 (0x02000000) +#define FLEXCAN_RXIMR3_MI26 (0x04000000) +#define FLEXCAN_RXIMR3_MI27 (0x08000000) +#define FLEXCAN_RXIMR3_MI28 (0x10000000) +#define FLEXCAN_RXIMR3_MI29 (0x20000000) +#define FLEXCAN_RXIMR3_MI30 (0x40000000) +#define FLEXCAN_RXIMR3_MI31 (0x80000000) + +/* Bit definitions and macros for FLEXCAN_RXIMR4 */ +#define FLEXCAN_RXIMR4_MI0 (0x00000001) +#define FLEXCAN_RXIMR4_MI1 (0x00000002) +#define FLEXCAN_RXIMR4_MI2 (0x00000004) +#define FLEXCAN_RXIMR4_MI3 (0x00000008) +#define FLEXCAN_RXIMR4_MI4 (0x00000010) +#define FLEXCAN_RXIMR4_MI5 (0x00000020) +#define FLEXCAN_RXIMR4_MI6 (0x00000040) +#define FLEXCAN_RXIMR4_MI7 (0x00000080) +#define FLEXCAN_RXIMR4_MI8 (0x00000100) +#define FLEXCAN_RXIMR4_MI9 (0x00000200) +#define FLEXCAN_RXIMR4_MI10 (0x00000400) +#define FLEXCAN_RXIMR4_MI11 (0x00000800) +#define FLEXCAN_RXIMR4_MI12 (0x00001000) +#define FLEXCAN_RXIMR4_MI13 (0x00002000) +#define FLEXCAN_RXIMR4_MI14 (0x00004000) +#define FLEXCAN_RXIMR4_MI15 (0x00008000) +#define FLEXCAN_RXIMR4_MI16 (0x00010000) +#define FLEXCAN_RXIMR4_MI17 (0x00020000) +#define FLEXCAN_RXIMR4_MI18 (0x00040000) +#define FLEXCAN_RXIMR4_MI19 (0x00080000) +#define FLEXCAN_RXIMR4_MI20 (0x00100000) +#define FLEXCAN_RXIMR4_MI21 (0x00200000) +#define FLEXCAN_RXIMR4_MI22 (0x00400000) +#define FLEXCAN_RXIMR4_MI23 (0x00800000) +#define FLEXCAN_RXIMR4_MI24 (0x01000000) +#define FLEXCAN_RXIMR4_MI25 (0x02000000) +#define FLEXCAN_RXIMR4_MI26 (0x04000000) +#define FLEXCAN_RXIMR4_MI27 (0x08000000) +#define FLEXCAN_RXIMR4_MI28 (0x10000000) +#define FLEXCAN_RXIMR4_MI29 (0x20000000) +#define FLEXCAN_RXIMR4_MI30 (0x40000000) +#define FLEXCAN_RXIMR4_MI31 (0x80000000) + +/* Bit definitions and macros for FLEXCAN_RXIMR5 */ +#define FLEXCAN_RXIMR5_MI0 (0x00000001) +#define FLEXCAN_RXIMR5_MI1 (0x00000002) +#define FLEXCAN_RXIMR5_MI2 (0x00000004) +#define FLEXCAN_RXIMR5_MI3 (0x00000008) +#define FLEXCAN_RXIMR5_MI4 (0x00000010) +#define FLEXCAN_RXIMR5_MI5 (0x00000020) +#define FLEXCAN_RXIMR5_MI6 (0x00000040) +#define FLEXCAN_RXIMR5_MI7 (0x00000080) +#define FLEXCAN_RXIMR5_MI8 (0x00000100) +#define FLEXCAN_RXIMR5_MI9 (0x00000200) +#define FLEXCAN_RXIMR5_MI10 (0x00000400) +#define FLEXCAN_RXIMR5_MI11 (0x00000800) +#define FLEXCAN_RXIMR5_MI12 (0x00001000) +#define FLEXCAN_RXIMR5_MI13 (0x00002000) +#define FLEXCAN_RXIMR5_MI14 (0x00004000) +#define FLEXCAN_RXIMR5_MI15 (0x00008000) +#define FLEXCAN_RXIMR5_MI16 (0x00010000) +#define FLEXCAN_RXIMR5_MI17 (0x00020000) +#define FLEXCAN_RXIMR5_MI18 (0x00040000) +#define FLEXCAN_RXIMR5_MI19 (0x00080000) +#define FLEXCAN_RXIMR5_MI20 (0x00100000) +#define FLEXCAN_RXIMR5_MI21 (0x00200000) +#define FLEXCAN_RXIMR5_MI22 (0x00400000) +#define FLEXCAN_RXIMR5_MI23 (0x00800000) +#define FLEXCAN_RXIMR5_MI24 (0x01000000) +#define FLEXCAN_RXIMR5_MI25 (0x02000000) +#define FLEXCAN_RXIMR5_MI26 (0x04000000) +#define FLEXCAN_RXIMR5_MI27 (0x08000000) +#define FLEXCAN_RXIMR5_MI28 (0x10000000) +#define FLEXCAN_RXIMR5_MI29 (0x20000000) +#define FLEXCAN_RXIMR5_MI30 (0x40000000) +#define FLEXCAN_RXIMR5_MI31 (0x80000000) + +/* Bit definitions and macros for FLEXCAN_RXIMR6 */ +#define FLEXCAN_RXIMR6_MI0 (0x00000001) +#define FLEXCAN_RXIMR6_MI1 (0x00000002) +#define FLEXCAN_RXIMR6_MI2 (0x00000004) +#define FLEXCAN_RXIMR6_MI3 (0x00000008) +#define FLEXCAN_RXIMR6_MI4 (0x00000010) +#define FLEXCAN_RXIMR6_MI5 (0x00000020) +#define FLEXCAN_RXIMR6_MI6 (0x00000040) +#define FLEXCAN_RXIMR6_MI7 (0x00000080) +#define FLEXCAN_RXIMR6_MI8 (0x00000100) +#define FLEXCAN_RXIMR6_MI9 (0x00000200) +#define FLEXCAN_RXIMR6_MI10 (0x00000400) +#define FLEXCAN_RXIMR6_MI11 (0x00000800) +#define FLEXCAN_RXIMR6_MI12 (0x00001000) +#define FLEXCAN_RXIMR6_MI13 (0x00002000) +#define FLEXCAN_RXIMR6_MI14 (0x00004000) +#define FLEXCAN_RXIMR6_MI15 (0x00008000) +#define FLEXCAN_RXIMR6_MI16 (0x00010000) +#define FLEXCAN_RXIMR6_MI17 (0x00020000) +#define FLEXCAN_RXIMR6_MI18 (0x00040000) +#define FLEXCAN_RXIMR6_MI19 (0x00080000) +#define FLEXCAN_RXIMR6_MI20 (0x00100000) +#define FLEXCAN_RXIMR6_MI21 (0x00200000) +#define FLEXCAN_RXIMR6_MI22 (0x00400000) +#define FLEXCAN_RXIMR6_MI23 (0x00800000) +#define FLEXCAN_RXIMR6_MI24 (0x01000000) +#define FLEXCAN_RXIMR6_MI25 (0x02000000) +#define FLEXCAN_RXIMR6_MI26 (0x04000000) +#define FLEXCAN_RXIMR6_MI27 (0x08000000) +#define FLEXCAN_RXIMR6_MI28 (0x10000000) +#define FLEXCAN_RXIMR6_MI29 (0x20000000) +#define FLEXCAN_RXIMR6_MI30 (0x40000000) +#define FLEXCAN_RXIMR6_MI31 (0x80000000) + +/* Bit definitions and macros for FLEXCAN_RXIMR7 */ +#define FLEXCAN_RXIMR7_MI0 (0x00000001) +#define FLEXCAN_RXIMR7_MI1 (0x00000002) +#define FLEXCAN_RXIMR7_MI2 (0x00000004) +#define FLEXCAN_RXIMR7_MI3 (0x00000008) +#define FLEXCAN_RXIMR7_MI4 (0x00000010) +#define FLEXCAN_RXIMR7_MI5 (0x00000020) +#define FLEXCAN_RXIMR7_MI6 (0x00000040) +#define FLEXCAN_RXIMR7_MI7 (0x00000080) +#define FLEXCAN_RXIMR7_MI8 (0x00000100) +#define FLEXCAN_RXIMR7_MI9 (0x00000200) +#define FLEXCAN_RXIMR7_MI10 (0x00000400) +#define FLEXCAN_RXIMR7_MI11 (0x00000800) +#define FLEXCAN_RXIMR7_MI12 (0x00001000) +#define FLEXCAN_RXIMR7_MI13 (0x00002000) +#define FLEXCAN_RXIMR7_MI14 (0x00004000) +#define FLEXCAN_RXIMR7_MI15 (0x00008000) +#define FLEXCAN_RXIMR7_MI16 (0x00010000) +#define FLEXCAN_RXIMR7_MI17 (0x00020000) +#define FLEXCAN_RXIMR7_MI18 (0x00040000) +#define FLEXCAN_RXIMR7_MI19 (0x00080000) +#define FLEXCAN_RXIMR7_MI20 (0x00100000) +#define FLEXCAN_RXIMR7_MI21 (0x00200000) +#define FLEXCAN_RXIMR7_MI22 (0x00400000) +#define FLEXCAN_RXIMR7_MI23 (0x00800000) +#define FLEXCAN_RXIMR7_MI24 (0x01000000) +#define FLEXCAN_RXIMR7_MI25 (0x02000000) +#define FLEXCAN_RXIMR7_MI26 (0x04000000) +#define FLEXCAN_RXIMR7_MI27 (0x08000000) +#define FLEXCAN_RXIMR7_MI28 (0x10000000) +#define FLEXCAN_RXIMR7_MI29 (0x20000000) +#define FLEXCAN_RXIMR7_MI30 (0x40000000) +#define FLEXCAN_RXIMR7_MI31 (0x80000000) + +/* Bit definitions and macros for FLEXCAN_RXIMR8 */ +#define FLEXCAN_RXIMR8_MI0 (0x00000001) +#define FLEXCAN_RXIMR8_MI1 (0x00000002) +#define FLEXCAN_RXIMR8_MI2 (0x00000004) +#define FLEXCAN_RXIMR8_MI3 (0x00000008) +#define FLEXCAN_RXIMR8_MI4 (0x00000010) +#define FLEXCAN_RXIMR8_MI5 (0x00000020) +#define FLEXCAN_RXIMR8_MI6 (0x00000040) +#define FLEXCAN_RXIMR8_MI7 (0x00000080) +#define FLEXCAN_RXIMR8_MI8 (0x00000100) +#define FLEXCAN_RXIMR8_MI9 (0x00000200) +#define FLEXCAN_RXIMR8_MI10 (0x00000400) +#define FLEXCAN_RXIMR8_MI11 (0x00000800) +#define FLEXCAN_RXIMR8_MI12 (0x00001000) +#define FLEXCAN_RXIMR8_MI13 (0x00002000) +#define FLEXCAN_RXIMR8_MI14 (0x00004000) +#define FLEXCAN_RXIMR8_MI15 (0x00008000) +#define FLEXCAN_RXIMR8_MI16 (0x00010000) +#define FLEXCAN_RXIMR8_MI17 (0x00020000) +#define FLEXCAN_RXIMR8_MI18 (0x00040000) +#define FLEXCAN_RXIMR8_MI19 (0x00080000) +#define FLEXCAN_RXIMR8_MI20 (0x00100000) +#define FLEXCAN_RXIMR8_MI21 (0x00200000) +#define FLEXCAN_RXIMR8_MI22 (0x00400000) +#define FLEXCAN_RXIMR8_MI23 (0x00800000) +#define FLEXCAN_RXIMR8_MI24 (0x01000000) +#define FLEXCAN_RXIMR8_MI25 (0x02000000) +#define FLEXCAN_RXIMR8_MI26 (0x04000000) +#define FLEXCAN_RXIMR8_MI27 (0x08000000) +#define FLEXCAN_RXIMR8_MI28 (0x10000000) +#define FLEXCAN_RXIMR8_MI29 (0x20000000) +#define FLEXCAN_RXIMR8_MI30 (0x40000000) +#define FLEXCAN_RXIMR8_MI31 (0x80000000) + +/* Bit definitions and macros for FLEXCAN_RXIMR9 */ +#define FLEXCAN_RXIMR9_MI0 (0x00000001) +#define FLEXCAN_RXIMR9_MI1 (0x00000002) +#define FLEXCAN_RXIMR9_MI2 (0x00000004) +#define FLEXCAN_RXIMR9_MI3 (0x00000008) +#define FLEXCAN_RXIMR9_MI4 (0x00000010) +#define FLEXCAN_RXIMR9_MI5 (0x00000020) +#define FLEXCAN_RXIMR9_MI6 (0x00000040) +#define FLEXCAN_RXIMR9_MI7 (0x00000080) +#define FLEXCAN_RXIMR9_MI8 (0x00000100) +#define FLEXCAN_RXIMR9_MI9 (0x00000200) +#define FLEXCAN_RXIMR9_MI10 (0x00000400) +#define FLEXCAN_RXIMR9_MI11 (0x00000800) +#define FLEXCAN_RXIMR9_MI12 (0x00001000) +#define FLEXCAN_RXIMR9_MI13 (0x00002000) +#define FLEXCAN_RXIMR9_MI14 (0x00004000) +#define FLEXCAN_RXIMR9_MI15 (0x00008000) +#define FLEXCAN_RXIMR9_MI16 (0x00010000) +#define FLEXCAN_RXIMR9_MI17 (0x00020000) +#define FLEXCAN_RXIMR9_MI18 (0x00040000) +#define FLEXCAN_RXIMR9_MI19 (0x00080000) +#define FLEXCAN_RXIMR9_MI20 (0x00100000) +#define FLEXCAN_RXIMR9_MI21 (0x00200000) +#define FLEXCAN_RXIMR9_MI22 (0x00400000) +#define FLEXCAN_RXIMR9_MI23 (0x00800000) +#define FLEXCAN_RXIMR9_MI24 (0x01000000) +#define FLEXCAN_RXIMR9_MI25 (0x02000000) +#define FLEXCAN_RXIMR9_MI26 (0x04000000) +#define FLEXCAN_RXIMR9_MI27 (0x08000000) +#define FLEXCAN_RXIMR9_MI28 (0x10000000) +#define FLEXCAN_RXIMR9_MI29 (0x20000000) +#define FLEXCAN_RXIMR9_MI30 (0x40000000) +#define FLEXCAN_RXIMR9_MI31 (0x80000000) + +/* Bit definitions and macros for FLEXCAN_RXIMR10 */ +#define FLEXCAN_RXIMR10_MI0 (0x00000001) +#define FLEXCAN_RXIMR10_MI1 (0x00000002) +#define FLEXCAN_RXIMR10_MI2 (0x00000004) +#define FLEXCAN_RXIMR10_MI3 (0x00000008) +#define FLEXCAN_RXIMR10_MI4 (0x00000010) +#define FLEXCAN_RXIMR10_MI5 (0x00000020) +#define FLEXCAN_RXIMR10_MI6 (0x00000040) +#define FLEXCAN_RXIMR10_MI7 (0x00000080) +#define FLEXCAN_RXIMR10_MI8 (0x00000100) +#define FLEXCAN_RXIMR10_MI9 (0x00000200) +#define FLEXCAN_RXIMR10_MI10 (0x00000400) +#define FLEXCAN_RXIMR10_MI11 (0x00000800) +#define FLEXCAN_RXIMR10_MI12 (0x00001000) +#define FLEXCAN_RXIMR10_MI13 (0x00002000) +#define FLEXCAN_RXIMR10_MI14 (0x00004000) +#define FLEXCAN_RXIMR10_MI15 (0x00008000) +#define FLEXCAN_RXIMR10_MI16 (0x00010000) +#define FLEXCAN_RXIMR10_MI17 (0x00020000) +#define FLEXCAN_RXIMR10_MI18 (0x00040000) +#define FLEXCAN_RXIMR10_MI19 (0x00080000) +#define FLEXCAN_RXIMR10_MI20 (0x00100000) +#define FLEXCAN_RXIMR10_MI21 (0x00200000) +#define FLEXCAN_RXIMR10_MI22 (0x00400000) +#define FLEXCAN_RXIMR10_MI23 (0x00800000) +#define FLEXCAN_RXIMR10_MI24 (0x01000000) +#define FLEXCAN_RXIMR10_MI25 (0x02000000) +#define FLEXCAN_RXIMR10_MI26 (0x04000000) +#define FLEXCAN_RXIMR10_MI27 (0x08000000) +#define FLEXCAN_RXIMR10_MI28 (0x10000000) +#define FLEXCAN_RXIMR10_MI29 (0x20000000) +#define FLEXCAN_RXIMR10_MI30 (0x40000000) +#define FLEXCAN_RXIMR10_MI31 (0x80000000) + +/* Bit definitions and macros for FLEXCAN_RXIMR11 */ +#define FLEXCAN_RXIMR11_MI0 (0x00000001) +#define FLEXCAN_RXIMR11_MI1 (0x00000002) +#define FLEXCAN_RXIMR11_MI2 (0x00000004) +#define FLEXCAN_RXIMR11_MI3 (0x00000008) +#define FLEXCAN_RXIMR11_MI4 (0x00000010) +#define FLEXCAN_RXIMR11_MI5 (0x00000020) +#define FLEXCAN_RXIMR11_MI6 (0x00000040) +#define FLEXCAN_RXIMR11_MI7 (0x00000080) +#define FLEXCAN_RXIMR11_MI8 (0x00000100) +#define FLEXCAN_RXIMR11_MI9 (0x00000200) +#define FLEXCAN_RXIMR11_MI10 (0x00000400) +#define FLEXCAN_RXIMR11_MI11 (0x00000800) +#define FLEXCAN_RXIMR11_MI12 (0x00001000) +#define FLEXCAN_RXIMR11_MI13 (0x00002000) +#define FLEXCAN_RXIMR11_MI14 (0x00004000) +#define FLEXCAN_RXIMR11_MI15 (0x00008000) +#define FLEXCAN_RXIMR11_MI16 (0x00010000) +#define FLEXCAN_RXIMR11_MI17 (0x00020000) +#define FLEXCAN_RXIMR11_MI18 (0x00040000) +#define FLEXCAN_RXIMR11_MI19 (0x00080000) +#define FLEXCAN_RXIMR11_MI20 (0x00100000) +#define FLEXCAN_RXIMR11_MI21 (0x00200000) +#define FLEXCAN_RXIMR11_MI22 (0x00400000) +#define FLEXCAN_RXIMR11_MI23 (0x00800000) +#define FLEXCAN_RXIMR11_MI24 (0x01000000) +#define FLEXCAN_RXIMR11_MI25 (0x02000000) +#define FLEXCAN_RXIMR11_MI26 (0x04000000) +#define FLEXCAN_RXIMR11_MI27 (0x08000000) +#define FLEXCAN_RXIMR11_MI28 (0x10000000) +#define FLEXCAN_RXIMR11_MI29 (0x20000000) +#define FLEXCAN_RXIMR11_MI30 (0x40000000) +#define FLEXCAN_RXIMR11_MI31 (0x80000000) + +/* Bit definitions and macros for FLEXCAN_RXIMR12 */ +#define FLEXCAN_RXIMR12_MI0 (0x00000001) +#define FLEXCAN_RXIMR12_MI1 (0x00000002) +#define FLEXCAN_RXIMR12_MI2 (0x00000004) +#define FLEXCAN_RXIMR12_MI3 (0x00000008) +#define FLEXCAN_RXIMR12_MI4 (0x00000010) +#define FLEXCAN_RXIMR12_MI5 (0x00000020) +#define FLEXCAN_RXIMR12_MI6 (0x00000040) +#define FLEXCAN_RXIMR12_MI7 (0x00000080) +#define FLEXCAN_RXIMR12_MI8 (0x00000100) +#define FLEXCAN_RXIMR12_MI9 (0x00000200) +#define FLEXCAN_RXIMR12_MI10 (0x00000400) +#define FLEXCAN_RXIMR12_MI11 (0x00000800) +#define FLEXCAN_RXIMR12_MI12 (0x00001000) +#define FLEXCAN_RXIMR12_MI13 (0x00002000) +#define FLEXCAN_RXIMR12_MI14 (0x00004000) +#define FLEXCAN_RXIMR12_MI15 (0x00008000) +#define FLEXCAN_RXIMR12_MI16 (0x00010000) +#define FLEXCAN_RXIMR12_MI17 (0x00020000) +#define FLEXCAN_RXIMR12_MI18 (0x00040000) +#define FLEXCAN_RXIMR12_MI19 (0x00080000) +#define FLEXCAN_RXIMR12_MI20 (0x00100000) +#define FLEXCAN_RXIMR12_MI21 (0x00200000) +#define FLEXCAN_RXIMR12_MI22 (0x00400000) +#define FLEXCAN_RXIMR12_MI23 (0x00800000) +#define FLEXCAN_RXIMR12_MI24 (0x01000000) +#define FLEXCAN_RXIMR12_MI25 (0x02000000) +#define FLEXCAN_RXIMR12_MI26 (0x04000000) +#define FLEXCAN_RXIMR12_MI27 (0x08000000) +#define FLEXCAN_RXIMR12_MI28 (0x10000000) +#define FLEXCAN_RXIMR12_MI29 (0x20000000) +#define FLEXCAN_RXIMR12_MI30 (0x40000000) +#define FLEXCAN_RXIMR12_MI31 (0x80000000) + +/* Bit definitions and macros for FLEXCAN_RXIMR13 */ +#define FLEXCAN_RXIMR13_MI0 (0x00000001) +#define FLEXCAN_RXIMR13_MI1 (0x00000002) +#define FLEXCAN_RXIMR13_MI2 (0x00000004) +#define FLEXCAN_RXIMR13_MI3 (0x00000008) +#define FLEXCAN_RXIMR13_MI4 (0x00000010) +#define FLEXCAN_RXIMR13_MI5 (0x00000020) +#define FLEXCAN_RXIMR13_MI6 (0x00000040) +#define FLEXCAN_RXIMR13_MI7 (0x00000080) +#define FLEXCAN_RXIMR13_MI8 (0x00000100) +#define FLEXCAN_RXIMR13_MI9 (0x00000200) +#define FLEXCAN_RXIMR13_MI10 (0x00000400) +#define FLEXCAN_RXIMR13_MI11 (0x00000800) +#define FLEXCAN_RXIMR13_MI12 (0x00001000) +#define FLEXCAN_RXIMR13_MI13 (0x00002000) +#define FLEXCAN_RXIMR13_MI14 (0x00004000) +#define FLEXCAN_RXIMR13_MI15 (0x00008000) +#define FLEXCAN_RXIMR13_MI16 (0x00010000) +#define FLEXCAN_RXIMR13_MI17 (0x00020000) +#define FLEXCAN_RXIMR13_MI18 (0x00040000) +#define FLEXCAN_RXIMR13_MI19 (0x00080000) +#define FLEXCAN_RXIMR13_MI20 (0x00100000) +#define FLEXCAN_RXIMR13_MI21 (0x00200000) +#define FLEXCAN_RXIMR13_MI22 (0x00400000) +#define FLEXCAN_RXIMR13_MI23 (0x00800000) +#define FLEXCAN_RXIMR13_MI24 (0x01000000) +#define FLEXCAN_RXIMR13_MI25 (0x02000000) +#define FLEXCAN_RXIMR13_MI26 (0x04000000) +#define FLEXCAN_RXIMR13_MI27 (0x08000000) +#define FLEXCAN_RXIMR13_MI28 (0x10000000) +#define FLEXCAN_RXIMR13_MI29 (0x20000000) +#define FLEXCAN_RXIMR13_MI30 (0x40000000) +#define FLEXCAN_RXIMR13_MI31 (0x80000000) + +/* Bit definitions and macros for FLEXCAN_RXIMR14 */ +#define FLEXCAN_RXIMR14_MI0 (0x00000001) +#define FLEXCAN_RXIMR14_MI1 (0x00000002) +#define FLEXCAN_RXIMR14_MI2 (0x00000004) +#define FLEXCAN_RXIMR14_MI3 (0x00000008) +#define FLEXCAN_RXIMR14_MI4 (0x00000010) +#define FLEXCAN_RXIMR14_MI5 (0x00000020) +#define FLEXCAN_RXIMR14_MI6 (0x00000040) +#define FLEXCAN_RXIMR14_MI7 (0x00000080) +#define FLEXCAN_RXIMR14_MI8 (0x00000100) +#define FLEXCAN_RXIMR14_MI9 (0x00000200) +#define FLEXCAN_RXIMR14_MI10 (0x00000400) +#define FLEXCAN_RXIMR14_MI11 (0x00000800) +#define FLEXCAN_RXIMR14_MI12 (0x00001000) +#define FLEXCAN_RXIMR14_MI13 (0x00002000) +#define FLEXCAN_RXIMR14_MI14 (0x00004000) +#define FLEXCAN_RXIMR14_MI15 (0x00008000) +#define FLEXCAN_RXIMR14_MI16 (0x00010000) +#define FLEXCAN_RXIMR14_MI17 (0x00020000) +#define FLEXCAN_RXIMR14_MI18 (0x00040000) +#define FLEXCAN_RXIMR14_MI19 (0x00080000) +#define FLEXCAN_RXIMR14_MI20 (0x00100000) +#define FLEXCAN_RXIMR14_MI21 (0x00200000) +#define FLEXCAN_RXIMR14_MI22 (0x00400000) +#define FLEXCAN_RXIMR14_MI23 (0x00800000) +#define FLEXCAN_RXIMR14_MI24 (0x01000000) +#define FLEXCAN_RXIMR14_MI25 (0x02000000) +#define FLEXCAN_RXIMR14_MI26 (0x04000000) +#define FLEXCAN_RXIMR14_MI27 (0x08000000) +#define FLEXCAN_RXIMR14_MI28 (0x10000000) +#define FLEXCAN_RXIMR14_MI29 (0x20000000) +#define FLEXCAN_RXIMR14_MI30 (0x40000000) +#define FLEXCAN_RXIMR14_MI31 (0x80000000) + +/* Bit definitions and macros for FLEXCAN_RXIMR15 */ +#define FLEXCAN_RXIMR15_MI0 (0x00000001) +#define FLEXCAN_RXIMR15_MI1 (0x00000002) +#define FLEXCAN_RXIMR15_MI2 (0x00000004) +#define FLEXCAN_RXIMR15_MI3 (0x00000008) +#define FLEXCAN_RXIMR15_MI4 (0x00000010) +#define FLEXCAN_RXIMR15_MI5 (0x00000020) +#define FLEXCAN_RXIMR15_MI6 (0x00000040) +#define FLEXCAN_RXIMR15_MI7 (0x00000080) +#define FLEXCAN_RXIMR15_MI8 (0x00000100) +#define FLEXCAN_RXIMR15_MI9 (0x00000200) +#define FLEXCAN_RXIMR15_MI10 (0x00000400) +#define FLEXCAN_RXIMR15_MI11 (0x00000800) +#define FLEXCAN_RXIMR15_MI12 (0x00001000) +#define FLEXCAN_RXIMR15_MI13 (0x00002000) +#define FLEXCAN_RXIMR15_MI14 (0x00004000) +#define FLEXCAN_RXIMR15_MI15 (0x00008000) +#define FLEXCAN_RXIMR15_MI16 (0x00010000) +#define FLEXCAN_RXIMR15_MI17 (0x00020000) +#define FLEXCAN_RXIMR15_MI18 (0x00040000) +#define FLEXCAN_RXIMR15_MI19 (0x00080000) +#define FLEXCAN_RXIMR15_MI20 (0x00100000) +#define FLEXCAN_RXIMR15_MI21 (0x00200000) +#define FLEXCAN_RXIMR15_MI22 (0x00400000) +#define FLEXCAN_RXIMR15_MI23 (0x00800000) +#define FLEXCAN_RXIMR15_MI24 (0x01000000) +#define FLEXCAN_RXIMR15_MI25 (0x02000000) +#define FLEXCAN_RXIMR15_MI26 (0x04000000) +#define FLEXCAN_RXIMR15_MI27 (0x08000000) +#define FLEXCAN_RXIMR15_MI28 (0x10000000) +#define FLEXCAN_RXIMR15_MI29 (0x20000000) +#define FLEXCAN_RXIMR15_MI30 (0x40000000) +#define FLEXCAN_RXIMR15_MI31 (0x80000000) + +/* Bit definitions for CRC register */ +#define FLEXCAN_CRCR_MBCRC_BIT_NO (16) +#define FLEXCAN_CRCR_MBCRC_MASK (0x007F0000) +#define FLEXCAN_CRCR_CRC_BIT_NO (0) +#define FLEXCAN_CRCR_CRC_MASK (0x00007FFF) + +/* Bit definition for Individual Matching Elements Update Register (IMEUR) */ +#define FLEXCAN_IMEUR_IMEUP_MASK (0x0000007F) +#define FLEXCAN_IMEUR_IMEUP_BIT_NO (0) +#define FLEXCAN_IMEUR_IMEUREQ_MASK (0x00000100) +#define FLEXCAN_IMEUR_IMEUACK_MASK (0x00000200) +#define FLEXCAN_Set_IMEUP(imeur, imeup) imeur = (imeur & ~(FLEXCAN_IMEUR_IMEUP_MASK)) | (imeup & FLEXCAN_IMEUR_IMEUP_MASK) +#define FLEXCAN_Get_IMEUP(imeur) (imeur & FLEXCAN_IMEUR_IMEUP_MASK) + +/* Bit definition for Lost Rx Frames Register (LRFR) + */ +#define FLEXCAN_LRFR_LOSTRLP_MASK (0x007F0000) +#define FLEXCAN_LRFR_LFIFOMTC_MASK (0x00008000) +#define FLEXCAN_LRFR_LOSTRMP_MASK (0x000001FF) +#define FLEXCAN_LRFR_LOSTRLP_BIT_NO (16) +#define FLEXCAN_LRFR_LFIFOMTC_BIT_NO (15) +#define FLEXCAN_LRFR_LOSTRMP_BIT_NO (0) +#define FLEXCAN_Get_LostMBLocked(lrfr) ((lrfr & FLEXCAN_LRFR_LOSTRLP_MASK) >> (FLEXCAN_LRFR_LOSTRLP_BIT_NO)) +#define FLEXCAN_Get_LostMBUpdated(lrfr) ((lrfr & FLEXCAN_LRFR_LOSTRMP_MASK)) + +/* Bit definition for Memory Error Control Register */ +#define FLEXCAN_MECR_NCEFAFRZ_MASK (0x00000080) +#define FLEXCAN_MECR_RERRDIS_MASK (0x00000100) +#define FLEXCAN_MECR_EXTERRIE_MAKS (0x00002000) +#define FLEXCAN_MECR_FAERRIE_MAKS (0x00004000) +#define FLEXCAN_MECR_HAERRIE_MAKS (0x00008000) +#define FLEXCAN_MECR_CEI_MSK_MAKS (0x00010000) +#define FLEXCAN_MECR_FANCEI_MSK_MAKS (0x00040000) +#define FLEXCAN_MECR_HANCEI_MSK_MAKS (0x00080000) +#define FLEXCAN_MECR_ECRWRDIS_MSK_MAKS (0x80000000) + +/* Bit definition for Error Report Address Register (RERRAR) */ +#define FLEXCAN_RERRAR_NCE_MASK (0x01000000) +#define FLEXCAN_RERRAR_SAID_MASK (0x00070000) +#define FLEXCAN_ERRADDR_MASK (0x00003FFF) + +/* Bit definition for Error Report Syndrome Register (RERRSYNR) */ +#define FLEXCAN_RERRSYNR_BE3_MASK (0x80000000) +#define FLEXCAN_RERRSYNR_SYND3_MASK (0x1F000000) +#define FLEXCAN_RERRSYNR_SYND3_BIT_NO (24) +#define FLEXCAN_RERRSYNR_BE2_MASK (0x00800000) +#define FLEXCAN_RERRSYNR_SYND2_MASK (0x001F0000) +#define FLEXCAN_RERRSYNR_SYND2_BIT_NO (16) +#define FLEXCAN_RERRSYNR_BE1_MASK (0x00008000) +#define FLEXCAN_RERRSYNR_SYND1_MASK (0x00001F00) +#define FLEXCAN_RERRSYNR_SYND1_BIT_NO (8) +#define FLEXCAN_RERRSYNR_BE0_MASK (0x00000080) +#define FLEXCAN_RERRSYNR_SYND0_MASK (0x0000001F) +#define FLEXCAN_RERRSYNR_SYND0_BIT_NO (0) + +#define FLEXCAN_RERRSYNR_check_BEn_Bit(errsynr, n) (errsynr & FLEXCAN_RERRSYNR_BE##n##_MASK) +#define FLEXCAN_RERRSYNR_get_SYNDn(errsynr, n) (errsynr & FLEXCAN_RERRSYNR_SYND##n##_MASK) +#define FLEXCAN_RERRSYNR_check_SYNDn_Bit(errsynr, n) ((errsynr & FLEXCAN_RERRSYNR_SYND##n##_MASK) >> FLEXCAN_RERRSYNR_SYND##n##_BIT_NO) + +/* Bit definition for Error Status Register (ERRSR) */ +#define FLEXCAN_ERRSR_CEIOF_MASK (0x00000001) +#define FLEXCAN_ERRSR_FANCEIOF_MASK (0x00000004) +#define FLEXCAN_ERRSR_HANCEIOF_MASK (0x00000008) +#define FLEXCAN_ERRSR_CEIF_MASK (0x00010000) +#define FLEXCAN_ERRSR_FANCEIF_MASK (0x00040000) +#define FLEXCAN_ERRSR_HANCEIF_MASK (0x00080000) + +/********************************************************************/ +#endif // __KINETIS_FLEXCAN_H diff --git a/src/nmea2000_fast_packet_protocol.cpp b/src/nmea2000_fast_packet_protocol.cpp new file mode 100644 index 0000000..1e14292 --- /dev/null +++ b/src/nmea2000_fast_packet_protocol.cpp @@ -0,0 +1,540 @@ +//================================================================================================ +/// @file nmea2000_fast_packet_protocol.cpp +/// +/// @brief A protocol that handles the NMEA 2000 fast packet protocol. +/// +/// @author Adrian Del Grosso +/// +/// @copyright 2022 Adrian Del Grosso +//================================================================================================ + +#include "nmea2000_fast_packet_protocol.hpp" + +#include "can_constants.hpp" +#include "can_network_manager.hpp" +#include "can_stack_logger.hpp" +#include "system_timing.hpp" + +#include + +namespace isobus +{ + FastPacketProtocol::FastPacketProtocolSession::FastPacketProtocolSession(Direction sessionDirection, std::uint8_t canPortIndex) : + sessionMessage(canPortIndex), + sessionCompleteCallback(nullptr), + frameChunkCallback(nullptr), + parent(nullptr), + timestamp_ms(0), + lastPacketNumber(0), + packetCount(0), + processedPacketsThisSession(0), + sequenceNumber(0), + sessionDirection(sessionDirection) + { + } + + bool FastPacketProtocol::FastPacketProtocolSession::operator==(const FastPacketProtocolSession &obj) + { + return ((sessionMessage.get_source_control_function() == obj.sessionMessage.get_source_control_function()) && + (sessionMessage.get_destination_control_function() == obj.sessionMessage.get_destination_control_function()) && + (sessionMessage.get_identifier().get_parameter_group_number() == obj.sessionMessage.get_identifier().get_parameter_group_number())); + } + + std::uint32_t FastPacketProtocol::FastPacketProtocolSession::get_message_data_length() const + { + if (nullptr != frameChunkCallback) + { + return frameChunkCallbackMessageLength; + } + return sessionMessage.get_data_length(); + } + + FastPacketProtocol::FastPacketProtocolSession::~FastPacketProtocolSession() + { + } + + void FastPacketProtocol::initialize(CANLibBadge) + { + if (!initialized) + { + initialized = true; + } + } + + void FastPacketProtocol::register_multipacket_message_callback(std::uint32_t parameterGroupNumber, CANLibCallback callback, void *parent, std::shared_ptr internalControlFunction) + { + parameterGroupNumberCallbacks.push_back(ParameterGroupNumberCallbackData(parameterGroupNumber, callback, parent, internalControlFunction)); + CANNetworkManager::CANNetwork.add_protocol_parameter_group_number_callback(parameterGroupNumber, process_message, this); + } + + void FastPacketProtocol::remove_multipacket_message_callback(std::uint32_t parameterGroupNumber, CANLibCallback callback, void *parent, std::shared_ptr internalControlFunction) + { + ParameterGroupNumberCallbackData tempObject(parameterGroupNumber, callback, parent, internalControlFunction); + auto callbackLocation = std::find(parameterGroupNumberCallbacks.begin(), parameterGroupNumberCallbacks.end(), tempObject); + if (parameterGroupNumberCallbacks.end() != callbackLocation) + { + parameterGroupNumberCallbacks.erase(callbackLocation); + } + CANNetworkManager::CANNetwork.remove_protocol_parameter_group_number_callback(parameterGroupNumber, process_message, this); + } + + bool FastPacketProtocol::send_multipacket_message(std::uint32_t parameterGroupNumber, + const std::uint8_t *data, + std::uint8_t messageLength, + std::shared_ptr source, + std::shared_ptr destination, + CANIdentifier::CANPriority priority, + TransmitCompleteCallback txCompleteCallback, + void *parentPointer, + DataChunkCallback frameChunkCallback) + { + bool retVal = false; + + if ((nullptr != source) && + (source->get_address_valid()) && + (parameterGroupNumber >= FP_MIN_PARAMETER_GROUP_NUMBER) && + (parameterGroupNumber <= FP_MAX_PARAMETER_GROUP_NUMBER) && + (messageLength <= MAX_PROTOCOL_MESSAGE_LENGTH) && + ((nullptr != data) || + (nullptr != frameChunkCallback))) + { + FastPacketProtocolSession *tempSession = nullptr; + + if (!get_session(tempSession, parameterGroupNumber, source, destination)) + { + tempSession = new FastPacketProtocolSession(FastPacketProtocolSession::Direction::Transmit, source->get_can_port()); + tempSession->sessionMessage.set_source_control_function(source); + tempSession->sessionMessage.set_destination_control_function(destination); + tempSession->sessionMessage.set_identifier(CANIdentifier(CANIdentifier::Type::Extended, parameterGroupNumber, priority, (destination == nullptr ? 0xFF : destination->get_address()), source->get_address())); + if (data != nullptr) + { + tempSession->sessionMessage.set_data(data, messageLength); + } + else + { + tempSession->frameChunkCallback = frameChunkCallback; + tempSession->frameChunkCallbackMessageLength = messageLength; + } + tempSession->parent = parentPointer; + tempSession->packetCount = ((messageLength - 6) / PROTOCOL_BYTES_PER_FRAME); + tempSession->timestamp_ms = SystemTiming::get_timestamp_ms(); + tempSession->processedPacketsThisSession = 0; + tempSession->sessionCompleteCallback = txCompleteCallback; + tempSession->sequenceNumber = get_new_sequence_number(tempSession); + + if ((messageLength > 6) && + (0 != ((messageLength - 6) % PROTOCOL_BYTES_PER_FRAME))) + { + tempSession->packetCount++; + } +#if !defined CAN_STACK_DISABLE_THREADS && !defined ARDUINO + std::unique_lock lock(sessionMutex); +#endif + + activeSessions.push_back(tempSession); + retVal = true; + } + else + { + // Already in a matching session, can't start another. + CANStackLogger::CAN_stack_log(CANStackLogger::LoggingLevel::Warning, "[FP]: Can't send fast packet message, already in matching session."); + } + } + else + { + CANStackLogger::CAN_stack_log(CANStackLogger::LoggingLevel::Error, "[FP]: Can't send fast packet message, bad parameters or ICF is invalid"); + } + return retVal; + } + + void FastPacketProtocol::update(CANLibBadge) + { +#if !defined CAN_STACK_DISABLE_THREADS && !defined ARDUINO + std::unique_lock lock(sessionMutex); +#endif + + for (auto i : activeSessions) + { + update_state_machine(i); + } + } + + void FastPacketProtocol::add_session_history(FastPacketProtocolSession *session) + { + if (nullptr != session) + { + bool formerSessionMatched = false; + + for (std::size_t i = 0; i < sessionHistory.size(); i++) + { + if ((sessionHistory[i].isoName == session->sessionMessage.get_source_control_function()->get_NAME()) && + (sessionHistory[i].parameterGroupNumber == session->sessionMessage.get_identifier().get_parameter_group_number())) + { + sessionHistory[i].sequenceNumber++; + formerSessionMatched = true; + break; + } + } + + if (!formerSessionMatched) + { + FastPacketHistory history{ + session->sessionMessage.get_source_control_function()->get_NAME(), + session->sessionMessage.get_identifier().get_parameter_group_number(), + session->sequenceNumber + }; + history.sequenceNumber++; + sessionHistory.push_back(history); + } + } + } + + void FastPacketProtocol::close_session(FastPacketProtocolSession *session, bool successfull) + { + if (nullptr != session) + { + process_session_complete_callback(session, successfull); + for (auto currentSession = activeSessions.begin(); currentSession != activeSessions.end(); currentSession++) + { + if (session == *currentSession) + { + activeSessions.erase(currentSession); + delete session; + break; + } + } + } + } + + std::uint8_t FastPacketProtocol::get_new_sequence_number(FastPacketProtocolSession *session) + { + std::uint8_t retVal = 0; + + if (nullptr != session) + { + for (auto &formerSessions : sessionHistory) + { + if ((formerSessions.isoName == session->sessionMessage.get_source_control_function()->get_NAME()) && + (formerSessions.parameterGroupNumber == session->sessionMessage.get_identifier().get_parameter_group_number())) + { + retVal = formerSessions.sequenceNumber; + break; + } + } + } + return retVal; + } + + bool FastPacketProtocol::get_session(FastPacketProtocolSession *&returnedSession, std::uint32_t parameterGroupNumber, std::shared_ptr source, std::shared_ptr destination) + { + returnedSession = nullptr; +#if !defined CAN_STACK_DISABLE_THREADS && !defined ARDUINO + std::unique_lock lock(sessionMutex); +#endif + + for (auto session : activeSessions) + { + if ((session->sessionMessage.get_identifier().get_parameter_group_number() == parameterGroupNumber) && + (session->sessionMessage.get_source_control_function() == source) && + (session->sessionMessage.get_destination_control_function() == destination)) + { + returnedSession = session; + break; + } + } + return (nullptr != returnedSession); + } + + void FastPacketProtocol::process_message(const CANMessage &message, void *parent) + { + if (nullptr != parent) + { + static_cast(parent)->process_message(message); + } + } + + void FastPacketProtocol::process_message(const CANMessage &message) + { + if ((CAN_DATA_LENGTH == message.get_data_length()) && + (message.get_identifier().get_parameter_group_number() >= FP_MIN_PARAMETER_GROUP_NUMBER) && + (message.get_identifier().get_parameter_group_number() <= FP_MAX_PARAMETER_GROUP_NUMBER)) + { + // See if we care about parsing this message + if (parameterGroupNumberCallbacks.size() > 0) + { + bool pgnNeedsParsing = false; + + for (auto &callback : parameterGroupNumberCallbacks) + { + if ((callback.get_parameter_group_number() == message.get_identifier().get_parameter_group_number()) && + ((nullptr == callback.get_internal_control_function()) || + (callback.get_internal_control_function()->get_address() == message.get_destination_control_function()->get_address()))) + { + pgnNeedsParsing = true; + break; + } + } + + if (pgnNeedsParsing) + { + FastPacketProtocolSession *currentSession = nullptr; + std::vector messageData = message.get_data(); + std::uint8_t frameCount = (messageData[0] & FRAME_COUNTER_BIT_MASK); + + // Check for a valid session + if (get_session(currentSession, message.get_identifier().get_parameter_group_number(), message.get_source_control_function(), message.get_destination_control_function())) + { + // Matched a session + if (0 != frameCount) + { + // Continue processing the message + for (std::uint8_t i = 0; i < PROTOCOL_BYTES_PER_FRAME; i++) + { + if (static_cast(i + (currentSession->processedPacketsThisSession * PROTOCOL_BYTES_PER_FRAME) - 1) < currentSession->sessionMessage.get_data_length()) + { + currentSession->sessionMessage.set_data(messageData[1 + i], i + (currentSession->processedPacketsThisSession * PROTOCOL_BYTES_PER_FRAME) - 1); + } + else + { + break; + } + } + currentSession->processedPacketsThisSession++; + + if (static_cast((currentSession->processedPacketsThisSession * PROTOCOL_BYTES_PER_FRAME) - 1) >= currentSession->sessionMessage.get_data_length()) + { + // Complete + // Find the appropriate callback and let them know + for (auto &callback : parameterGroupNumberCallbacks) + { + if (callback.get_parameter_group_number() == currentSession->sessionMessage.get_identifier().get_parameter_group_number()) + { + callback.get_callback()(currentSession->sessionMessage, callback.get_parent()); + } + } + close_session(currentSession, true); // All done + } + } + else + { + CANStackLogger::CAN_stack_log(CANStackLogger::LoggingLevel::Error, "[FP]: Existing session matched new frame counter, aborting the matching session."); + close_session(currentSession, false); + } + } + else + { + // No matching session. See if we need to start a new session + if (0 == frameCount) + { + if (messageData[1] <= MAX_PROTOCOL_MESSAGE_LENGTH) + { + // This is the beginning of a new message + currentSession = new FastPacketProtocolSession(FastPacketProtocolSession::Direction::Receive, message.get_can_port_index()); + currentSession->frameChunkCallback = nullptr; + if (messageData[1] >= PROTOCOL_BYTES_PER_FRAME - 1) + { + currentSession->packetCount = ((messageData[1] - 6) / PROTOCOL_BYTES_PER_FRAME); + } + else + { + currentSession->packetCount = 1; + } + currentSession->lastPacketNumber = ((messageData[0] >> SEQUENCE_NUMBER_BIT_OFFSET) & SEQUENCE_NUMBER_BIT_MASK); + currentSession->processedPacketsThisSession = 1; + currentSession->sessionMessage.set_data_size(messageData[1]); + currentSession->sessionMessage.set_identifier(message.get_identifier()); + currentSession->sessionMessage.set_source_control_function(message.get_source_control_function()); + currentSession->sessionMessage.set_destination_control_function(message.get_destination_control_function()); + currentSession->timestamp_ms = SystemTiming::get_timestamp_ms(); + + if (0 != (messageData[1] % PROTOCOL_BYTES_PER_FRAME)) + { + currentSession->packetCount++; + } + + // Save the 6 bytes of payload in this first message + for (std::uint8_t i = 0; i < (PROTOCOL_BYTES_PER_FRAME - 1); i++) + { + currentSession->sessionMessage.set_data(messageData[2 + i], i); + } +#if !defined CAN_STACK_DISABLE_THREADS && !defined ARDUINO + std::unique_lock lock(sessionMutex); +#endif + + activeSessions.push_back(currentSession); + } + else + { + CANStackLogger::CAN_stack_log(CANStackLogger::LoggingLevel::Warning, "[FP]: Ignoring possible new FP session with advertised length > 233."); + } + } + else + { + // This is the middle of some message that we have no context for. + // Ignore the message for now until we receive it with a fresh packet counter. + CANStackLogger::CAN_stack_log(CANStackLogger::LoggingLevel::Warning, "[FP]: Ignoring FP message with PGN %u, no context available. The message may be processed when packet count returns to zero.", message.get_identifier().get_parameter_group_number()); + } + } + } + } + } + } + + void FastPacketProtocol::process_session_complete_callback(FastPacketProtocolSession *session, bool success) + { + if ((nullptr != session) && + (nullptr != session->sessionCompleteCallback) && + (nullptr != session->sessionMessage.get_source_control_function()) && + (ControlFunction::Type::Internal == session->sessionMessage.get_source_control_function()->get_type())) + { + session->sessionCompleteCallback(session->sessionMessage.get_identifier().get_parameter_group_number(), + session->get_message_data_length(), + std::static_pointer_cast(session->sessionMessage.get_source_control_function()), + session->sessionMessage.get_destination_control_function(), + success, + session->parent); + } + } + + bool FastPacketProtocol::protocol_transmit_message(std::uint32_t, + const std::uint8_t *, + std::uint32_t, + std::shared_ptr, + std::shared_ptr, + TransmitCompleteCallback, + void *, + DataChunkCallback) + { + return false; + } + + void FastPacketProtocol::update_state_machine(FastPacketProtocolSession *session) + { + if (nullptr != session) + { + switch (session->sessionDirection) + { + case FastPacketProtocolSession::Direction::Receive: + { + if (SystemTiming::time_expired_ms(session->timestamp_ms, FP_TIMEOUT_MS)) + { + CANStackLogger::CAN_stack_log(CANStackLogger::LoggingLevel::Error, "[FP]: Rx session timed out."); + close_session(session, false); + } + } + break; + + case FastPacketProtocolSession::Direction::Transmit: + { + std::array dataBuffer; + std::vector messageData; + bool txSessionCancelled = false; + + for (std::uint8_t i = session->processedPacketsThisSession; i <= session->packetCount; i++) + { + std::uint8_t bytesProcessedSoFar = (session->processedPacketsThisSession > 0 ? 6 : 0); + + if (0 != bytesProcessedSoFar) + { + bytesProcessedSoFar += (PROTOCOL_BYTES_PER_FRAME * (session->processedPacketsThisSession - 1)); + } + + std::uint16_t numberBytesLeft = (session->get_message_data_length() - bytesProcessedSoFar); + + if (numberBytesLeft > PROTOCOL_BYTES_PER_FRAME) + { + numberBytesLeft = PROTOCOL_BYTES_PER_FRAME; + } + + if (nullptr != session->frameChunkCallback) + { + std::uint8_t callbackBuffer[CAN_DATA_LENGTH] = { 0 }; // Only need 7 but give them 8 in case they make a mistake + bool callbackSuccessful = session->frameChunkCallback(dataBuffer[0], (PROTOCOL_BYTES_PER_FRAME * session->processedPacketsThisSession), numberBytesLeft, callbackBuffer, session->parent); + + if (callbackSuccessful) + { + for (std::uint8_t j = 0; j < PROTOCOL_BYTES_PER_FRAME; j++) + { + dataBuffer[1 + j] = callbackBuffer[j]; + } + } + else + { + close_session(session, false); + break; + } + } + else + { + messageData = session->sessionMessage.get_data(); + if (0 == session->processedPacketsThisSession) + { + dataBuffer[0] = session->processedPacketsThisSession; + dataBuffer[0] |= (session->sequenceNumber << SEQUENCE_NUMBER_BIT_OFFSET); + dataBuffer[1] = session->get_message_data_length(); + dataBuffer[2] = messageData[0]; + dataBuffer[3] = messageData[1]; + dataBuffer[4] = messageData[2]; + dataBuffer[5] = messageData[3]; + dataBuffer[6] = messageData[4]; + dataBuffer[7] = messageData[5]; + } + else + { + dataBuffer[0] = session->processedPacketsThisSession; + dataBuffer[0] |= (session->sequenceNumber << SEQUENCE_NUMBER_BIT_OFFSET); + + if (numberBytesLeft < PROTOCOL_BYTES_PER_FRAME) + { + dataBuffer[1] = 0xFF; + dataBuffer[2] = 0xFF; + dataBuffer[3] = 0xFF; + dataBuffer[4] = 0xFF; + dataBuffer[5] = 0xFF; + dataBuffer[6] = 0xFF; + dataBuffer[7] = 0xFF; + } + + for (std::uint8_t j = 0; j < numberBytesLeft; j++) + { + dataBuffer[1 + j] = messageData[6 + ((i - 1) * PROTOCOL_BYTES_PER_FRAME) + j]; + } + } + } + if (CANNetworkManager::CANNetwork.send_can_message(session->sessionMessage.get_identifier().get_parameter_group_number(), + dataBuffer.data(), + CAN_DATA_LENGTH, + std::static_pointer_cast(session->sessionMessage.get_source_control_function()), + session->sessionMessage.get_destination_control_function(), + session->sessionMessage.get_identifier().get_priority(), + nullptr, + nullptr)) + { + session->processedPacketsThisSession++; + session->timestamp_ms = SystemTiming::get_timestamp_ms(); + } + else + { + if (SystemTiming::time_expired_ms(session->timestamp_ms, FP_TIMEOUT_MS)) + { + CANStackLogger::CAN_stack_log(CANStackLogger::LoggingLevel::Error, "[FP]: Tx session timed out."); + close_session(session, false); + txSessionCancelled = true; + } + break; + } + } + + if ((!txSessionCancelled) && + (session->processedPacketsThisSession >= session->packetCount)) + { + add_session_history(session); + close_session(session, true); // Session is done! + } + } + break; + } + } + } + +} // namespace isobus diff --git a/src/nmea2000_fast_packet_protocol.hpp b/src/nmea2000_fast_packet_protocol.hpp new file mode 100644 index 0000000..84a2d33 --- /dev/null +++ b/src/nmea2000_fast_packet_protocol.hpp @@ -0,0 +1,218 @@ +//================================================================================================ +/// @file nmea2000_fast_packet_protocol.hpp +/// +/// @brief A protocol that handles the NMEA 2000 (IEC 61162-3) fast packet protocol. +/// +/// @details This higher layer protocol is used primarily on boats and ships to connect +/// equipment such as GPS, auto pilots, depth sounders, navigation instruments, engines, etc. +/// The Fast Packet protocol provides a means to stream up to 223 bytes of data, with the +/// advantage that each frame retains the parameter group number and priority. +/// The first frame transmitted uses 2 bytes to identify sequential Fast Packet parameter groups +/// and sequential frames within a single parameter group transmission. +/// The first byte contains a sequence counter to distinguish consecutive transmission +/// of the same parameter groups and a frame counter set to frame zero. +/// The second byte in the first frame identifies the total size of the +/// parameter group to follow. Successive frames use just single data byte for the +/// sequence counter and the frame counter. +/// +/// @note This library and its authors are not affiliated with the National Marine +/// Electronics Association in any way. +/// +/// @author Adrian Del Grosso +/// +/// @copyright 2022 Adrian Del Grosso +//================================================================================================ + +#ifndef NMEA2000_FAST_PACKET_PROTOCOL_HPP +#define NMEA2000_FAST_PACKET_PROTOCOL_HPP + +#include "can_internal_control_function.hpp" +#include "can_protocol.hpp" + +#if !defined CAN_STACK_DISABLE_THREADS && !defined ARDUINO +#include +#endif + +namespace isobus +{ + /// @brief A protocol that handles the NMEA 2000 fast packet protocol. + class FastPacketProtocol : public CANLibProtocol + { + public: + /// @brief A generic way to initialize a protocol + /// @details The network manager will call a protocol's initialize function + /// when it is first updated, if it has yet to be initialized. + void initialize(CANLibBadge) override; + + /// @brief Similar to add_parameter_group_number_callback but tells the stack to parse those PGNs as Fast Packet + /// @param[in] parameterGroupNumber The PGN to parse as fast packet + /// @param[in] callback The callback that the stack will call when a matching message is received + /// @param[in] parent Generic context variable + /// @param[in] internalControlFunction An internal control function to use as an additional filter for the callback. + /// Only messages destined for the specified ICF will generate a callback. Use nullptr to receive all messages. + void register_multipacket_message_callback(std::uint32_t parameterGroupNumber, CANLibCallback callback, void *parent, std::shared_ptr internalControlFunction = nullptr); + + // @brief Removes a callback previously added with register_multipacket_message_callback + /// @param[in] parameterGroupNumber The PGN to parse as fast packet + /// @param[in] callback The callback that the stack will call when a matching message is received + /// @param[in] parent Generic context variable + /// @param[in] internalControlFunction An internal control function to use as an additional filter for the callback + void remove_multipacket_message_callback(std::uint32_t parameterGroupNumber, CANLibCallback callback, void *parent, std::shared_ptr internalControlFunction = nullptr); + + /// @brief Used to send CAN messages using fast packet + /// @details You have to use this function instead of the network manager + /// because otherwise the CAN stack has no way of knowing to send your message + /// with FP instead of TP. + /// @param[in] parameterGroupNumber The PGN of the message + /// @param[in] data The data to be sent + /// @param[in] messageLength The length of the data to be sent + /// @param[in] source The source control function + /// @param[in] destination The destination control function + /// @param[in] priority The priority to encode in the IDs of the component CAN messages + /// @param[in] txCompleteCallback A callback for when the protocol completes its work + /// @param[in] parentPointer A generic context object for the tx complete and chunk callbacks + /// @param[in] frameChunkCallback A callback to get some data to send + /// @returns true if the message was accepted by the protocol for processing + bool send_multipacket_message(std::uint32_t parameterGroupNumber, + const std::uint8_t *data, + std::uint8_t messageLength, + std::shared_ptr source, + std::shared_ptr destination, + CANIdentifier::CANPriority priority = CANIdentifier::CANPriority::PriorityDefault6, + TransmitCompleteCallback txCompleteCallback = nullptr, + void *parentPointer = nullptr, + DataChunkCallback frameChunkCallback = nullptr); + + /// @brief This will be called by the network manager on every cyclic update of the stack + void update(CANLibBadge) override; + + private: + /// @brief An object for tracking fast packet session state + class FastPacketProtocolSession + { + public: + /// @brief Enumerates the possible session directions, Rx or Tx + enum class Direction + { + Transmit, ///< We are transmitting a message + Receive ///< We are receving a message + }; + + /// @brief A useful way to compare sesson objects to each other for equality + bool operator==(const FastPacketProtocolSession &obj); + + /// @brief Get the total number of bytes that will be sent or received in this session + /// @return The length of the message in number of bytes + std::uint32_t get_message_data_length() const; + + private: + friend class FastPacketProtocol; ///< Allows the TP manager full access + + /// @brief The constructor for a TP session + /// @param[in] sessionDirection Tx or Rx + /// @param[in] canPortIndex The CAN channel index for the session + FastPacketProtocolSession(Direction sessionDirection, std::uint8_t canPortIndex); + + /// @brief The destructor for a TP session + ~FastPacketProtocolSession(); + + CANMessage sessionMessage; ///< A CAN message is used in the session to represent and store data like PGN + TransmitCompleteCallback sessionCompleteCallback; ///< A callback that is to be called when the session is completed + DataChunkCallback frameChunkCallback; ///< A callback that might be used to get chunks of data to send + std::uint32_t frameChunkCallbackMessageLength; ///< The length of the message that is being sent in chunks + void *parent; ///< A generic context variable that helps identify what object callbacks are destined for. Can be nullptr + std::uint32_t timestamp_ms; ///< A timestamp used to track session timeouts + std::uint16_t lastPacketNumber; ///< The last processed sequence number for this set of packets + std::uint8_t packetCount; ///< The total number of packets to receive or send in this session + std::uint8_t processedPacketsThisSession; ///< The total processed packet count for the whole session so far + std::uint8_t sequenceNumber; ///< The sequence number for this PGN + const Direction sessionDirection; ///< Represents Tx or Rx session + }; + + /// @brief A structure for keeping track of past sessions so we can resume with the right session number + struct FastPacketHistory + { + NAME isoName; ///< The ISO name of the internal control function used in a session + std::uint32_t parameterGroupNumber; ///< The PGN of the session being saved + std::uint8_t sequenceNumber; ///< The sequence number to use in the next matching session + }; + + /// @brief Adds a session's info to the history so that we can continue the sequence number later + /// @param[in] session The session to add to the history + void add_session_history(FastPacketProtocolSession *session); + + /// @brief Ends a session and cleans up the memory associated with its metadata + /// @param[in] session The session to close + /// @param[in] successful `true` if the session was closed successfully, otherwise `false` + void close_session(FastPacketProtocolSession *session, bool successfull); + + /// @brief Gets the sequence number to use for a new session based on the history + /// @param[in] session The new session we're starting + /// @returns The new sequence number to use + std::uint8_t get_new_sequence_number(FastPacketProtocolSession *session); + + /// @brief Returns a session that matches the parameters, if one exists + /// @param[in,out] returnedSession The returned session + /// @param[in] parameterGroupNumber The PGN + /// @param[in] source The session source control function + /// @param[in] destination The sesssion destination control function + /// @returns `true` if a session was found that matches, otherwise `false` + bool get_session(FastPacketProtocolSession *&returnedSession, std::uint32_t parameterGroupNumber, std::shared_ptr source, std::shared_ptr destination); + + /// @brief A generic way for a protocol to process a received message + /// @param[in] message A received CAN message + void process_message(const CANMessage &message) override; + + /// @brief A generic way for a protocol to process a received message + /// @param[in] message A received CAN message + /// @param[in] parent Provides the context to the actual FP manager object + static void process_message(const CANMessage &message, void *parent); + + /// @brief Processes end of session callbacks + /// @param[in] session The session we've just completed + /// @param[in] success Denotes if the session was successful + void process_session_complete_callback(FastPacketProtocolSession *session, bool success); + + /// @brief The network manager calls this to see if the protocol can accept a non-raw CAN message for processing + /// @param[in] parameterGroupNumber The PGN of the message + /// @param[in] data The data to be sent + /// @param[in] messageLength The length of the data to be sent + /// @param[in] source The source control function + /// @param[in] destination The destination control function + /// @param[in] transmitCompleteCallback A callback for when the protocol completes its work + /// @param[in] parentPointer A generic context object for the tx complete and chunk callbacks + /// @param[in] frameChunkCallback A callback to get some data to send + /// @returns true if the message was accepted by the protocol for processing + bool protocol_transmit_message(std::uint32_t parameterGroupNumber, + const std::uint8_t *data, + std::uint32_t messageLength, + std::shared_ptr source, + std::shared_ptr destination, + TransmitCompleteCallback transmitCompleteCallback, + void *parentPointer, + DataChunkCallback frameChunkCallback) override; + + /// @brief Updates in-progress sessions + /// @param[in] session The session to process + void update_state_machine(FastPacketProtocolSession *session); + + static constexpr std::uint32_t FP_MIN_PARAMETER_GROUP_NUMBER = 0x1F000; ///< Start of PGNs that can be received via Fast Packet + static constexpr std::uint32_t FP_MAX_PARAMETER_GROUP_NUMBER = 0x1FFFF; ///< End of PGNs that can be received via Fast Packet + static constexpr std::uint32_t FP_TIMEOUT_MS = 750; ///< Protocol timeout in milliseconds + static constexpr std::uint8_t MAX_PROTOCOL_MESSAGE_LENGTH = 223; ///< Max message length based on there being 5 bits of sequence data + static constexpr std::uint8_t FRAME_COUNTER_BIT_MASK = 0x1F; ///< Bit mask for masking out the frame counter + static constexpr std::uint8_t SEQUENCE_NUMBER_BIT_MASK = 0x07; ///< Bit mask for masking out the sequence number bits + static constexpr std::uint8_t SEQUENCE_NUMBER_BIT_OFFSET = 0x05; ///< The bit offset into the first byte of data to get the seq number + static constexpr std::uint8_t PROTOCOL_BYTES_PER_FRAME = 7; ///< The number of payload bytes per frame for all but the first message, which has 6 + + std::vector activeSessions; ///< A list of all active TP sessions + std::vector sessionHistory; ///< Used to keep track of sequence numbers for future sessions + std::vector parameterGroupNumberCallbacks; ///< A list of all parameter group number callbacks that will be parsed as fast packet messages +#if !defined CAN_STACK_DISABLE_THREADS && !defined ARDUINO + std::mutex sessionMutex; ///< A mutex to lock the sessions list in case someone starts a Tx while the stack is processing sessions +#endif + }; + +} // namespace isobus + +#endif // NMEA2000_FAST_PACKET_PROTOCOL_HPP diff --git a/src/platform_endianness.cpp b/src/platform_endianness.cpp new file mode 100644 index 0000000..77add5f --- /dev/null +++ b/src/platform_endianness.cpp @@ -0,0 +1,27 @@ +//================================================================================================ +/// @file platform_endianness.cpp +/// +/// @brief Provides a runtime way to determine platform endianness. +/// Useful when trying to convert bytes in memory (like float*) to a specific endianness. +/// @author Adrian Del Grosso +/// +/// @copyright 2023 Adrian Del Grosso +//================================================================================================ +#include "platform_endianness.hpp" + +#include + +namespace isobus +{ + bool is_little_endian() + { + std::int32_t number = 1; + auto numPtr = reinterpret_cast(&number); + return (numPtr[0] == 1); + } + + bool is_big_endian() + { + return (false == is_little_endian()); + } +} // namespace isobus diff --git a/src/platform_endianness.hpp b/src/platform_endianness.hpp new file mode 100644 index 0000000..359d3b9 --- /dev/null +++ b/src/platform_endianness.hpp @@ -0,0 +1,25 @@ +//================================================================================================ +/// @file platform_endianness.hpp +/// +/// @brief Provides a runtime way to determine platform endianness. +/// Useful when trying to convert bytes in memory (like float*) to a specific endianness. +/// @author Adrian Del Grosso +/// +/// @copyright 2023 Adrian Del Grosso +//================================================================================================ +#ifndef PLATFORM_ENDIANNESS_HPP +#define PLATFORM_ENDIANNESS_HPP + +namespace isobus +{ + /// @brief Returns if the platform is little endian + /// @returns `true` if the platform is little endian, otherwise false + bool is_little_endian(); + + /// @brief Returns if the platform is big endian + /// @returns `true` if the platform is big endian, otherwise false + bool is_big_endian(); + +} // namespace isobus + +#endif //PLATFORM_ENDIANNESS_HPP diff --git a/src/processing_flags.cpp b/src/processing_flags.cpp new file mode 100644 index 0000000..70f48df --- /dev/null +++ b/src/processing_flags.cpp @@ -0,0 +1,62 @@ +//================================================================================================ +/// @file processing_flags.cpp +/// +/// @brief A class that manages 1 bit flags. Handy as a retry machanism for sending CAN messages. +/// @author Adrian Del Grosso +/// +/// @copyright 2022 Adrian Del Grosso +//================================================================================================ + +#include "processing_flags.hpp" + +#include + +namespace isobus +{ + ProcessingFlags::ProcessingFlags(std::uint32_t numberOfFlags, ProcessFlagsCallback processingCallback, void *parentPointer) : + callback(processingCallback), + maxFlag(numberOfFlags), + flagBitfield(nullptr), + parent(parentPointer) + { + const std::uint32_t numberBytes = (numberOfFlags / 8) + 1; + flagBitfield = new std::uint8_t[numberBytes]; + memset(flagBitfield, 0, numberBytes); + } + + ProcessingFlags ::~ProcessingFlags() + { + delete[] flagBitfield; + flagBitfield = nullptr; + } + + void ProcessingFlags::set_flag(std::uint32_t flag) + { + if (flag <= maxFlag) + { + flagBitfield[(flag / 8)] |= (1 << (flag % 8)); + } + } + + void ProcessingFlags::process_all_flags() + { + const std::uint32_t numberBytes = (maxFlag / 8) + 1; + + for (std::uint32_t i = 0; i < numberBytes; i++) + { + if (flagBitfield[i]) + { + for (std::uint8_t j = 0; j < 8; j++) + { + std::uint8_t currentFlag = (flagBitfield[i] & (1 << j)); + + if (currentFlag) + { + flagBitfield[i] &= ~(currentFlag); + callback((8 * i) + j, parent); + } + } + } + } + } +} // namespace isobus diff --git a/src/processing_flags.hpp b/src/processing_flags.hpp new file mode 100644 index 0000000..a6d54c1 --- /dev/null +++ b/src/processing_flags.hpp @@ -0,0 +1,35 @@ +//================================================================================================ +/// @file processing_flags.hpp +/// +/// @brief A class that manages 1 bit flags. Handy as a retry machanism for sending CAN messages. +/// @author Adrian Del Grosso +/// +/// @copyright 2022 Adrian Del Grosso +//================================================================================================ +#ifndef PROCESSING_FLAGS_HPP +#define PROCESSING_FLAGS_HPP + +#include + +namespace isobus +{ + class ProcessingFlags + { + public: + typedef void (*ProcessFlagsCallback)(std::uint32_t flag, void *parentPointer); + + ProcessingFlags(std::uint32_t numberOfFlags, ProcessFlagsCallback processingCallback, void *parentPointer); + ~ProcessingFlags(); + + void set_flag(std::uint32_t flag); + void process_all_flags(); + + private: + ProcessFlagsCallback callback; + const std::uint32_t maxFlag; + std::uint8_t *flagBitfield; + void *parent; + }; +} // namespace isobus + +#endif // PROCESSING_FLAGS_HPP diff --git a/src/system_timing.cpp b/src/system_timing.cpp new file mode 100644 index 0000000..228a443 --- /dev/null +++ b/src/system_timing.cpp @@ -0,0 +1,80 @@ +//================================================================================================ +/// @file system_timing.cpp +/// +/// @brief Utility class for getting system time and handling u32 time rollover +/// @author Adrian Del Grosso +/// +/// @copyright 2022 Adrian Del Grosso +//================================================================================================ + +#include "system_timing.hpp" + +#include +#include + +namespace isobus +{ + std::uint64_t SystemTiming::s_timestamp_ms = static_cast(std::chrono::duration_cast(std::chrono::steady_clock::now().time_since_epoch()).count()); + std::uint64_t SystemTiming::s_timestamp_us = static_cast(std::chrono::duration_cast(std::chrono::steady_clock::now().time_since_epoch()).count()); + + std::uint32_t SystemTiming::get_timestamp_ms() + { + return incrementing_difference(static_cast(static_cast(std::chrono::duration_cast(std::chrono::steady_clock::now().time_since_epoch()).count()) & std::numeric_limits::max()), static_cast(s_timestamp_ms)); + } + + std::uint64_t SystemTiming::get_timestamp_us() + { + return incrementing_difference(static_cast(static_cast(std::chrono::duration_cast(std::chrono::steady_clock::now().time_since_epoch()).count()) & std::numeric_limits::max()), s_timestamp_us); + } + + std::uint32_t SystemTiming::get_time_elapsed_ms(std::uint32_t timestamp_ms) + { + return (get_timestamp_ms() - timestamp_ms); + } + + std::uint64_t SystemTiming::get_time_elapsed_us(std::uint64_t timestamp_us) + { + return (get_timestamp_us() - timestamp_us); + } + + bool SystemTiming::time_expired_ms(std::uint32_t timestamp_ms, std::uint32_t timeout_ms) + { + return (get_time_elapsed_ms(timestamp_ms) >= timeout_ms); + } + + bool SystemTiming::time_expired_us(std::uint64_t timestamp_us, std::uint64_t timeout_us) + { + return (get_time_elapsed_us(timestamp_us) >= timeout_us); + } + + std::uint32_t SystemTiming::incrementing_difference(std::uint32_t currentValue, std::uint32_t previousValue) + { + std::uint32_t retVal; + + if (currentValue >= previousValue) + { + retVal = currentValue - previousValue; + } + else + { + retVal = (std::numeric_limits::max() - previousValue) + currentValue + 1; + } + return retVal; + } + + std::uint64_t SystemTiming::incrementing_difference(std::uint64_t currentValue, std::uint64_t previousValue) + { + std::uint64_t retVal; + + if (currentValue >= previousValue) + { + retVal = currentValue - previousValue; + } + else + { + retVal = (std::numeric_limits::max() - previousValue) + currentValue + 1; + } + return retVal; + } + +} diff --git a/src/system_timing.hpp b/src/system_timing.hpp new file mode 100644 index 0000000..962c3df --- /dev/null +++ b/src/system_timing.hpp @@ -0,0 +1,33 @@ +//================================================================================================ +/// @file system_timing.cpp +/// +/// @brief Utility class for getting system time and handling u32 time rollover +/// @author Adrian Del Grosso +/// +/// @copyright 2022 Adrian Del Grosso +//================================================================================================ + +#include + +namespace isobus +{ + class SystemTiming + { + public: + static std::uint32_t get_timestamp_ms(); + static std::uint64_t get_timestamp_us(); + + static std::uint32_t get_time_elapsed_ms(std::uint32_t timestamp_ms); + static std::uint64_t get_time_elapsed_us(std::uint64_t timestamp_us); + + static bool time_expired_ms(std::uint32_t timestamp_ms, std::uint32_t timeout_ms); + static bool time_expired_us(std::uint64_t timestamp_us, std::uint64_t timeout_us); + + private: + static std::uint32_t incrementing_difference(std::uint32_t currentValue, std::uint32_t previousValue); + static std::uint64_t incrementing_difference(std::uint64_t currentValue, std::uint64_t previousValue); + static std::uint64_t s_timestamp_ms; + static std::uint64_t s_timestamp_us; + }; + +} // namespace isobus diff --git a/src/to_string.hpp b/src/to_string.hpp new file mode 100644 index 0000000..404d597 --- /dev/null +++ b/src/to_string.hpp @@ -0,0 +1,33 @@ +//================================================================================================ +/// @file to_string.hpp +/// +/// @brief A compatibility template to replace `std::to_string` +/// @details Some compilers don't support `std::to_string` so this file is meant to abstract it +/// away with a workaround if it's not supported. This solution was inspired by Catch2's +/// implementation. +/// @author Adrian Del Grosso +/// +/// @copyright 2022 Adrian Del Grosso +//================================================================================================ +#ifndef TO_STRING_HPP +#define TO_STRING_HPP + +#include +#include + +namespace isobus +{ + template + /// @brief A replacement for std::to_string + /// @tparam T The data type + /// @param t The thing to convert to string + /// @returns the string form of `t` + std::string to_string(T const &t) + { + std::ostringstream oss; + oss << t; + return oss.str(); + } +} // namespace isobus_utils + +#endif // TO_STRING_HPP