From 8d704b1bbbc2ee1f33c704a55154e876014fc363 Mon Sep 17 00:00:00 2001 From: Kyle Johannes Date: Tue, 22 Aug 2023 20:13:34 +0000 Subject: [PATCH] LiveData: Initial implementation Add support for live data subscription via Device::subscribeLiveData() and Device::unsubscribeLiveData(). The live data API can be used to subscribe to individual "signals", a full list of which can be found in LiveDataValueType. --- CMakeLists.txt | 4 + communication/decoder.cpp | 5 + communication/encoder.cpp | 13 ++ communication/livedata.cpp | 22 ++ communication/message/livedatamessage.cpp | 15 ++ communication/packet/livedatapacket.cpp | 129 ++++++++++++ device/device.cpp | 143 +++++++++++++ examples/CMakeLists.txt | 5 + examples/cpp/livedata/CMakeLists.txt | 27 +++ examples/cpp/livedata/src/LiveDataExample.cpp | 91 +++++++++ include/icsneo/api/event.h | 10 + include/icsneo/communication/command.h | 13 +- include/icsneo/communication/livedata.h | 146 ++++++++++++++ .../communication/message/livedatamessage.h | 44 ++++ .../icsneo/communication/message/message.h | 1 + .../communication/packet/livedatapacket.h | 27 +++ include/icsneo/device/device.h | 8 + .../device/tree/neovifire3/neovifire3.h | 2 + .../icsneo/device/tree/neovired2/neovired2.h | 2 + test/livedataencoderdecodertest.cpp | 188 ++++++++++++++++++ 20 files changed, 894 insertions(+), 1 deletion(-) create mode 100644 communication/livedata.cpp create mode 100644 communication/message/livedatamessage.cpp create mode 100644 communication/packet/livedatapacket.cpp create mode 100644 examples/cpp/livedata/CMakeLists.txt create mode 100644 examples/cpp/livedata/src/LiveDataExample.cpp create mode 100644 include/icsneo/communication/livedata.h create mode 100644 include/icsneo/communication/message/livedatamessage.h create mode 100644 include/icsneo/communication/packet/livedatapacket.h create mode 100644 test/livedataencoderdecodertest.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 9efd19c..3be4b0e 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -234,6 +234,7 @@ set(SRC_FILES communication/message/neomessage.cpp communication/message/ethphymessage.cpp communication/message/linmessage.cpp + communication/message/livedatamessage.cpp communication/packet/flexraypacket.cpp communication/packet/canpacket.cpp communication/packet/a2bpacket.cpp @@ -241,6 +242,7 @@ set(SRC_FILES communication/packet/versionpacket.cpp communication/packet/iso9141packet.cpp communication/packet/ethphyregpacket.cpp + communication/packet/livedatapacket.cpp communication/packet/logicaldiskinfopacket.cpp communication/packet/wivicommandpacket.cpp communication/packet/i2cpacket.cpp @@ -257,6 +259,7 @@ set(SRC_FILES communication/multichannelcommunication.cpp communication/communication.cpp communication/driver.cpp + communication/livedata.cpp device/extensions/flexray/extension.cpp device/extensions/flexray/controller.cpp device/idevicesettings.cpp @@ -468,6 +471,7 @@ if(LIBICSNEO_BUILD_TESTS) test/linencoderdecodertest.cpp test/a2bencoderdecodertest.cpp test/mdioencoderdecodertest.cpp + test/livedataencoderdecodertest.cpp ) target_link_libraries(libicsneo-tests gtest gtest_main) diff --git a/communication/decoder.cpp b/communication/decoder.cpp index 45a892a..e7a3ea7 100644 --- a/communication/decoder.cpp +++ b/communication/decoder.cpp @@ -14,6 +14,7 @@ #include "icsneo/communication/message/linmessage.h" #include "icsneo/communication/message/mdiomessage.h" #include "icsneo/communication/message/extendeddatamessage.h" +#include "icsneo/communication/message/livedatamessage.h" #include "icsneo/communication/command.h" #include "icsneo/device/device.h" #include "icsneo/communication/packet/canpacket.h" @@ -32,6 +33,7 @@ #include "icsneo/communication/packet/supportedfeaturespacket.h" #include "icsneo/communication/packet/mdiopacket.h" #include "icsneo/communication/packet/genericbinarystatuspacket.h" +#include "icsneo/communication/packet/livedatapacket.h" #include using namespace icsneo; @@ -279,6 +281,9 @@ bool Decoder::decode(std::shared_ptr& result, const std::shared_ptr(resp.command, resp.returnCode); return true; + case ExtendedCommand::LiveData: + result = HardwareLiveDataPacket::DecodeToMessage(packet->data, report); + return true; default: // No defined handler, treat this as a RawMessage break; diff --git a/communication/encoder.cpp b/communication/encoder.cpp index 2a4d514..25c10dc 100644 --- a/communication/encoder.cpp +++ b/communication/encoder.cpp @@ -1,6 +1,8 @@ #include "icsneo/communication/encoder.h" #include "icsneo/communication/message/ethernetmessage.h" +#include "icsneo/communication/message/livedatamessage.h" #include "icsneo/communication/message/main51message.h" +#include "icsneo/communication/packet/livedatapacket.h" #include "icsneo/communication/packet/ethernetpacket.h" #include "icsneo/communication/packet/iso9141packet.h" #include "icsneo/communication/packet/canpacket.h" @@ -197,6 +199,17 @@ bool Encoder::encode(const Packetizer& packetizer, std::vector& result, return false; break; } + case Message::Type::LiveData: { + auto liveDataMsg = std::dynamic_pointer_cast(message); + if(!liveDataMsg) { + report(APIEvent::Type::MessageFormattingError, APIEvent::Severity::Error); + return false; // The message was not a properly formed LiveDataMessage + } + if(!HardwareLiveDataPacket::EncodeFromMessage(*liveDataMsg, result, report)) + return false; + result = packetizer.packetWrap(result, false); + return true; + } break; } diff --git a/communication/livedata.cpp b/communication/livedata.cpp new file mode 100644 index 0000000..883fb20 --- /dev/null +++ b/communication/livedata.cpp @@ -0,0 +1,22 @@ +#include "icsneo/communication/livedata.h" +namespace icsneo { + +namespace LiveDataUtil { + +LiveDataHandle getNewHandle() { + static LiveDataHandle currentHandle = 0; + ++currentHandle; + if(currentHandle == std::numeric_limits::max()) { + EventManager::GetInstance().add(APIEvent::Type::LiveDataInvalidHandle, APIEvent::Severity::Error); + currentHandle = 1; + } + return currentHandle; +} + +double liveDataValueToDouble(const LiveDataValue& val) { + constexpr double liveDataFixedPointToDouble = 0.00000000023283064365386962890625; + return val.value * liveDataFixedPointToDouble; +} + +} // namespace LiveDataUtil +} // namespace icsneo \ No newline at end of file diff --git a/communication/message/livedatamessage.cpp b/communication/message/livedatamessage.cpp new file mode 100644 index 0000000..a1dfbc6 --- /dev/null +++ b/communication/message/livedatamessage.cpp @@ -0,0 +1,15 @@ +#include "icsneo/communication/message/livedatamessage.h" +#include "icsneo/communication/livedata.h" + +namespace icsneo +{ + +void LiveDataCommandMessage::appendSignalArg(LiveDataValueType valueType) { + auto& arg = args.emplace_back(std::make_shared()); + arg->objectType = LiveDataObjectType::MISC; + arg->objectIndex = 0u; + arg->signalIndex = 0u; + arg->valueType = valueType; +} + +} // namespace icsneo diff --git a/communication/packet/livedatapacket.cpp b/communication/packet/livedatapacket.cpp new file mode 100644 index 0000000..4af80e9 --- /dev/null +++ b/communication/packet/livedatapacket.cpp @@ -0,0 +1,129 @@ +#include "icsneo/communication/packet/livedatapacket.h" +#include "icsneo/communication/message/livedatamessage.h" +#include +#include + +namespace icsneo { + +std::shared_ptr HardwareLiveDataPacket::DecodeToMessage(const std::vector& bytes, const device_eventhandler_t& report) { + if(bytes.empty() || (bytes.size() < (sizeof(LiveDataHeader) + sizeof(ExtResponseHeader)))) { + report(APIEvent::Type::RequiredParameterNull, APIEvent::Severity::Error); + return nullptr; + } + + const auto header = reinterpret_cast(bytes.data()); + if(ExtendedCommand::LiveData != static_cast(header->command)) { + report(APIEvent::Type::LiveDataInvalidCommand, APIEvent::Severity::Error); + return nullptr; + } + + const auto ldHeader = reinterpret_cast(bytes.data() + sizeof(ExtResponseHeader)); + // Versioning check to avoid bad data interpretation between disparate libicsneo and firmware versions + if(icsneo::LiveDataUtil::LiveDataVersion != ldHeader->version) { + report(APIEvent::Type::LiveDataVersionMismatch, APIEvent::Severity::Error); + return nullptr; + } + switch(LiveDataCommand(ldHeader->cmd)) { + case LiveDataCommand::RESPONSE: { + auto retMsg = std::make_shared(); + const auto responseBytes = reinterpret_cast(ldHeader); + retMsg->handle = responseBytes->handle; + retMsg->cmd = static_cast(responseBytes->cmd); + retMsg->numArgs = responseBytes->numArgs; + for(uint32_t i = 0; i < retMsg->numArgs; ++i) { + retMsg->values.emplace_back(std::make_shared(responseBytes->values[i])); + } + return retMsg; + } + case LiveDataCommand::STATUS: { + auto retMsg = std::make_shared(); + const auto responseBytes = reinterpret_cast(ldHeader); + retMsg->handle = responseBytes->handle; + retMsg->cmd = static_cast(responseBytes->cmd); + retMsg->status = responseBytes->status; + retMsg->requestedCommand = static_cast(responseBytes->requestedCommand); + return retMsg; + } + default: { + report(APIEvent::Type::LiveDataInvalidCommand, APIEvent::Severity::Error); + break; + } + } + return nullptr; +} + +bool HardwareLiveDataPacket::EncodeFromMessage(LiveDataMessage& message, std::vector& bytestream, const device_eventhandler_t& report) { + uint16_t payloadSize = 0; + switch(message.cmd) { + case LiveDataCommand::SUBSCRIBE: { + auto commandMsg = reinterpret_cast(&message); + const auto numArgs = commandMsg->args.size(); + if(numArgs) { + payloadSize = static_cast(sizeof(LiveDataSubscribe) + (sizeof(LiveDataArgument) * (numArgs-1))); + bytestream.resize((payloadSize + sizeof(ExtendedCommandHeader)),0); + LiveDataSubscribe* out = reinterpret_cast(bytestream.data() + sizeof(ExtendedCommandHeader)); + out->version = icsneo::LiveDataUtil::LiveDataVersion; + out->cmd = static_cast(commandMsg->cmd); + if(!commandMsg->handle) + commandMsg->handle = LiveDataUtil::getNewHandle(); + out->handle = commandMsg->handle; + out->numArgs = static_cast(commandMsg->args.size()); + out->freqMs = static_cast(commandMsg->updatePeriod.count()); + out->expireMs = static_cast(commandMsg->expirationTime.count()); + for(size_t i = 0; i < numArgs; ++i) { + out->args[i].objectType = commandMsg->args[i]->objectType; + out->args[i].objectIndex = commandMsg->args[i]->objectIndex; + out->args[i].signalIndex = commandMsg->args[i]->signalIndex; + out->args[i].valueType = commandMsg->args[i]->valueType; + } + } else { + report(APIEvent::Type::LiveDataInvalidArgument, APIEvent::Severity::Error); + return false; + } + break; + } + case LiveDataCommand::UNSUBSCRIBE: { + payloadSize = sizeof(LiveDataHeader); + bytestream.resize((payloadSize + sizeof(ExtendedCommandHeader)),0); + auto ldUnsubMsg = reinterpret_cast(bytestream.data() + sizeof(ExtendedCommandHeader)); + ldUnsubMsg->version = static_cast(icsneo::LiveDataUtil::LiveDataVersion); + ldUnsubMsg->cmd = static_cast(message.cmd); + ldUnsubMsg->handle = static_cast(message.handle); + break; + } + case LiveDataCommand::CLEAR_ALL: { + payloadSize = sizeof(LiveDataHeader); + bytestream.resize((payloadSize + sizeof(ExtendedCommandHeader)),0); + auto clearMsg = reinterpret_cast(bytestream.data() + sizeof(ExtendedCommandHeader)); + clearMsg->version = static_cast(icsneo::LiveDataUtil::LiveDataVersion); + clearMsg->cmd = static_cast(message.cmd); + break; + } + default: { + report(APIEvent::Type::LiveDataInvalidCommand, APIEvent::Severity::Error); + return false; + } + } + // Send as a long message without setting the NetID to RED first + // Size in long format is the size of the entire packet + // So +1 for AA header, +1 for short format header (only netID) + // +2 for long format size, +1 for the Main51 extended command + // +2 for the extended subcommand, +2 for the payload length + uint16_t fullSize = static_cast(1 + sizeof(ExtendedCommandHeader) + payloadSize); + + ExtendedCommandHeader* header = reinterpret_cast(bytestream.data()); + if(!header) { + report(APIEvent::Type::LiveDataEncoderError, APIEvent::Severity::Error); + return false; + } + + header->netid = static_cast(Network::NetID::Main51); + header->fullLength = fullSize; + header->command = static_cast(Command::Extended); + header->extendedCommand = static_cast(ExtendedCommand::LiveData); + header->payloadLength = payloadSize; + + return true; +} + +} // namespace icsneo diff --git a/device/device.cpp b/device/device.cpp index b3ce976..dac7d01 100644 --- a/device/device.cpp +++ b/device/device.cpp @@ -301,6 +301,9 @@ bool Device::open(OpenFlags flags, OpenStatusHandler handler) { com->removeMessageCallback(messageReceivedCallbackID); }); + if(supportsLiveData()) + clearAllLiveData(); + return true; } @@ -1871,5 +1874,145 @@ bool Device::readBinaryFile(std::ostream& stream, uint16_t binaryIndex) { return false; } } + return true; +} + +bool Device::subscribeLiveData(std::shared_ptr message) { + if(!supportsLiveData()) { + report(APIEvent::Type::LiveDataNotSupported, APIEvent::Severity::Error); + return false; + } + if(!isOpen()) { + report(APIEvent::Type::DeviceCurrentlyClosed, APIEvent::Severity::Error); + return false; + } + if((message->args.size() > MAX_LIVE_DATA_ENTRIES) || message->args.empty()) { + report(APIEvent::Type::LiveDataInvalidArgument, APIEvent::Severity::Error); + return false; + } + + std::vector bytes; + if(!com->encoder->encode(*com->packetizer, bytes, message)) { + report(APIEvent::Type::LiveDataEncoderError, APIEvent::Severity::Error); + return false; + } + + std::shared_ptr response = com->waitForMessageSync( + [this, &bytes](){ return com->sendPacket(bytes); }, + std::make_shared(Message::Type::LiveData)); + + if(response) { + auto statusMsg = std::dynamic_pointer_cast(response); + if(statusMsg->requestedCommand == message->cmd) { + switch(statusMsg->status) { + case LiveDataStatus::SUCCESS: + return true; + case LiveDataStatus::ERR_DUPLICATE: + case LiveDataStatus::ERR_HANDLE: + { + report(APIEvent::Type::LiveDataInvalidHandle, APIEvent::Severity::Error); + return false; + } + case LiveDataStatus::ERR_FULL: + { + report(APIEvent::Type::LiveDataMaxSignalsReached, APIEvent::Severity::Error); + return false; + } + case LiveDataStatus::ERR_UNKNOWN_COMMAND: + { + report(APIEvent::Type::LiveDataCommandFailed, APIEvent::Severity::Error); + return false; + } + default: + break; + } + } + } + report(APIEvent::Type::LiveDataNoDeviceResponse, APIEvent::Severity::Error); + return false; +} + +bool Device::unsubscribeLiveData(const LiveDataHandle& handle) { + if(!supportsLiveData()) { + report(APIEvent::Type::LiveDataNotSupported, APIEvent::Severity::Error); + return false; + } + if(!isOpen()) { + report(APIEvent::Type::DeviceCurrentlyClosed, APIEvent::Severity::Error); + return false; + } + if(!handle) { + report(APIEvent::Type::RequiredParameterNull, APIEvent::Severity::Error); + return false; + } + auto msg = std::make_shared(); + msg->cmd = LiveDataCommand::UNSUBSCRIBE; + msg->handle = handle; + std::vector bytes; + if(!com->encoder->encode(*com->packetizer, bytes, msg)) { + report(APIEvent::Type::LiveDataEncoderError, APIEvent::Severity::Error); + return false; + } + + std::shared_ptr response = com->waitForMessageSync( + [this, &bytes](){ return com->sendPacket(bytes); }, + std::make_shared(Message::Type::LiveData)); + + if(!response) { + report(APIEvent::Type::LiveDataNoDeviceResponse, APIEvent::Severity::Error); + return false; + } + + auto statusMsg = std::dynamic_pointer_cast(response); + if(!statusMsg || statusMsg->requestedCommand != msg->cmd) { + report(APIEvent::Type::MessageFormattingError, APIEvent::Severity::Error); + return false; + } + + if(statusMsg->status != LiveDataStatus::SUCCESS) { + report(APIEvent::Type::LiveDataCommandFailed, APIEvent::Severity::Error); + return false; + } + + return true; +} + +bool Device::clearAllLiveData() { + if(!supportsLiveData()) { + report(APIEvent::Type::LiveDataNotSupported, APIEvent::Severity::Error); + return false; + } + if(!isOpen()) { + report(APIEvent::Type::DeviceCurrentlyClosed, APIEvent::Severity::Error); + return false; + } + + auto msg = std::make_shared(); + msg->cmd = LiveDataCommand::CLEAR_ALL; + std::vector bytes; + if(!com->encoder->encode(*com->packetizer, bytes, msg)) { + report(APIEvent::Type::LiveDataEncoderError, APIEvent::Severity::Error); + return false; + } + std::shared_ptr response = com->waitForMessageSync( + [this, &bytes](){ return com->sendPacket(bytes); }, + std::make_shared(Message::Type::LiveData)); + + if(!response) { + report(APIEvent::Type::LiveDataNoDeviceResponse, APIEvent::Severity::Error); + return false; + } + + auto statusMsg = std::dynamic_pointer_cast(response); + if(!statusMsg || statusMsg->requestedCommand != msg->cmd) { + report(APIEvent::Type::MessageFormattingError, APIEvent::Severity::Error); + return false; + } + + if(statusMsg->status != LiveDataStatus::SUCCESS) { + report(APIEvent::Type::LiveDataCommandFailed, APIEvent::Severity::Error); + return false; + } + return true; } \ No newline at end of file diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt index 7329681..8ca17d5 100644 --- a/examples/CMakeLists.txt +++ b/examples/CMakeLists.txt @@ -5,6 +5,7 @@ option(LIBICSNEO_BUILD_CPP_SIMPLE_EXAMPLE "Build the simple C++ example." ON) option(LIBICSNEO_BUILD_CPP_INTERACTIVE_EXAMPLE "Build the command-line interactive C++ example." ON) option(LIBICSNEO_BUILD_CPP_A2B_EXAMPLE "Build the A2B example." ON) option(LIBICSNEO_BUILD_CPP_LIN_EXAMPLE "Build the LIN example." ON) +option(LIBICSNEO_BUILD_CPP_LIVEDATA_EXAMPLE "Build the Live Data example." ON) option(LIBICSNEO_BUILD_CPP_COREMINI_EXAMPLE "Build the Coremini example." ON) option(LIBICSNEO_BUILD_CPP_MDIO_EXAMPLE "Build the MDIO example." ON) @@ -40,6 +41,10 @@ if(LIBICSNEO_BUILD_CPP_LIN_EXAMPLE) add_subdirectory(cpp/lin) endif() +if(LIBICSNEO_BUILD_CPP_LIVEDATA_EXAMPLE) + add_subdirectory(cpp/livedata) +endif() + if(LIBICSNEO_BUILD_CPP_COREMINI_EXAMPLE) add_subdirectory(cpp/coremini) endif() diff --git a/examples/cpp/livedata/CMakeLists.txt b/examples/cpp/livedata/CMakeLists.txt new file mode 100644 index 0000000..0bcc2c4 --- /dev/null +++ b/examples/cpp/livedata/CMakeLists.txt @@ -0,0 +1,27 @@ +cmake_minimum_required(VERSION 3.2) +project(libicsneocpp-livedata VERSION 0.1.0) + +set(CMAKE_CXX_STANDARD_REQUIRED 11) + +include(GNUInstallDirs) + +# Add an include directory like so if desired +#include_directories(${CMAKE_CURRENT_SOURCE_DIR}/include) + +# Enable Warnings +if(MSVC) + # Force to always compile with W4 + if(CMAKE_CXX_FLAGS MATCHES "/W[0-4]") + string(REGEX REPLACE "/W[0-4]" "/W4" CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS}") + else() + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} /W4") + endif() +else() #if(CMAKE_COMPILER_IS_GNUCC OR CMAKE_COMPILER_IS_GNUCXX) + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wno-switch -Wno-unknown-pragmas") +endif() + +# Add libicsneo, usually a git submodule within your project works well +#add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/../third-party/libicsneo ${CMAKE_CURRENT_BINARY_DIR}/third-party/libicsneo) + +add_executable(libicsneocpp-livedata src/LiveDataExample.cpp) +target_link_libraries(libicsneocpp-livedata icsneocpp) \ No newline at end of file diff --git a/examples/cpp/livedata/src/LiveDataExample.cpp b/examples/cpp/livedata/src/LiveDataExample.cpp new file mode 100644 index 0000000..cb46ae7 --- /dev/null +++ b/examples/cpp/livedata/src/LiveDataExample.cpp @@ -0,0 +1,91 @@ +#include +#include +#include +#include + +#include "icsneo/icsneocpp.h" +#include "icsneo/communication/message/livedatamessage.h" +#include "icsneo/communication/livedata.h" + +int main() { + // Print version + std::cout << "Running libicsneo " << icsneo::GetVersion() << std::endl; + std::cout << "\nFinding devices... " << std::flush; + auto devices = icsneo::FindAllDevices(); // This is type std::vector> + std::cout << "OK, " << devices.size() << " device" << (devices.size() == 1 ? "" : "s") << " found" << std::endl; + + // List off the devices + for(auto& device : devices) + std::cout << '\t' << device->describe() << " @ Handle " << device->getNeoDevice().handle << std::endl; + std::cout << std::endl; + + for(auto& device : devices) { + std::cout << "Connecting to " << device->describe() << "... "; + bool ret = device->open(); + if(!ret) { // Failed to open + std::cout << "FAIL" << std::endl; + std::cout << icsneo::GetLastError() << std::endl << std::endl; + continue; + } + std::cout << "OK" << std::endl; + + // Create a subscription message for the GPS signals + std::cout << "\tSending a live data subscribe command... "; + auto msg = std::make_shared(); + msg->appendSignalArg(icsneo::LiveDataValueType::GPS_LATITUDE); + msg->appendSignalArg(icsneo::LiveDataValueType::GPS_LONGITUDE); + msg->appendSignalArg(icsneo::LiveDataValueType::GPS_ACCURACY); + msg->cmd = icsneo::LiveDataCommand::SUBSCRIBE; + msg->handle = icsneo::LiveDataUtil::getNewHandle(); + msg->updatePeriod = std::chrono::milliseconds(100); + msg->expirationTime = std::chrono::milliseconds(0); + // Transmit the subscription message + ret = device->subscribeLiveData(msg); + std::cout << (ret ? "OK" : "FAIL") << std::endl; + + // Register a handler that uses the data after it arrives every ~100ms + std::cout << "\tStreaming messages for 3 seconds... " << std::endl << std::endl; + auto filter = std::make_shared(icsneo::Message::Type::LiveData); + auto handler = device->addMessageCallback(std::make_shared(filter, [&msg](std::shared_ptr message) { + auto ldMsg = std::dynamic_pointer_cast(message); + switch(ldMsg->cmd) { + case icsneo::LiveDataCommand::STATUS: { + auto msg2 = std::dynamic_pointer_cast(message); + std::cout << "[Handle] " << ldMsg->handle << std::endl; + std::cout << "[Requested Command] " << msg2->requestedCommand << std::endl; + std::cout << "[Status] " << msg2->status << std::endl << std::endl; + break; + } + case icsneo::LiveDataCommand::RESPONSE: { + auto valueMsg = std::dynamic_pointer_cast(message); + if((valueMsg->handle == msg->handle) && (valueMsg->values.size() == msg->args.size())) + { + std::cout << "[Handle] " << msg->handle << std::endl; + std::cout << "[Values] " << valueMsg->numArgs << std::endl; + for(uint32_t i = 0; i < valueMsg->numArgs; ++i) { + std::cout << "[" << msg->args[i]->valueType << "] "; + auto scaledValue = icsneo::LiveDataUtil::liveDataValueToDouble(*valueMsg->values[i]); + std::cout << scaledValue << std::endl; + } + std::cout << std::endl; + } + break; + } + default: // Ignoring other commands + break; + } + })); + // Run handler for three seconds to observe the signal data + std::this_thread::sleep_for(std::chrono::seconds(3)); + // Unsubscribe from the GPS signals and run handler for one more second + // Unsubscription only requires a valid in-use handle, in this case from our previous subscription + ret = device->unsubscribeLiveData(msg->handle); + // The handler should no longer print values + std::this_thread::sleep_for(std::chrono::seconds(1)); + device->removeMessageCallback(handler); + std::cout << "\tDisconnecting... "; + ret = device->close(); + std::cout << (ret ? "OK\n" : "FAIL\n") << std::endl; + } + return 0; +} \ No newline at end of file diff --git a/include/icsneo/api/event.h b/include/icsneo/api/event.h index 3b9140b..5e40b79 100644 --- a/include/icsneo/api/event.h +++ b/include/icsneo/api/event.h @@ -94,6 +94,16 @@ public: CoreminiUploadVersionMismatch = 0x2040, DiskNotConnected = 0x2041, UnexpectedResponse = 0x2042, + LiveDataInvalidHandle = 0x2043, + LiveDataInvalidCommand = 0x2044, + LiveDataInvalidArgument = 0x2045, + LiveDataVersionMismatch = 0x2046, + LiveDataNoDeviceResponse = 0x2047, + LiveDataMaxSignalsReached = 0x2048, + LiveDataCommandFailed = 0x2049, + LiveDataEncoderError = 0x2050, + LiveDataDecoderError = 0x2051, + LiveDataNotSupported = 0x2052, // Transport Events FailedToRead = 0x3000, diff --git a/include/icsneo/communication/command.h b/include/icsneo/communication/command.h index f931e21..d74d7e1 100644 --- a/include/icsneo/communication/command.h +++ b/include/icsneo/communication/command.h @@ -35,7 +35,7 @@ enum class Command : uint8_t { ExtendedData = 0xF2, // Previously known as RED_CMD_EXTENDED_DATA FlexRayControl = 0xF3, CoreMiniPreload = 0xF4, // Previously known as RED_CMD_COREMINI_PRELOAD - PHYControlRegisters = 0xEF + PHYControlRegisters = 0xEF, }; enum class ExtendedCommand : uint16_t { @@ -53,6 +53,7 @@ enum class ExtendedCommand : uint16_t { Reboot = 0x001C, SetUploadedFlag = 0x0027, GenericBinaryInfo = 0x0030, + LiveData = 0x0035, }; enum class ExtendedResponse : int32_t { @@ -68,6 +69,16 @@ enum class ExtendedDataSubCommand : uint32_t { GenericBinaryRead = 13, }; +#pragma pack(push,1) +struct ExtendedCommandHeader { + uint8_t netid; // should be Main51 + uint16_t fullLength; + uint8_t command; // should be Command::Extended + uint16_t extendedCommand; + uint16_t payloadLength; +}; +#pragma pack(pop) + } #endif // __cplusplus diff --git a/include/icsneo/communication/livedata.h b/include/icsneo/communication/livedata.h new file mode 100644 index 0000000..da9c968 --- /dev/null +++ b/include/icsneo/communication/livedata.h @@ -0,0 +1,146 @@ +#ifndef __LIVEDATA_H__ +#define __LIVEDATA_H__ +#ifdef __cplusplus + +#include +#include +#include +#include "icsneo/communication/command.h" +#include "icsneo/api/eventmanager.h" + +namespace icsneo { + +typedef uint32_t LiveDataHandle; +static constexpr size_t MAX_LIVE_DATA_ENTRIES = 20; + +enum class LiveDataCommand : uint32_t { + STATUS = 0, + SUBSCRIBE, + UNSUBSCRIBE, + RESPONSE, + CLEAR_ALL, +}; + +enum class LiveDataStatus : uint32_t { + SUCCESS = 0, + ERR_UNKNOWN_COMMAND, + ERR_HANDLE, + ERR_DUPLICATE, + ERR_FULL +}; + +enum LiveDataObjectType : uint16_t { + MISC = 8, + SNA = UINT16_MAX, +}; + +enum class LiveDataValueType : uint32_t { + GPS_LATITUDE = 2, + GPS_LONGITUDE, + GPS_ALTITUDE, + GPS_SPEED, + GPS_VALID, + GPS_ENABLE = 62, + GPS_ACCURACY = 120, + GPS_BEARING = 121, + GPS_TIME = 122, + GPS_TIME_VALID = 123, +}; + +inline std::ostream& operator<<(std::ostream& os, const LiveDataCommand cmd) { + switch (cmd) { + case LiveDataCommand::STATUS: return os << "Status"; + case LiveDataCommand::SUBSCRIBE: return os << "Subscribe"; + case LiveDataCommand::UNSUBSCRIBE: return os << "Unsubscribe"; + case LiveDataCommand::RESPONSE: return os << "Response"; + } + return os; +} + +inline std::ostream& operator<<(std::ostream& os, const LiveDataStatus cmd) { + switch (cmd) { + case LiveDataStatus::SUCCESS: return os << "Success"; + case LiveDataStatus::ERR_UNKNOWN_COMMAND: return os << "Error: Unknown Command"; + case LiveDataStatus::ERR_HANDLE: return os << "Error: Handle"; + case LiveDataStatus::ERR_DUPLICATE: return os << "Error: Duplicate"; + case LiveDataStatus::ERR_FULL: return os << "Error: Argument limit reached"; + } + return os; +} + +inline std::ostream& operator<<(std::ostream& os, const LiveDataValueType cmd) { + switch (cmd) { + case LiveDataValueType::GPS_LATITUDE: return os << "GPS Latitude"; + case LiveDataValueType::GPS_LONGITUDE: return os << "GPS Longitude"; + case LiveDataValueType::GPS_ALTITUDE: return os << "GPS Altitude"; + case LiveDataValueType::GPS_SPEED: return os << "GPS Speed"; + case LiveDataValueType::GPS_VALID: return os << "GPS Valid"; + case LiveDataValueType::GPS_ENABLE: return os << "GPS Enabled"; + case LiveDataValueType::GPS_ACCURACY: return os << "GPS Accuracy"; + case LiveDataValueType::GPS_BEARING: return os << "GPS Bearing"; + case LiveDataValueType::GPS_TIME: return os << "GPS Time"; + case LiveDataValueType::GPS_TIME_VALID: return os << "GPS Time Valid"; + } + return os; +} + +#pragma pack(push,2) +struct LiveDataHeader { + uint32_t version; // See LiveDataVersion + uint32_t cmd; + uint32_t handle; +}; + +struct LiveDataArgument { + LiveDataObjectType objectType; + uint32_t objectIndex; + uint32_t signalIndex; + LiveDataValueType valueType; +}; + +struct LiveDataValueHeader { + uint16_t length; // Number of bytes to follow header + uint8_t reserved[2]; +}; + +typedef struct +{ + LiveDataValueHeader header; + int64_t value; +} LiveDataValue; + +struct LiveDataValueResponse : public LiveDataHeader { + uint32_t numArgs; + LiveDataValue values[1]; +}; + +struct LiveDataStatusResponse : public LiveDataHeader { + LiveDataCommand requestedCommand; + LiveDataStatus status; +}; + +struct LiveDataSubscribe : public LiveDataHeader { + uint32_t numArgs; + uint32_t freqMs; + uint32_t expireMs; + LiveDataArgument args[1]; +}; + +struct ExtResponseHeader { + ExtendedCommand command; + uint16_t length; +}; +#pragma pack(pop) + +namespace LiveDataUtil +{ + +LiveDataHandle getNewHandle(); +double liveDataValueToDouble(const LiveDataValue& val); +static constexpr uint32_t LiveDataVersion = 1; + +} // namespace LiveDataUtil +} // namespace icsneo + +#endif // _cplusplus +#endif // __LIVEDATA_H__ diff --git a/include/icsneo/communication/message/livedatamessage.h b/include/icsneo/communication/message/livedatamessage.h new file mode 100644 index 0000000..853e761 --- /dev/null +++ b/include/icsneo/communication/message/livedatamessage.h @@ -0,0 +1,44 @@ +#ifndef __EXTENDEDLIVEDATAMESSAGE_H_ +#define __EXTENDEDLIVEDATAMESSAGE_H_ +#ifdef __cplusplus + +#include +#include "icsneo/communication/message/message.h" +#include "icsneo/communication/command.h" +#include "icsneo/communication/livedata.h" + +namespace icsneo { +class LiveDataMessage : public RawMessage { +public: + LiveDataMessage() : RawMessage(Message::Type::LiveData, Network::NetID::ExtendedCommand) {} + LiveDataHandle handle; + LiveDataCommand cmd; +}; + +class LiveDataCommandMessage : public LiveDataMessage { +public: + LiveDataCommandMessage() {} + std::chrono::milliseconds updatePeriod; + std::chrono::milliseconds expirationTime; + std::vector> args; + void appendSignalArg(LiveDataValueType valueType); +}; + +class LiveDataValueMessage : public LiveDataMessage { +public: + LiveDataValueMessage() {} + uint32_t numArgs; + std::vector> values; +}; + +class LiveDataStatusMessage : public LiveDataMessage { +public: + LiveDataStatusMessage() {} + LiveDataCommand requestedCommand; + LiveDataStatus status; +}; + +} // namespace icsneo + +#endif // __cplusplus +#endif // __EXTENDEDLIVEDATAMESSAGE_H_ \ No newline at end of file diff --git a/include/icsneo/communication/message/message.h b/include/icsneo/communication/message/message.h index a91d96b..bba8cde 100644 --- a/include/icsneo/communication/message/message.h +++ b/include/icsneo/communication/message/message.h @@ -37,6 +37,7 @@ public: ComponentVersions = 0x800c, SupportedFeatures = 0x800d, GenericBinaryStatus = 0x800e, + LiveData = 0x800f, }; Message(Type t) : type(t) {} diff --git a/include/icsneo/communication/packet/livedatapacket.h b/include/icsneo/communication/packet/livedatapacket.h new file mode 100644 index 0000000..938f7f2 --- /dev/null +++ b/include/icsneo/communication/packet/livedatapacket.h @@ -0,0 +1,27 @@ +#ifndef __LIVEDATAPACKET_H__ +#define __LIVEDATAPACKET_H__ + +#ifdef __cplusplus + +#include +#include +#include +#include "icsneo/api/eventmanager.h" +#include "icsneo/communication/message/message.h" +#include "icsneo/communication/message/livedatamessage.h" +#include "icsneo/communication/livedata.h" + +namespace icsneo { + +class LiveDataMessage; + +struct HardwareLiveDataPacket { + static std::shared_ptr DecodeToMessage(const std::vector& bytes, const device_eventhandler_t& report); + static bool EncodeFromMessage(LiveDataMessage& message, std::vector& bytes, const device_eventhandler_t& report); +}; + +} + +#endif // __cplusplus + +#endif // __LIVEDATAPACKET_H__ diff --git a/include/icsneo/device/device.h b/include/icsneo/device/device.h index c739e8f..bc2ba98 100644 --- a/include/icsneo/device/device.h +++ b/include/icsneo/device/device.h @@ -35,7 +35,9 @@ #include "icsneo/communication/message/supportedfeaturesmessage.h" #include "icsneo/communication/message/genericbinarystatusmessage.h" #include "icsneo/communication/message/extendeddatamessage.h" +#include "icsneo/communication/message/livedatamessage.h" #include "icsneo/communication/packet/genericbinarystatuspacket.h" +#include "icsneo/communication/packet/livedatapacket.h" #include "icsneo/device/extensions/flexray/controller.h" #include "icsneo/communication/message/flexray/control/flexraycontrolmessage.h" #include "icsneo/communication/message/ethphymessage.h" @@ -561,6 +563,9 @@ public: */ virtual bool supportsWiVI() const { return false; } + // Returns true if this device supports Live Data subscription + virtual bool supportsLiveData() const { return false; } + std::optional sendEthPhyMsg(const EthPhyMessage& message, std::chrono::milliseconds timeout = std::chrono::milliseconds(50)); std::optional SetCollectionUploaded(uint32_t collectionEntryByteAddress); @@ -570,6 +575,9 @@ public: std::optional getGenericBinarySize(uint16_t binaryIndex); bool readBinaryFile(std::ostream& stream, uint16_t binaryIndex); + bool subscribeLiveData(std::shared_ptr message); + bool unsubscribeLiveData(const LiveDataHandle& handle); + bool clearAllLiveData(); protected: bool online = false; diff --git a/include/icsneo/device/tree/neovifire3/neovifire3.h b/include/icsneo/device/tree/neovifire3/neovifire3.h index 211f90a..1ce8e40 100644 --- a/include/icsneo/device/tree/neovifire3/neovifire3.h +++ b/include/icsneo/device/tree/neovifire3/neovifire3.h @@ -75,6 +75,8 @@ protected: bool supportsWiVI() const override { return true; } + bool supportsLiveData() const override { return true; } + std::optional getCoreminiStartAddressFlash() const override { return 33*1024*1024; } diff --git a/include/icsneo/device/tree/neovired2/neovired2.h b/include/icsneo/device/tree/neovired2/neovired2.h index ee7d0e0..9bf7d65 100644 --- a/include/icsneo/device/tree/neovired2/neovired2.h +++ b/include/icsneo/device/tree/neovired2/neovired2.h @@ -60,6 +60,8 @@ protected: bool supportsWiVI() const override { return true; } + bool supportsLiveData() const override { return true; } + std::optional getCoreminiStartAddressFlash() const override { return 33*1024*1024; } diff --git a/test/livedataencoderdecodertest.cpp b/test/livedataencoderdecodertest.cpp new file mode 100644 index 0000000..256aabd --- /dev/null +++ b/test/livedataencoderdecodertest.cpp @@ -0,0 +1,188 @@ +#include "icsneo/icsneocpp.h" +#include "icsneo/communication/encoder.h" +#include "icsneo/communication/packet/livedatapacket.h" +#include "icsneo/communication/message/livedatamessage.h" +#include "icsneo/communication/packetizer.h" +#include "icsneo/api/eventmanager.h" +#include "gtest/gtest.h" +#include +#include + +using namespace icsneo; + +class LiveDataEncoderDecoderTest : public ::testing::Test { +protected: + void SetUp() override { + report = [](APIEvent::Type, APIEvent::Severity) { + // Unless caught by the test, the packetizer should not throw errors + EXPECT_TRUE(false); + }; + packetizer.emplace([this](APIEvent::Type t, APIEvent::Severity s) { + report(t, s); + }); + packetEncoder.emplace([this](APIEvent::Type t, APIEvent::Severity s) { + report(t, s); + }); + packetDecoder.emplace([this](APIEvent::Type t, APIEvent::Severity s) { + report(t, s); + }); + + msg = std::make_shared(); + arg = std::make_shared(); + msg->handle = 1; + msg->updatePeriod = std::chrono::milliseconds(100); + msg->expirationTime = std::chrono::milliseconds(0); + arg->objectType = LiveDataObjectType::MISC; + arg->objectIndex = 0; + arg->signalIndex = 0; + arg->valueType = LiveDataValueType::GPS_LATITUDE; + msg->args.push_back(arg); + } + device_eventhandler_t report; + std::optional packetEncoder; + std::optional packetizer; + std::optional packetDecoder; + + const std::vector testBytesSub = + { + 0xaa, //start AA + 0x0B, //netid main51 + 0x2F, 0x00, //size little end 16 + 0xF0, //extended header command + 0x35, 0x00, //Live data subcommand little 16 + 0x26, 0x00, //extended subcommand size, little 16 + 0x01, 0x00, 0x00, 0x00, //live data version + 0x01, 0x00, 0x00, 0x00, //live data command (subscribe) + 0x01, 0x00, 0x00, 0x00, //live data handle + 0x01, 0x00, 0x00, 0x00, //numArgs + 0x64, 0x00, 0x00, 0x00, //freqMs (100ms) + 0x00, 0x00, 0x00, 0x00, //expireMs (zero, never expire) + 0x08, 0x00, //lObjectType eCoreMiniObjectTypeMisc + 0x00, 0x00, 0x00, 0x00, //lObjectIndex + 0x00, 0x00, 0x00, 0x00, //lSignalIndex + 0x02, 0x00, 0x00, 0x00, //enumCoreMiniMiscGPSLatitude + 0x41 //padding byte + }; + + const std::vector testBytesUnsub = + { + 0xaa, //start AA + 0x0B, //netid main51 + 0x15, 0x00, //size little end 16 + 0xF0, //extended header command + 0x35, 0x00, //Live data subcommand little 16 + 0x0C, 0x00, //extended subcommand size, little 16 + 0x01, 0x00, 0x00, 0x00, //LiveDataUtil::LiveDataVersion + 0x02, 0x00, 0x00, 0x00, //LiveDataCommand::UNSUBSCRIBE + 0x01, 0x00, 0x00, 0x00, //handle + 0x41 //padding byte + }; + + const std::vector testBytesClear = + { + 0xaa, //start AA + 0x0B, //netid main51 + 0x15, 0x00, //size little end 16 + 0xF0, //extended header command + 0x35, 0x00, //Live data subcommand little 16 + 0x0C, 0x00, //extended subcommand size, little 16 + 0x01, 0x00, 0x00, 0x00, //LiveDataUtil::LiveDataVersion + 0x04, 0x00, 0x00, 0x00, //LiveDataCommand::CLEAR_ALL + 0x00, 0x00, 0x00, 0x00, //handle + 0x41 //padding byte + }; + + const std::vector testBytesResponse = + { + 0xaa, //start AA + 0x0C, //netid RED + 0x2C, 0x00, //size little end 16 + 0xF0, 0x00, //extended header command + 0x35, 0x00, //Live data subcommand little 16 + 0x1C, 0x00, //extended subcommand size, little 16 + 0x01, 0x00, 0x00, 0x00, //version + 0x03, 0x00, 0x00, 0x00, //cmd + 0x01, 0x00, 0x00, 0x00, //handle + 0x01, 0x00, 0x00, 0x00, //numArgs + 0x08, 0x00, //value 1 header (length) + 0x00, 0x00, //value 1 reserved + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, //value large + 0x08, 0x00, 0x00, 0x00, 0x00, 0x00, + }; + + const std::vector testBytesStatus = + { + 0xaa, //start AA + 0x0C, //netid RED + 0x24, 0x00, //size little end 16 + 0xF0, 0x00, //extended header command + 0x35, 0x00, //Live data subcommand little 16 + 0x14, 0x00, //extended subcommand size, little 16 + 0x01, 0x00, 0x00, 0x00, //version + 0x00, 0x00, 0x00, 0x00, //cmd (status) + 0x01, 0x00, 0x00, 0x00, //handle + 0x01, 0x00, 0x00, 0x00, //requested command (subscribe) + 0x00, 0x00, 0x00, 0x00, //error + 0x00, 0x00, 0x00, 0x00, //padding + 0x00, 0x00, + }; + + std::shared_ptr msg; + std::shared_ptr arg; +}; + +TEST_F(LiveDataEncoderDecoderTest, EncodeSubscribeCommandTest) { + std::vector bytestream; + msg->cmd = icsneo::LiveDataCommand::SUBSCRIBE; + packetEncoder->encode(*packetizer, bytestream, msg); + EXPECT_EQ(bytestream, testBytesSub); +} + +TEST_F(LiveDataEncoderDecoderTest, EncodeUnsubscribeCommandTest) { + std::vector bytestream; + auto unsubMsg = std::make_shared(); + unsubMsg->cmd = icsneo::LiveDataCommand::UNSUBSCRIBE; + unsubMsg->handle = msg->handle; + packetEncoder->encode(*packetizer, bytestream, unsubMsg); + EXPECT_EQ(bytestream, testBytesUnsub); +} + +TEST_F(LiveDataEncoderDecoderTest, EncodeClearCommandTest) { + std::vector bytestream; + auto unsubMsg = std::make_shared(); + unsubMsg->cmd = icsneo::LiveDataCommand::CLEAR_ALL; + packetEncoder->encode(*packetizer, bytestream, unsubMsg); + EXPECT_EQ(bytestream, testBytesClear); +} + +TEST_F(LiveDataEncoderDecoderTest, DecoderStatusTest) { + std::shared_ptr result; + if (packetizer->input(testBytesStatus)) { + for (const auto& packet : packetizer->output()) { + if (!packetDecoder->decode(result, packet)) + continue; + } + } + EXPECT_TRUE(result != nullptr); + auto response = std::dynamic_pointer_cast(result); + EXPECT_EQ(response->handle, static_cast(1u)); + EXPECT_EQ(response->cmd, LiveDataCommand::STATUS); + EXPECT_EQ(response->requestedCommand, LiveDataCommand::SUBSCRIBE); + EXPECT_EQ(response->status, LiveDataStatus::SUCCESS); +} + +TEST_F(LiveDataEncoderDecoderTest, DecoderResponseTest) { + std::shared_ptr result; + if (packetizer->input(testBytesResponse)) { + for (const auto& packet : packetizer->output()) { + if (!packetDecoder->decode(result, packet)) + continue; + } + } + EXPECT_TRUE(result != nullptr); + auto response = std::dynamic_pointer_cast(result); + EXPECT_EQ(response->handle, static_cast(1u)); + EXPECT_EQ(response->cmd, LiveDataCommand::RESPONSE); + EXPECT_EQ(response->numArgs, static_cast(1u)); + EXPECT_EQ(icsneo::LiveDataUtil::liveDataValueToDouble(*response->values[0]), 0.0); +}