From 5bfd7e3300b7ede0ab15a4525230402a7174863b Mon Sep 17 00:00:00 2001 From: Ben Kleinheksel Date: Fri, 13 Jun 2025 13:37:10 -0400 Subject: [PATCH] Add set value to live data Print new commands and signals --- communication/livedata.cpp | 54 ++++++++++++++++++ communication/packet/livedatapacket.cpp | 27 +++++++++ device/device.cpp | 55 +++++++++++++++++++ include/icsneo/communication/livedata.h | 18 ++++++ .../communication/message/livedatamessage.h | 7 +++ include/icsneo/device/device.h | 1 + 6 files changed, 162 insertions(+) diff --git a/communication/livedata.cpp b/communication/livedata.cpp index 883fb20..0cd0951 100644 --- a/communication/livedata.cpp +++ b/communication/livedata.cpp @@ -18,5 +18,59 @@ double liveDataValueToDouble(const LiveDataValue& val) { return val.value * liveDataFixedPointToDouble; } +int liveDataDoubleToValue(LiveDataValue& value, const double& dFloat) { + union { + struct + { + uint32_t ValueFractionPart; + int32_t ValueInt32; + } parts; + int64_t ValueLarge; + } CminiFixedPt; + constexpr double CM_FIXED_POINT_TO_DOUBLEVALUE = (1.0 / (double)(1ULL << 32)); // 2^-32 + constexpr double CM_DOUBLEVALUE_TO_FIXED_POINT = ((double)(1ULL << 32)); // 2^32 + // Use const for limits (C++98 compatible) + const double INT32_MAX_DOUBLE = + static_cast(std::numeric_limits::max()) + (1.0 - std::numeric_limits::epsilon()); + const double INT32_MIN_DOUBLE = static_cast(std::numeric_limits::min()); + const double MIN_FIXED_POINT_DOUBLE = (double)(1ull * CM_FIXED_POINT_TO_DOUBLEVALUE); + + // This needs to be assigned separately, otherwise, for dFloat >= 2^31, + // long double dBigFloat = dFloat * CM_DOUBLEVALUE_TO_FIXED_POINT overflows + // long long (value is >= 2^63) and so the assignment ValueLarge = dBigFloat is undefined + + int32_t intPart; //creating temp variable due to static analysis warning about writing and reading to different union members + if (dFloat < 0.0) + intPart = (int32_t)floor(dFloat); + else + intPart = (int32_t)dFloat; + + //using temp varialbes to avoid static analysis warning about read/write to different union members + double frac = dFloat - (double)(intPart); + uint32_t fracPart = (uint32_t)floor((frac * CM_DOUBLEVALUE_TO_FIXED_POINT) + 0.5); + + //write temp vars back into the union + CminiFixedPt.parts.ValueInt32 = intPart; + CminiFixedPt.parts.ValueFractionPart = fracPart; + value.value = CminiFixedPt.ValueLarge; + + if (dFloat == (double)0.0) + return 0; + + //check if double can be stored as 32.32 + // 0x1 0000 0000 0000 0000 * CM_FIXED_POINT_TO_DOUBLEVALUE = 0x1 0000 0000 + if (dFloat > INT32_MAX_DOUBLE) + return 1; + if (dFloat < INT32_MIN_DOUBLE) + return -1; + + // Use absolute value for minimum fixed point check + double absFloat = (dFloat < 0.0) ? -dFloat : dFloat; + if (absFloat < MIN_FIXED_POINT_DOUBLE) + return -2; + + return 0; +} + } // namespace LiveDataUtil } // namespace icsneo \ No newline at end of file diff --git a/communication/packet/livedatapacket.cpp b/communication/packet/livedatapacket.cpp index eb796f1..c665812 100644 --- a/communication/packet/livedatapacket.cpp +++ b/communication/packet/livedatapacket.cpp @@ -99,6 +99,33 @@ bool HardwareLiveDataPacket::EncodeFromMessage(LiveDataMessage& message, std::ve clearMsg->cmd = static_cast(message.cmd); break; } + case LiveDataCommand::SET_VALUE: { + auto setValMsg = reinterpret_cast(&message); + const auto numArgs = setValMsg->args.size(); + if(numArgs) { + payloadSize = static_cast(sizeof(LiveDataSetValue) + (sizeof(LiveDataSetValueEntry) * (numArgs-1))); + bytestream.resize((payloadSize + sizeof(ExtendedCommandHeader)),0); + LiveDataSetValue* out = reinterpret_cast(bytestream.data() + sizeof(ExtendedCommandHeader)); + out->version = icsneo::LiveDataUtil::LiveDataVersion; + out->cmd = static_cast(setValMsg->cmd); + if(!setValMsg->handle) + setValMsg->handle = LiveDataUtil::getNewHandle(); + out->handle = setValMsg->handle; + out->numArgs = static_cast(setValMsg->args.size()); + for(size_t i = 0; i < numArgs; ++i) { + out->values[i].arg.objectType = setValMsg->args[i]->objectType; + out->values[i].arg.objectIndex = setValMsg->args[i]->objectIndex; + out->values[i].arg.signalIndex = setValMsg->args[i]->signalIndex; + out->values[i].arg.valueType = setValMsg->args[i]->valueType; + out->values[i].value.value = setValMsg->values[i]->value; + out->values[i].value.header.length = sizeof(int64_t); + } + } else { + report(APIEvent::Type::LiveDataInvalidArgument, APIEvent::Severity::Error); + return false; + } + break; + } default: { report(APIEvent::Type::LiveDataInvalidCommand, APIEvent::Severity::Error); return false; diff --git a/device/device.cpp b/device/device.cpp index 77630e0..db33837 100644 --- a/device/device.cpp +++ b/device/device.cpp @@ -2235,6 +2235,61 @@ bool Device::clearAllLiveData() { return true; } +bool Device::setValueLiveData(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() != message->values.size()) || 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 && 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::readVSA(const VSAExtractionSettings& extractionSettings) { if(isOnline()) { goOffline(); diff --git a/include/icsneo/communication/livedata.h b/include/icsneo/communication/livedata.h index 227b860..d879f67 100644 --- a/include/icsneo/communication/livedata.h +++ b/include/icsneo/communication/livedata.h @@ -19,6 +19,7 @@ enum class LiveDataCommand : uint32_t { UNSUBSCRIBE, RESPONSE, CLEAR_ALL, + SET_VALUE, }; enum class LiveDataStatus : uint32_t { @@ -41,10 +42,13 @@ enum class LiveDataValueType : uint32_t { GPS_SPEED, GPS_VALID, GPS_ENABLE = 62, + MANUAL_TRIGGER = 108, + TIME_SINCE_MSG = 111, GPS_ACCURACY = 120, GPS_BEARING = 121, GPS_TIME = 122, GPS_TIME_VALID = 123, + DAQ_OVERRIDE = 124, }; inline std::ostream& operator<<(std::ostream& os, const LiveDataCommand cmd) { @@ -54,6 +58,7 @@ inline std::ostream& operator<<(std::ostream& os, const LiveDataCommand cmd) { case LiveDataCommand::UNSUBSCRIBE: return os << "Unsubscribe"; case LiveDataCommand::RESPONSE: return os << "Response"; case LiveDataCommand::CLEAR_ALL: return os << "Clear All"; + case LiveDataCommand::SET_VALUE: return os << "Set Value"; } return os; } @@ -81,6 +86,8 @@ inline std::ostream& operator<<(std::ostream& os, const LiveDataValueType cmd) { 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"; + case LiveDataValueType::DAQ_OVERRIDE: return os << "DAQ Override"; + case LiveDataValueType::MANUAL_TRIGGER: return os << "Manual Trigger"; } return os; } @@ -127,6 +134,17 @@ struct LiveDataSubscribe : public LiveDataHeader { LiveDataArgument args[1]; }; +struct LiveDataSetValueEntry +{ + LiveDataArgument arg; + LiveDataValue value; +}; + +struct LiveDataSetValue : public LiveDataHeader { + uint32_t numArgs; + LiveDataSetValueEntry values[1]; +}; + struct ExtResponseHeader { ExtendedCommand command; uint16_t length; diff --git a/include/icsneo/communication/message/livedatamessage.h b/include/icsneo/communication/message/livedatamessage.h index 853e761..7dc208d 100644 --- a/include/icsneo/communication/message/livedatamessage.h +++ b/include/icsneo/communication/message/livedatamessage.h @@ -38,6 +38,13 @@ public: LiveDataStatus status; }; +class LiveDataSetValueMessage : public LiveDataMessage { +public: + LiveDataSetValueMessage() {} + std::vector> args; + std::vector> values; +}; + } // namespace icsneo #endif // __cplusplus diff --git a/include/icsneo/device/device.h b/include/icsneo/device/device.h index d7b12fb..a520e13 100644 --- a/include/icsneo/device/device.h +++ b/include/icsneo/device/device.h @@ -636,6 +636,7 @@ public: bool subscribeLiveData(std::shared_ptr message); bool unsubscribeLiveData(const LiveDataHandle& handle); bool clearAllLiveData(); + bool setValueLiveData(std::shared_ptr message); // VSA Read functions