From c91db6355ca9b37d93fa1bdbcb04164565ed14e7 Mon Sep 17 00:00:00 2001 From: Ben Kleinheksel Date: Wed, 18 Jun 2025 21:51:32 +0000 Subject: [PATCH] Device: Add setValueLiveData() * Adds the ability to set a CoreMini signal value via the live data interface * Adds definition for the Manual Trigger and DAQ Enable signals --- api/icsneocpp/event.cpp | 6 ++ bindings/python/icsneopy/api/event.cpp | 4 +- communication/livedata.cpp | 57 +++++++++++++++++++ communication/message/livedatamessage.cpp | 10 ++++ communication/packet/livedatapacket.cpp | 27 +++++++++ device/device.cpp | 55 ++++++++++++++++++ examples/cpp/livedata/src/LiveDataExample.cpp | 22 +++++++ include/icsneo/api/event.h | 2 + include/icsneo/communication/livedata.h | 19 +++++++ .../communication/message/livedatamessage.h | 8 +++ include/icsneo/device/device.h | 1 + 11 files changed, 210 insertions(+), 1 deletion(-) diff --git a/api/icsneocpp/event.cpp b/api/icsneocpp/event.cpp index e02f280..dfdc409 100644 --- a/api/icsneocpp/event.cpp +++ b/api/icsneocpp/event.cpp @@ -74,6 +74,8 @@ static constexpr const char* TIMEOUT = "The timeout was reached."; static constexpr const char* WIVI_NOT_SUPPORTED = "Wireless neoVI functions are not supported on this device."; static constexpr const char* RESTRICTED_ENTRY_FLAG = "Attempted to set a restricted flag in a Root Directory entry."; static constexpr const char* NOT_SUPPORTED = "The requested feature is not supported."; +static constexpr const char* FIXED_POINT_OVERFLOW = "Value is too large to convert to fixed point."; +static constexpr const char* FIXED_POINT_PRECISION = "Value is too small for fixed point precision."; // Device Errors static constexpr const char* POLLING_MESSAGE_OVERFLOW = "Too many messages have been recieved for the polling message buffer, some have been lost!"; @@ -240,6 +242,10 @@ const char* APIEvent::DescriptionForType(Type type) { return RESTRICTED_ENTRY_FLAG; case Type::NotSupported: return NOT_SUPPORTED; + case Type::FixedPointOverflow: + return FIXED_POINT_OVERFLOW; + case Type::FixedPointPrecision: + return FIXED_POINT_PRECISION; // Device Errors case Type::PollingMessageOverflow: diff --git a/bindings/python/icsneopy/api/event.cpp b/bindings/python/icsneopy/api/event.cpp index 2789651..aa0db32 100644 --- a/bindings/python/icsneopy/api/event.cpp +++ b/bindings/python/icsneopy/api/event.cpp @@ -142,7 +142,9 @@ void init_event(pybind11::module_& m) { .value("VSAOtherError", APIEvent::Type::VSAOtherError) .value("NoErrorFound", APIEvent::Type::NoErrorFound) .value("TooManyEvents", APIEvent::Type::TooManyEvents) - .value("Unknown", APIEvent::Type::Unknown); + .value("Unknown", APIEvent::Type::Unknown) + .value("FixedPointOverflow", APIEvent::Type::FixedPointOverflow) + .value("FixedPointPrecision", APIEvent::Type::FixedPointPrecision); pybind11::enum_(apiEvent, "Severity") .value("Any", APIEvent::Severity::Any) diff --git a/communication/livedata.cpp b/communication/livedata.cpp index 883fb20..a54804d 100644 --- a/communication/livedata.cpp +++ b/communication/livedata.cpp @@ -1,4 +1,5 @@ #include "icsneo/communication/livedata.h" +#include namespace icsneo { namespace LiveDataUtil { @@ -18,5 +19,61 @@ double liveDataValueToDouble(const LiveDataValue& val) { return val.value * liveDataFixedPointToDouble; } +bool liveDataDoubleToValue(const double& dFloat, LiveDataValue& value) { + 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)std::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)std::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 true; + + //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 || dFloat < INT32_MIN_DOUBLE) { + EventManager::GetInstance().add(APIEvent::Type::FixedPointOverflow, APIEvent::Severity::Error); + return false; + } + + // Use absolute value for minimum fixed point check + double absFloat = (dFloat < 0.0) ? -dFloat : dFloat; + if(absFloat < MIN_FIXED_POINT_DOUBLE) { + EventManager::GetInstance().add(APIEvent::Type::FixedPointPrecision, APIEvent::Severity::Error); + return false; + } + + return true; +} + } // namespace LiveDataUtil } // namespace icsneo \ No newline at end of file diff --git a/communication/message/livedatamessage.cpp b/communication/message/livedatamessage.cpp index a1dfbc6..4f0753d 100644 --- a/communication/message/livedatamessage.cpp +++ b/communication/message/livedatamessage.cpp @@ -12,4 +12,14 @@ void LiveDataCommandMessage::appendSignalArg(LiveDataValueType valueType) { arg->valueType = valueType; } +void LiveDataSetValueMessage::appendSetValue(LiveDataValueType valueType, const LiveDataValue& value) { + auto& arg = args.emplace_back(std::make_shared()); + arg->objectType = LiveDataObjectType::MISC; + arg->objectIndex = 0u; + arg->signalIndex = 0u; + arg->valueType = valueType; + + values.push_back(std::make_shared(value)); +} + } // namespace icsneo diff --git a/communication/packet/livedatapacket.cpp b/communication/packet/livedatapacket.cpp index eb796f1..8d7fd15 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->numSetValues = (uint32_t)numArgs; + 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(LiveDataValue::value); + } + } 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/examples/cpp/livedata/src/LiveDataExample.cpp b/examples/cpp/livedata/src/LiveDataExample.cpp index cb46ae7..9819f7d 100644 --- a/examples/cpp/livedata/src/LiveDataExample.cpp +++ b/examples/cpp/livedata/src/LiveDataExample.cpp @@ -35,6 +35,8 @@ int main() { msg->appendSignalArg(icsneo::LiveDataValueType::GPS_LATITUDE); msg->appendSignalArg(icsneo::LiveDataValueType::GPS_LONGITUDE); msg->appendSignalArg(icsneo::LiveDataValueType::GPS_ACCURACY); + msg->appendSignalArg(icsneo::LiveDataValueType::DAQ_ENABLE); + msg->appendSignalArg(icsneo::LiveDataValueType::MANUAL_TRIGGER); msg->cmd = icsneo::LiveDataCommand::SUBSCRIBE; msg->handle = icsneo::LiveDataUtil::getNewHandle(); msg->updatePeriod = std::chrono::milliseconds(100); @@ -77,6 +79,26 @@ int main() { })); // Run handler for three seconds to observe the signal data std::this_thread::sleep_for(std::chrono::seconds(3)); + double val = 0; + for (unsigned int i = 0; i < 10; ++i) { + // Set the values of signals we're watching so we can see them change live + auto setValMsg = std::make_shared(); + setValMsg->cmd = icsneo::LiveDataCommand::SET_VALUE; + setValMsg->handle = msg->handle; + // Convert the value format + icsneo::LiveDataValue ldValueDAQEnable; + icsneo::LiveDataValue ldValueManTrig; + if (!icsneo::LiveDataUtil::liveDataDoubleToValue(val / 3, ldValueDAQEnable) || + !icsneo::LiveDataUtil::liveDataDoubleToValue(val, ldValueManTrig)) { + break; + } + setValMsg->appendSetValue(icsneo::LiveDataValueType::DAQ_ENABLE, ldValueDAQEnable); + setValMsg->appendSetValue(icsneo::LiveDataValueType::MANUAL_TRIGGER, ldValueManTrig); + device->setValueLiveData(setValMsg); + ++val; + // 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); diff --git a/include/icsneo/api/event.h b/include/icsneo/api/event.h index 281a7b9..f217671 100644 --- a/include/icsneo/api/event.h +++ b/include/icsneo/api/event.h @@ -51,6 +51,8 @@ public: WiVINotSupported = 0x1015, RestrictedEntryFlag = 0x1016, NotSupported = 0x1017, + FixedPointOverflow = 0x1018, + FixedPointPrecision = 0x1019, // Device Events PollingMessageOverflow = 0x2000, diff --git a/include/icsneo/communication/livedata.h b/include/icsneo/communication/livedata.h index 227b860..1a2d47a 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_ENABLE = 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_ENABLE: return os << "DAQ Enable"; + 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 numSetValues; + LiveDataSetValueEntry values[1]; +}; + struct ExtResponseHeader { ExtendedCommand command; uint16_t length; @@ -138,6 +156,7 @@ namespace LiveDataUtil LiveDataHandle getNewHandle(); double liveDataValueToDouble(const LiveDataValue& val); +bool liveDataDoubleToValue(const double& dFloat, LiveDataValue& value); static constexpr uint32_t LiveDataVersion = 1; } // namespace LiveDataUtil diff --git a/include/icsneo/communication/message/livedatamessage.h b/include/icsneo/communication/message/livedatamessage.h index 853e761..cb739c3 100644 --- a/include/icsneo/communication/message/livedatamessage.h +++ b/include/icsneo/communication/message/livedatamessage.h @@ -38,6 +38,14 @@ public: LiveDataStatus status; }; +class LiveDataSetValueMessage : public LiveDataMessage { +public: + LiveDataSetValueMessage() {} + std::vector> args; + std::vector> values; + void appendSetValue(LiveDataValueType valueType, const LiveDataValue& value); +}; + } // 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