FirmIO: Fix instability and memory leak issues

bindings/python/CMakeLists.txt

@@ -22,6 +22,7 @@ pybind11_add_module(icsneopy
     icsneopy/device/devicetype.cpp
     icsneopy/communication/network.cpp
     icsneopy/communication/io.cpp
+    icsneopy/communication/livedata.cpp
     icsneopy/communication/message/message.cpp
     icsneopy/communication/message/canmessage.cpp
     icsneopy/communication/message/canerrormessage.cpp
@@ -34,6 +35,7 @@ pybind11_add_module(icsneopy
     icsneopy/communication/message/spimessage.cpp
     icsneopy/communication/message/scriptstatusmessage.cpp
     icsneopy/communication/message/ethphymessage.cpp
+    icsneopy/communication/message/livedatamessage.cpp
     icsneopy/communication/message/callback/messagecallback.cpp
     icsneopy/communication/message/filter/messagefilter.cpp
     icsneopy/core/macseccfg.cpp

bindings/python/icsneopy/communication/livedata.cpp

@@ -0,0 +1,79 @@
+#include <pybind11/pybind11.h>
+#include <pybind11/stl.h>
+#include <pybind11/functional.h>
+#include <pybind11/native_enum.h>
+
+#include "icsneo/communication/livedata.h"
+
+namespace icsneo {
+
+void init_livedata(pybind11::module_& m) {
+	// LiveDataValue struct
+	pybind11::classh<LiveDataValue>(m, "LiveDataValue")
+		.def(pybind11::init<>())
+		.def_readwrite("value", &LiveDataValue::value);
+
+	// LiveDataArgument struct
+	pybind11::classh<LiveDataArgument>(m, "LiveDataArgument")
+		.def(pybind11::init<>())
+		.def_readwrite("object_type", &LiveDataArgument::objectType)
+		.def_readwrite("object_index", &LiveDataArgument::objectIndex)
+		.def_readwrite("signal_index", &LiveDataArgument::signalIndex)
+		.def_readwrite("value_type", &LiveDataArgument::valueType);
+
+	// LiveDataCommand enum
+	pybind11::native_enum<LiveDataCommand>(m, "LiveDataCommand", "enum.IntEnum")
+		.value("STATUS", LiveDataCommand::STATUS)
+		.value("SUBSCRIBE", LiveDataCommand::SUBSCRIBE)
+		.value("UNSUBSCRIBE", LiveDataCommand::UNSUBSCRIBE)
+		.value("RESPONSE", LiveDataCommand::RESPONSE)
+		.value("CLEAR_ALL", LiveDataCommand::CLEAR_ALL)
+		.value("SET_VALUE", LiveDataCommand::SET_VALUE)
+		.finalize();
+
+	// LiveDataStatus enum
+	pybind11::native_enum<LiveDataStatus>(m, "LiveDataStatus", "enum.IntEnum")
+		.value("SUCCESS", LiveDataStatus::SUCCESS)
+		.value("ERR_UNKNOWN_COMMAND", LiveDataStatus::ERR_UNKNOWN_COMMAND)
+		.value("ERR_HANDLE", LiveDataStatus::ERR_HANDLE)
+		.value("ERR_DUPLICATE", LiveDataStatus::ERR_DUPLICATE)
+		.value("ERR_FULL", LiveDataStatus::ERR_FULL)
+		.finalize();
+
+	// LiveDataObjectType enum
+	pybind11::enum_<LiveDataObjectType>(m, "LiveDataObjectType")
+		.value("MISC", LiveDataObjectType::MISC)
+		.value("SNA", LiveDataObjectType::SNA)
+		.export_values();
+
+	// LiveDataValueType enum
+	pybind11::native_enum<LiveDataValueType>(m, "LiveDataValueType", "enum.IntEnum")
+		.value("GPS_LATITUDE", LiveDataValueType::GPS_LATITUDE)
+		.value("GPS_LONGITUDE", LiveDataValueType::GPS_LONGITUDE)
+		.value("GPS_ALTITUDE", LiveDataValueType::GPS_ALTITUDE)
+		.value("GPS_SPEED", LiveDataValueType::GPS_SPEED)
+		.value("GPS_VALID", LiveDataValueType::GPS_VALID)
+		.value("GPS_ENABLE", LiveDataValueType::GPS_ENABLE)
+		.value("MANUAL_TRIGGER", LiveDataValueType::MANUAL_TRIGGER)
+		.value("TIME_SINCE_MSG", LiveDataValueType::TIME_SINCE_MSG)
+		.value("GPS_ACCURACY", LiveDataValueType::GPS_ACCURACY)
+		.value("GPS_BEARING", LiveDataValueType::GPS_BEARING)
+		.value("GPS_TIME", LiveDataValueType::GPS_TIME)
+		.value("GPS_TIME_VALID", LiveDataValueType::GPS_TIME_VALID)
+		.value("DAQ_ENABLE", LiveDataValueType::DAQ_ENABLE)
+		.finalize();
+
+	// LiveDataUtil namespace functions
+	m.def("get_new_handle", &LiveDataUtil::getNewHandle,
+		"Generate a new unique LiveData handle");
+
+	m.def("livedata_value_to_double", &LiveDataUtil::liveDataValueToDouble,
+		pybind11::arg("val"),
+		"Convert LiveDataValue to double (32.32 fixed-point to floating-point)");
+
+	m.def("livedata_double_to_value", &LiveDataUtil::liveDataDoubleToValue,
+		pybind11::arg("d"),
+		"Convert double to LiveDataValue (32.32 fixed-point format). Returns LiveDataValue or None on failure.");
+}
+
+} // namespace icsneo

bindings/python/icsneopy/communication/message/livedatamessage.cpp

@@ -0,0 +1,50 @@
+#include <pybind11/pybind11.h>
+#include <pybind11/stl.h>
+#include <pybind11/functional.h>
+#include <pybind11/chrono.h>
+
+#include "icsneo/communication/message/livedatamessage.h"
+
+namespace icsneo {
+
+void init_livedatamessage(pybind11::module_& m) {
+	// LiveDataMessage base class
+	pybind11::classh<LiveDataMessage, RawMessage>(m, "LiveDataMessage")
+		.def(pybind11::init<>())
+		.def_readwrite("handle", &LiveDataMessage::handle)
+		.def_readwrite("cmd", &LiveDataMessage::cmd);
+
+	// LiveDataCommandMessage (for subscribe/unsubscribe)
+	pybind11::classh<LiveDataCommandMessage, LiveDataMessage>(m, "LiveDataCommandMessage")
+		.def(pybind11::init<>())
+		.def_readwrite("update_period", &LiveDataCommandMessage::updatePeriod)
+		.def_readwrite("expiration_time", &LiveDataCommandMessage::expirationTime)
+		.def_readwrite("args", &LiveDataCommandMessage::args)
+		.def("append_signal_arg", &LiveDataCommandMessage::appendSignalArg,
+			pybind11::arg("value_type"),
+			"Append a signal argument to the command message");
+
+	// LiveDataValueMessage (received values)
+	pybind11::classh<LiveDataValueMessage, LiveDataMessage>(m, "LiveDataValueMessage")
+		.def(pybind11::init<>())
+		.def_readwrite("num_args", &LiveDataValueMessage::numArgs)
+		.def_readwrite("values", &LiveDataValueMessage::values);
+
+	// LiveDataStatusMessage (status responses)
+	pybind11::classh<LiveDataStatusMessage, LiveDataMessage>(m, "LiveDataStatusMessage")
+		.def(pybind11::init<>())
+		.def_readwrite("requested_command", &LiveDataStatusMessage::requestedCommand)
+		.def_readwrite("status", &LiveDataStatusMessage::status);
+
+	// LiveDataSetValueMessage (for setting values)
+	pybind11::classh<LiveDataSetValueMessage, LiveDataMessage>(m, "LiveDataSetValueMessage")
+		.def(pybind11::init<>())
+		.def_readwrite("args", &LiveDataSetValueMessage::args)
+		.def_readwrite("values", &LiveDataSetValueMessage::values)
+		.def("append_set_value", &LiveDataSetValueMessage::appendSetValue,
+			pybind11::arg("value_type"),
+			pybind11::arg("value"),
+			"Append a value to set in the message");
+}
+
+} // namespace icsneo

bindings/python/icsneopy/device/device.cpp

@@ -52,6 +52,11 @@ void init_device(pybind11::module_& m) {
 		.def("start_script", &Device::startScript, pybind11::call_guard<pybind11::gil_scoped_release>())
 		.def("stop_script", &Device::stopScript, pybind11::call_guard<pybind11::gil_scoped_release>())
 		.def("supports_tc10", &Device::supportsTC10)
+		.def("supports_live_data", &Device::supportsLiveData)
+		.def("subscribe_live_data", &Device::subscribeLiveData, pybind11::arg("message"), pybind11::call_guard<pybind11::gil_scoped_release>())
+		.def("unsubscribe_live_data", &Device::unsubscribeLiveData, pybind11::arg("handle"), pybind11::call_guard<pybind11::gil_scoped_release>())
+		.def("clear_all_live_data", &Device::clearAllLiveData, pybind11::call_guard<pybind11::gil_scoped_release>())
+		.def("set_value_live_data", &Device::setValueLiveData, pybind11::arg("message"), pybind11::call_guard<pybind11::gil_scoped_release>())
 		.def("transmit", pybind11::overload_cast<std::shared_ptr<Frame>>(&Device::transmit), pybind11::call_guard<pybind11::gil_scoped_release>())
 		.def("upload_coremini", [](Device& device, std::string& path, Disk::MemoryType memType) { std::ifstream ifs(path, std::ios::binary); return device.uploadCoremini(ifs, memType); }, pybind11::call_guard<pybind11::gil_scoped_release>())
 		.def("write_macsec_config", &Device::writeMACsecConfig, pybind11::call_guard<pybind11::gil_scoped_release>())

bindings/python/icsneopy/device/idevicesettings.cpp

@@ -31,16 +31,49 @@ void init_idevicesettings(pybind11::module_& m) {
         .value("Speed5G", DeviceSettingsNamespace::LinkSpeed::ETH_SPEED_5000)
         .value("Speed10G", DeviceSettingsNamespace::LinkSpeed::ETH_SPEED_10000);
 
+    pybind11::enum_<LINMode>(settings, "LINMode")
+        .value("Sleep", LINMode::SLEEP_MODE)
+        .value("Slow", LINMode::SLOW_MODE)
+        .value("Normal", LINMode::NORMAL_MODE)
+        .value("Fast", LINMode::FAST_MODE);
+
     pybind11::classh<IDeviceSettings>(m, "IDeviceSettings")
         .def("apply", &IDeviceSettings::apply, pybind11::arg("temporary") = 0, pybind11::call_guard<pybind11::gil_scoped_release>())
         .def("apply_defaults", &IDeviceSettings::applyDefaults, pybind11::arg("temporary") = 0, pybind11::call_guard<pybind11::gil_scoped_release>())
+        .def("refresh", &IDeviceSettings::refresh, pybind11::call_guard<pybind11::gil_scoped_release>())
+        
+        // Baudrate methods
+        .def("get_baudrate", &IDeviceSettings::getBaudrateFor, pybind11::call_guard<pybind11::gil_scoped_release>())
+        .def("set_baudrate", &IDeviceSettings::setBaudrateFor, pybind11::call_guard<pybind11::gil_scoped_release>())
+        .def("get_fd_baudrate", &IDeviceSettings::getFDBaudrateFor, pybind11::call_guard<pybind11::gil_scoped_release>())
+        .def("set_fd_baudrate", &IDeviceSettings::setFDBaudrateFor, pybind11::call_guard<pybind11::gil_scoped_release>())
+        
+        // Termination methods
+        .def("is_termination_supported", &IDeviceSettings::isTerminationSupportedFor, pybind11::call_guard<pybind11::gil_scoped_release>())
+        .def("can_termination_be_enabled", &IDeviceSettings::canTerminationBeEnabledFor, pybind11::call_guard<pybind11::gil_scoped_release>())
+        .def("is_termination_enabled", &IDeviceSettings::isTerminationEnabledFor, pybind11::call_guard<pybind11::gil_scoped_release>())
+        .def("set_termination", &IDeviceSettings::setTerminationFor, pybind11::call_guard<pybind11::gil_scoped_release>())
+        .def("get_termination_groups", &IDeviceSettings::getTerminationGroups, pybind11::call_guard<pybind11::gil_scoped_release>())
+        
+        // LIN methods
+        .def("is_commander_resistor_enabled", &IDeviceSettings::isCommanderResistorEnabledFor, pybind11::call_guard<pybind11::gil_scoped_release>())
+        .def("set_commander_resistor", &IDeviceSettings::setCommanderResistorFor, pybind11::call_guard<pybind11::gil_scoped_release>())
+        .def("get_lin_mode", &IDeviceSettings::getLINModeFor, pybind11::call_guard<pybind11::gil_scoped_release>())
+        .def("set_lin_mode", &IDeviceSettings::setLINModeFor, pybind11::call_guard<pybind11::gil_scoped_release>())
+        .def("get_lin_commander_response_time", &IDeviceSettings::getLINCommanderResponseTimeFor, pybind11::call_guard<pybind11::gil_scoped_release>())
+        .def("set_lin_commander_response_time", &IDeviceSettings::setLINCommanderResponseTimeFor, pybind11::call_guard<pybind11::gil_scoped_release>())
+        
+        // Ethernet PHY methods
         .def("get_phy_enable", &IDeviceSettings::getPhyEnable, pybind11::call_guard<pybind11::gil_scoped_release>())
         .def("get_phy_mode", &IDeviceSettings::getPhyMode, pybind11::call_guard<pybind11::gil_scoped_release>())
         .def("get_phy_speed", &IDeviceSettings::getPhySpeed, pybind11::call_guard<pybind11::gil_scoped_release>())
         .def("set_phy_enable", &IDeviceSettings::setPhyEnable, pybind11::call_guard<pybind11::gil_scoped_release>())
         .def("set_phy_mode", &IDeviceSettings::setPhyMode, pybind11::call_guard<pybind11::gil_scoped_release>())
         .def("set_phy_speed", &IDeviceSettings::setPhySpeed, pybind11::call_guard<pybind11::gil_scoped_release>())
-        .def("refresh", &IDeviceSettings::refresh, pybind11::call_guard<pybind11::gil_scoped_release>());
+        
+        // Status properties
+        .def_readonly("disabled", &IDeviceSettings::disabled)
+        .def_readonly("readonly", &IDeviceSettings::readonly);
 }
 
 } // namespace icsneo

bindings/python/icsneopy/icsneocpp.cpp

@@ -35,6 +35,8 @@ void init_version(pybind11::module_&);
 void init_flexray(pybind11::module_& m);
 void init_idevicesettings(pybind11::module_&);
 void init_ethphymessage(pybind11::module_&);
+void init_livedata(pybind11::module_&);
+void init_livedatamessage(pybind11::module_&);
 
 PYBIND11_MODULE(icsneopy, m) {
 	pybind11::options options;
@@ -48,6 +50,7 @@ PYBIND11_MODULE(icsneopy, m) {
 	init_devicetype(m);
 	init_network(m);
 	init_io(m);
+	init_livedata(m);
 	init_message(m);
 	init_canmessage(m);
 	init_canerrormessage(m);
@@ -60,6 +63,7 @@ PYBIND11_MODULE(icsneopy, m) {
 	init_macsecconfig(m);
 	init_scriptstatusmessage(m);
 	init_spimessage(m);
+	init_livedatamessage(m);
 	init_messagefilter(m);
 	init_messagecallback(m);
 	init_diskdriver(m);

communication/livedata.cpp

@@ -19,7 +19,8 @@ double liveDataValueToDouble(const LiveDataValue& val) {
 	return val.value * liveDataFixedPointToDouble; 
 } 
 
-bool liveDataDoubleToValue(const double& dFloat, LiveDataValue& value) {
+std::optional<LiveDataValue> liveDataDoubleToValue(const double& dFloat) {
+	LiveDataValue value;
 	union {
 		struct
 		{
@@ -56,23 +57,23 @@ bool liveDataDoubleToValue(const double& dFloat, LiveDataValue& value) {
 	value.value = CminiFixedPt.ValueLarge;
 
 	if(dFloat == (double)0.0)
-		return true;
+		return value;
 
 	//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;
+		return std::nullopt;
 	}
 
 	// 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 std::nullopt;
 	}
 
-	return true;
+	return value;
 }
 
 } // namespace LiveDataUtil

docs/icsneopy/examples.rst

@@ -27,6 +27,15 @@ Complete CAN Example
    :language: python
 
 
+LiveData Subscription and Monitoring
+=====================================
+
+:download:`Download example <../../examples/python/livedata/livedata_example.py>`
+
+.. literalinclude:: ../../examples/python/livedata/livedata_example.py
+   :language: python
+
+
 Transmit Ethernet frames on Ethernet 01
 ========================================
 

examples/cpp/livedata/src/LiveDataExample.cpp

@@ -29,7 +29,7 @@ int main() {
 		}
 		std::cout << "OK" << std::endl;
 
-		// Create a subscription message for the GPS signals
+		// Create a subscription message for the GPS signals and TIME_SINCE_MSG
 		std::cout << "\tSending a live data subscribe command... ";
 		auto msg = std::make_shared<icsneo::LiveDataCommandMessage>();
 		msg->appendSignalArg(icsneo::LiveDataValueType::GPS_LATITUDE);
@@ -37,6 +37,7 @@ int main() {
 		msg->appendSignalArg(icsneo::LiveDataValueType::GPS_ACCURACY);
 		msg->appendSignalArg(icsneo::LiveDataValueType::DAQ_ENABLE);
 		msg->appendSignalArg(icsneo::LiveDataValueType::MANUAL_TRIGGER);
+		msg->appendSignalArg(icsneo::LiveDataValueType::TIME_SINCE_MSG);
 		msg->cmd = icsneo::LiveDataCommand::SUBSCRIBE;
 		msg->handle = icsneo::LiveDataUtil::getNewHandle();
 		msg->updatePeriod = std::chrono::milliseconds(100);
@@ -44,6 +45,9 @@ int main() {
 		// Transmit the subscription message
 		ret = device->subscribeLiveData(msg);
 		std::cout << (ret ? "OK" : "FAIL") << std::endl;
+		if (!ret) {
+			std::cout << "\t\tError: " << icsneo::GetLastError() << 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;
@@ -53,19 +57,21 @@ int main() {
 			switch(ldMsg->cmd) {
 				case icsneo::LiveDataCommand::STATUS: {
 					auto msg2 = std::dynamic_pointer_cast<icsneo::LiveDataStatusMessage>(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;
+					std::cout << "[STATUS Message]" << std::endl;
+					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<icsneo::LiveDataValueMessage>(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;
+						std::cout << "[Response Message]" << std::endl;
+						std::cout << "  Handle: " << msg->handle << std::endl;
+						std::cout << "  Number of Values: " << valueMsg->numArgs << std::endl;
 						for(uint32_t i = 0; i < valueMsg->numArgs; ++i) {
-							std::cout << "[" << msg->args[i]->valueType << "] ";
+							std::cout << "  [" << msg->args[i]->valueType << "] ";
 							auto scaledValue = icsneo::LiveDataUtil::liveDataValueToDouble(*valueMsg->values[i]);
 							std::cout << scaledValue << std::endl;
 						}
@@ -86,22 +92,33 @@ int main() {
 			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)) {
+			auto ldValueDAQEnable = icsneo::LiveDataUtil::liveDataDoubleToValue(val / 3);
+			auto ldValueManTrig = icsneo::LiveDataUtil::liveDataDoubleToValue(val);
+			auto ldValueTimeSinceMsg = icsneo::LiveDataUtil::liveDataDoubleToValue(val);
+			if (!ldValueDAQEnable || !ldValueManTrig || !ldValueTimeSinceMsg) {
+				std::cout << "\tError: Failed to convert values" << std::endl;
 				break;
 			}
-			setValMsg->appendSetValue(icsneo::LiveDataValueType::DAQ_ENABLE, ldValueDAQEnable);
-			setValMsg->appendSetValue(icsneo::LiveDataValueType::MANUAL_TRIGGER, ldValueManTrig);
-			device->setValueLiveData(setValMsg);
+			setValMsg->appendSetValue(icsneo::LiveDataValueType::DAQ_ENABLE, *ldValueDAQEnable);
+			setValMsg->appendSetValue(icsneo::LiveDataValueType::MANUAL_TRIGGER, *ldValueManTrig);
+			setValMsg->appendSetValue(icsneo::LiveDataValueType::TIME_SINCE_MSG, *ldValueTimeSinceMsg);
+			
+			std::cout << "\tSetting values: DAQ_ENABLE=" << (val / 3) 
+			          << ", MANUAL_TRIGGER=" << val 
+			          << ", TIME_SINCE_MSG=" << val << std::endl;
+			          
+			if (!device->setValueLiveData(setValMsg)) {
+				std::cout << "\tError setting values: " << icsneo::GetLastError() << std::endl;
+			}
 			++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
+		std::cout << "\tUnsubscribing... ";
 		ret = device->unsubscribeLiveData(msg->handle);
+		std::cout << (ret ? "OK" : "FAIL") << std::endl;
 		// The handler should no longer print values
 		std::this_thread::sleep_for(std::chrono::seconds(1));
 		device->removeMessageCallback(handler);

examples/python/livedata/livedata_example.py

@@ -0,0 +1,130 @@
+"""
+LiveData subscription and monitoring example using icsneopy library.
+
+"""
+
+import icsneopy
+import time
+from datetime import timedelta
+
+
+def livedata_example():
+	"""Subscribe to and monitor LiveData signals."""
+	devices = icsneopy.find_all_devices()
+	if not devices:
+		raise RuntimeError("No devices found")
+
+	device = devices[0]
+	print(f"Using device: {device}")
+
+	try:
+		if not device.open():
+			raise RuntimeError("Failed to open device")
+
+		if not device.go_online():
+			raise RuntimeError("Failed to go online")
+
+		device.enable_message_polling()
+
+		# Create subscription message
+		msg = icsneopy.LiveDataCommandMessage()
+		msg.handle = icsneopy.get_new_handle()
+		msg.cmd = icsneopy.LiveDataCommand.SUBSCRIBE
+		msg.update_period = timedelta(milliseconds=500)
+		msg.expiration_time = timedelta(milliseconds=0)
+
+		# Subscribe to various LiveData signals
+		msg.append_signal_arg(icsneopy.LiveDataValueType.GPS_LATITUDE)
+		msg.append_signal_arg(icsneopy.LiveDataValueType.GPS_LONGITUDE)
+		msg.append_signal_arg(icsneopy.LiveDataValueType.GPS_ACCURACY)
+		msg.append_signal_arg(icsneopy.LiveDataValueType.DAQ_ENABLE)
+		msg.append_signal_arg(icsneopy.LiveDataValueType.MANUAL_TRIGGER)
+		msg.append_signal_arg(icsneopy.LiveDataValueType.TIME_SINCE_MSG)
+
+		print("\nSubscribing to LiveData signals...")
+		if not device.subscribe_live_data(msg):
+			raise RuntimeError(f"Subscription failed: {icsneopy.get_last_error()}")
+
+		print("Subscription successful")
+		print("\nMonitoring LiveData for 5 seconds...")
+
+		response_count = 0
+		start_time = time.time()
+
+		while time.time() - start_time < 5:
+			result = device.get_messages()
+			messages = result[0] if isinstance(result, tuple) else result
+
+			for m in messages:
+				if isinstance(m, icsneopy.LiveDataStatusMessage):
+					if m.handle == msg.handle:
+						print(f"\n[Status] Command: {m.requested_command}, Status: {m.status}")
+
+				elif isinstance(m, icsneopy.LiveDataValueMessage):
+					if m.handle == msg.handle:
+						response_count += 1
+						print(f"\n[Response #{response_count}]")
+						signal_names = ["GPS_LAT", "GPS_LON", "GPS_ACC", 
+									   "DAQ_EN", "MAN_TRIG", "TIME_SINCE"]
+						for idx, val in enumerate(m.values):
+							value = icsneopy.livedata_value_to_double(val)
+							name = signal_names[idx] if idx < len(signal_names) else f"Signal_{idx}"
+							print(f"  {name:12s}: {value:10.2f}")
+
+			time.sleep(0.1)
+
+		print(f"\nReceived {response_count} response messages")
+
+		# Demonstrate setting values
+		print("\nSetting custom values...")
+		set_msg = icsneopy.LiveDataSetValueMessage()
+		set_msg.handle = icsneopy.get_new_handle()
+		set_msg.cmd = icsneopy.LiveDataCommand.SET_VALUE
+
+		# Set DAQ_ENABLE
+		value = icsneopy.livedata_double_to_value(1.0)
+		if value:
+			set_msg.append_set_value(icsneopy.LiveDataValueType.DAQ_ENABLE, value)
+
+		# Set MANUAL_TRIGGER
+		value = icsneopy.livedata_double_to_value(1.0)
+		if value:
+			set_msg.append_set_value(icsneopy.LiveDataValueType.MANUAL_TRIGGER, value)
+
+		if device.set_value_live_data(set_msg):
+			print("Values set successfully")
+			time.sleep(0.5)
+
+			# Check the results
+			result = device.get_messages()
+			messages = result[0] if isinstance(result, tuple) else result
+			for m in messages:
+				if isinstance(m, icsneopy.LiveDataStatusMessage):
+					if m.handle == set_msg.handle:
+						print(f" Set status: {m.status}")
+
+		# Keep device awake by resetting TIME_SINCE_MSG
+		print("\nResetting TIME_SINCE_MSG to keep device awake...")
+		reset_msg = icsneopy.LiveDataSetValueMessage()
+		reset_msg.handle = icsneopy.get_new_handle()
+		reset_msg.cmd = icsneopy.LiveDataCommand.SET_VALUE
+		
+		value = icsneopy.livedata_double_to_value(0.0)
+		if value:
+			reset_msg.append_set_value(icsneopy.LiveDataValueType.TIME_SINCE_MSG, value)
+			if device.set_value_live_data(reset_msg):
+				print("TIME_SINCE_MSG reset to 0")
+
+		# Unsubscribe
+		print("\nUnsubscribing...")
+		if device.unsubscribe_live_data(msg.handle):
+			print("Unsubscribed successfully")
+
+	finally:
+		device.close()
+		print("\nDevice closed")
+
+
+if __name__ == "__main__":
+	livedata_example()
+

include/icsneo/communication/livedata.h

@@ -5,6 +5,7 @@
 #include <cstdint>
 #include <vector>
 #include <memory>
+#include <optional>
 #include "icsneo/communication/command.h"
 #include "icsneo/api/eventmanager.h"
 
@@ -157,7 +158,7 @@ namespace LiveDataUtil
 
 LiveDataHandle getNewHandle();
 double liveDataValueToDouble(const LiveDataValue& val);
-bool liveDataDoubleToValue(const double& dFloat, LiveDataValue& value);
+std::optional<LiveDataValue> liveDataDoubleToValue(const double& dFloat);
 static constexpr uint32_t LiveDataVersion = 1;
 
 } // namespace LiveDataUtil

include/icsneo/platform/posix/firmio.h

@@ -19,6 +19,9 @@ class FirmIO : public Driver {
 public:
 	static void Find(std::vector<FoundDevice>& foundDevices);
 
+	FirmIO(const device_eventhandler_t& report) : Driver(report) {
+		writeQueueSize = 256;
+	}
 	using Driver::Driver; // Inherit constructor
 	~FirmIO();
 	bool open() override;
@@ -26,16 +29,16 @@ public:
 	bool close() override;
 	driver_finder_t getFinder() override { return FirmIO::Find; }
 
+	// bool writeQueueFull() override;
+	// bool writeQueueAlmostFull() override;
+	bool writeInternal(const std::vector<uint8_t>& b) override;
+
 private:
 	std::thread readThread, writeThread;
 
 	void readTask();
 	void writeTask();
 
-	bool writeQueueFull() override;
-	bool writeQueueAlmostFull() override;
-	bool writeInternal(const std::vector<uint8_t>& bytes) override;
-
 	struct DataInfo {
 		uint32_t type;
 		uint32_t offset;
@@ -111,7 +114,11 @@ private:
 		bool free(uint8_t* addr);
 		PhysicalAddress translate(uint8_t* addr) const;
 
-	private:
+		uint32_t getUsedBlocks() const { return usedBlocks; }
+		size_t getTotalBlocks() const { return blocks.size(); }
+		bool isFull() const { return usedBlocks == blocks.size(); }
+
+
 		struct BlockInfo {
 			enum class Status : uint32_t {
 				Free = 0,
@@ -121,6 +128,7 @@ private:
 			uint8_t* addr;
 		};
 
+	private:
 		std::vector<BlockInfo> blocks;
 		std::atomic<uint32_t> usedBlocks;
 
@@ -137,6 +145,10 @@ private:
 	std::mutex outMutex;
 	std::optional<MsgQueue> out;
 	std::optional<Mempool> outMemory;
+
+	std::atomic<size_t> num_read = 0;
+	std::atomic<size_t> num_written = 0;
+	std::atomic<size_t> num_freed = 0;
 };
 
 }

platform/posix/firmio.cpp

@@ -42,16 +42,16 @@ void FirmIO::Find(std::vector<FoundDevice>& found) {
 	Packetizer packetizer([](APIEvent::Type, APIEvent::Severity) {});
 	Decoder decoder([](APIEvent::Type, APIEvent::Severity) {});
 	using namespace std::chrono;
-	const auto start = steady_clock::now();
 	// Get an absolute wall clock to compare to
-	const auto overallTimeout = start + milliseconds(500);
-	while(!temp.readAvailable()) {
-		if(steady_clock::now() > overallTimeout) {
-			// failed to read out a serial number reponse in time
-			break;
-		}
+	const auto overallTimeout = steady_clock::now() + milliseconds(200);
+	size_t lastBufferSize = 0;
+	while (steady_clock::now() < overallTimeout) 
+	{
+		temp.waitForRx(lastBufferSize + 1, milliseconds(100));
+		bool havePacket = packetizer.input(temp.getReadBuffer());
+		lastBufferSize = temp.getReadBuffer().size();
 
-		if(!packetizer.input(temp.getReadBuffer()))
+		if(!havePacket)
 			continue; // A full packet has not yet been read out
 
 		for(const auto& packet : packetizer.output()) {
@@ -75,6 +75,7 @@ void FirmIO::Find(std::vector<FoundDevice>& found) {
 			};
 
 			found.push_back(foundDevice);
+			break; // never going to find two!
 		}
 	}
 }
@@ -141,17 +142,27 @@ bool FirmIO::open() {
 		}
 	}
 
-	//std::cout << "Flushed " << std::dec << i << " freeing " << toFree.size() << std::endl;
+	// std::cout << "Flushed " << std::dec << i << " freeing " << toFree.size() << std::endl;
 
-	while(!toFree.empty()) {
-		std::lock_guard<std::mutex> lk(outMutex);
-		out->write(&toFree.back());
+	auto endTime = std::chrono::steady_clock::now() + std::chrono::milliseconds(100);
+	while(std::chrono::steady_clock::now() < endTime && !toFree.empty()) {
+		bool pass = false;
+		{
+			std::scoped_lock lk(outMutex);
+			pass = out->write(&toFree.back());
+		}
+		if (!pass)
+		{
+			std::this_thread::sleep_for(std::chrono::milliseconds(1));
+			continue;
+		}
 		toFree.pop_back();
 	}
 
-	// Create thread
-	// No thread for writing since we don't need the extra buffer
+	// Create threads
 	readThread = std::thread(&FirmIO::readTask, this);
+	//logThread = std::thread(&FirmIO::logTask, this);
+	writeThread = std::thread(&FirmIO::writeTask, this);
 	
 	return true;
 }
@@ -171,6 +182,13 @@ bool FirmIO::close() {
 	if(readThread.joinable())
 		readThread.join();
 
+	if (writeThread.joinable())
+		writeThread.join();
+
+	// if(logThread.joinable())
+	// 	logThread.join();
+
+
 	setIsClosing(false);
 	setIsDisconnected(false);
 
@@ -194,7 +212,8 @@ bool FirmIO::close() {
 void FirmIO::readTask() {
 	EventManager::GetInstance().downgradeErrorsOnCurrentThread();
 	Msg msg;
-	std::vector<Msg> toFree;
+	std::vector<Msg::Ref> toFree;
+	toFree.reserve(outMemory->getTotalBlocks());
 
 	// attempt to elevate the thread priority. PRIO_MIN is actually the highest priority but the lowest value.
 	int err = setpriority(PRIO_PROCESS, 0, -1);
@@ -208,7 +227,6 @@ void FirmIO::readTask() {
 		FD_SET(fd, &rfds);
 		tv.tv_usec = 50000; // 50ms
 		int ret = ::select(fd + 1, &rfds, NULL, NULL, &tv);
-		// std::cout << "select returned " << ret << ' ' << errno << std::endl;
 		if(ret < 0)
 			report(APIEvent::Type::FailedToRead, APIEvent::Severity::Error);
 		if(ret <= 0)
@@ -221,24 +239,12 @@ void FirmIO::readTask() {
 		if(ret < int(sizeof(interruptCount)) || interruptCount < 1)
 			continue;
 
-		toFree.clear();
-		int i = 0;
-		while(in->read(&msg) && i++ < 1000) {
+		while(in->read(&msg)) {
 			switch(msg.command) {
 			case Msg::Command::ComData: {
-				if(toFree.empty() || toFree.back().payload.free.refCount == 6) {
-					toFree.emplace_back();
-					toFree.back().command = Msg::Command::ComFree;
-					toFree.back().payload.free.refCount = 0;
-				}
 
-				// Add this ref to the list of payloads to free
-				// After we process these, we'll send this list back to the device
-				// so that it can free these entries
-				toFree.back().payload.free.ref[toFree.back().payload.free.refCount] = msg.payload.data.ref;
-				toFree.back().payload.free.refCount++;
-
-				// std::cout << "Got some data @ 0x" << std::hex << msg.payload.data.addr << " " << std::dec << msg.payload.data.len << std::endl;
+				toFree.push_back(msg.payload.data.ref);
+				++num_read;
 
 				// Translate the physical address back to our virtual address space
 				uint8_t* addr = reinterpret_cast<uint8_t*>(msg.payload.data.addr - PHY_ADDR_BASE + vbase);
@@ -251,58 +257,95 @@ void FirmIO::readTask() {
 			}
 			break;
 			case Msg::Command::ComFree: {
-				std::lock_guard<std::mutex> lk(outMutex);
-				// std::cout << "Got some free " << std::hex << msg.payload.free.ref[0] << std::endl;
+				std::scoped_lock lk(outMutex);
 				for(uint32_t i = 0; i < msg.payload.free.refCount; i++)
 					outMemory->free(reinterpret_cast<uint8_t*>(msg.payload.free.ref[i]));
 				break;
 			}
+			default:
+				// std::cout << "invalid command: " << std::hex << static_cast<uint32_t>(msg.command) << std::dec << std::endl;
+				break;
 			}
+			if (isClosing() || isDisconnected())
+				break;
 		}
-
-		while(!toFree.empty()) {
-			std::lock_guard<std::mutex> lk(outMutex);
-			out->write(&toFree.back());
-			toFree.pop_back();
+		while (toFree.size()) {
+			Msg freeMsg = { Msg::Command::ComFree };
+			freeMsg.payload.free.refCount = std::min(static_cast<uint32_t>(toFree.size()), 6u);
+			for (size_t i = 0; i < freeMsg.payload.free.refCount; ++i) {
+				freeMsg.payload.free.ref[i] = toFree[i];
+			}
+			std::scoped_lock lk(outMutex);
+			if (!out->write(&freeMsg)) {
+				break;
+			}
+			num_freed += freeMsg.payload.free.refCount;
+			toFree.erase(toFree.begin(), toFree.begin() + freeMsg.payload.free.refCount);
 		}
 	}
+	while (toFree.size())
+	{
+		Msg freeMsg = { Msg::Command::ComFree };
+		freeMsg.payload.free.refCount = std::min(static_cast<uint32_t>(toFree.size()), 6u);
+		for (size_t i = 0; i < freeMsg.payload.free.refCount; ++i) {
+			freeMsg.payload.free.ref[i] = toFree[i];
+		}
+		std::scoped_lock lk(outMutex);
+		if (!out->write(&freeMsg)) {
+			std::this_thread::sleep_for(std::chrono::milliseconds(1));
+			continue;
+		}
+		toFree.erase(toFree.begin(), toFree.begin() + freeMsg.payload.free.refCount);
+	}
+	// std::cout << "FirmIO readTask exiting: " << "closing=" << isClosing() << " disconnected=" << isDisconnected() << std::endl;
 }
 
 void FirmIO::writeTask() {
-	return; // We're overriding Driver::writeInternal() and doing the work there
-}
+	constexpr uint32_t genInterrupt = 0x01;
+	std::pair<std::optional<WriteOperation>, uint8_t*> op;
+	while (!isClosing() && !isDisconnected()) {
+		if (!op.first) {
+			writeQueue.wait_dequeue_timed(op.first, std::chrono::milliseconds(100));
+			continue;
+		}
 
-bool FirmIO::writeQueueFull() {
-	return out->isFull();
-}
+		if (!op.second) {
+			op.second = outMemory->alloc(static_cast<uint32_t>(op.first->bytes.size()));
+			if (op.second == nullptr) {
+				std::this_thread::sleep_for(std::chrono::milliseconds(1));
+				continue;
+			}
+			memcpy(op.second, op.first->bytes.data(), op.first->bytes.size());
+		}
 
-bool FirmIO::writeQueueAlmostFull() {
-	// TODO: Better implementation here
-	return writeQueueFull();
+		Msg msg = { Msg::Command::ComData };
+		msg.payload.data.addr = outMemory->translate(op.second);
+		msg.payload.data.len = op.first->bytes.size();
+		msg.payload.data.ref = reinterpret_cast<Msg::Ref>(op.second);
+
+		
+		std::scoped_lock lk(outMutex);
+		if(!out->write(&msg))
+		{
+			std::this_thread::sleep_for(std::chrono::milliseconds(1));
+			continue;
+		}
+		++num_written;
+		::write(fd, &genInterrupt, sizeof(genInterrupt));
+		op.first.reset();
+		op.second = nullptr;
+	}
+	std::cout << "FirmIO writeTask exiting: " << "closing=" << isClosing() << " disconnected=" << isDisconnected() << std::endl;
 }
 
 bool FirmIO::writeInternal(const std::vector<uint8_t>& bytes) {
 	if(bytes.empty() || bytes.size() > Mempool::BlockSize)
+	{
+		// std::cout << "Invalid write size of " << bytes.size() << std::endl;
 		return false;
+	}
 	
-	std::lock_guard<std::mutex> lk(outMutex);
-	uint8_t* sharedData = outMemory->alloc(bytes.size());
-	if(sharedData == nullptr)
-		return false;
-
-	// std::cout << "coping " << bytes.size() << " bytes of data" << std::endl;
-	memcpy(sharedData, bytes.data(), bytes.size());
-
-	Msg msg = { Msg::Command::ComData };
-	msg.payload.data.addr = outMemory->translate(sharedData);
-	msg.payload.data.len = static_cast<uint32_t>(bytes.size());
-	msg.payload.data.ref = reinterpret_cast<Msg::Ref>(sharedData);
-
-	if(!out->write(&msg))
-		return false;
-
-	uint32_t genInterrupt = 0x01;
-	return ::write(fd, &genInterrupt, sizeof(genInterrupt)) == sizeof(genInterrupt);
+	return writeQueue.enqueue(WriteOperation(bytes));
 }
 
 bool FirmIO::MsgQueue::read(Msg* msg) {
@@ -369,13 +412,17 @@ bool FirmIO::Mempool::free(uint8_t* addr) {
 		return b.addr == addr;
 	});
 
-	if(found == blocks.end())
+	if(found == blocks.end()) {
+		// std::cout << "failed to free block address " << std::hex << reinterpret_cast<uintptr_t>(addr) << std::dec << std::endl;
 		return false; // Invalid address
+	}
 
-	if(found->status != BlockInfo::Status::Used)
+	if(found->status != BlockInfo::Status::Used) {
+		// std::cout << "invalid state for free of block address " << std::hex << reinterpret_cast<uintptr_t>(addr) << std::dec << std::endl;
 		return false; // Double free
+	}
 
-	usedBlocks--;
+	--usedBlocks;
 	found->status = BlockInfo::Status::Free;
 	return true;
 }
@@ -383,3 +430,12 @@ bool FirmIO::Mempool::free(uint8_t* addr) {
 FirmIO::Mempool::PhysicalAddress FirmIO::Mempool::translate(uint8_t* addr) const {
 	return reinterpret_cast<PhysicalAddress>(addr - virtualAddress + physicalAddress);
 }
+
+// void FirmIO::logTask()
+// {
+// 	while (!isClosing() && !isDisconnected()) {
+// 		std::cout << "FirmIO Stats: RX Count: " << num_read << " TX Count: " << num_written << " Used Blocks (out): " << outMemory->getUsedBlocks() << " Freed Blocks: " << num_freed << std::endl;
+// 		std::this_thread::sleep_for(std::chrono::seconds(1));
+// 	}
+// 	std::cout << "FirmIO logTask exiting: " << "closing=" << isClosing() << " disconnected=" << isDisconnected() << std::endl;
+// }