From 7a86e2524cffcd02e15410af248c67d75c505398 Mon Sep 17 00:00:00 2001 From: Kyle Mallory Date: Wed, 15 Jan 2025 10:59:36 -0700 Subject: [PATCH] Adds support for Port Monitor (General & GPX) to logPlotter.py and logInspectorInternal.py. Added some additional doxygen to InertialSense.cpp --- .../logInspector/logInspectorInternal.py | 2 + .../inertialsense/logInspector/logPlotter.py | 70 +++++++++++++++++++ .../inertialsense/logs/include/log_reader.h | 3 +- python/inertialsense/logs/src/log_reader.cpp | 7 +- src/InertialSense.cpp | 63 +++++++++++------ src/pybindMacros.h | 5 +- 6 files changed, 122 insertions(+), 28 deletions(-) diff --git a/python/inertialsense/logInspector/logInspectorInternal.py b/python/inertialsense/logInspector/logInspectorInternal.py index e678be014..f62919bf0 100644 --- a/python/inertialsense/logInspector/logInspectorInternal.py +++ b/python/inertialsense/logInspector/logInspectorInternal.py @@ -73,6 +73,8 @@ def __init__(self, config): def createListSystem(self): super(logInspectorInternal, self).createListSystem() self.addListItem('General Fault Codes', 'genFaultCodes') + self.addListItem('Port Monitor (General)', 'portMonitor') + self.addListItem('GPX Port Monitor (behind IMX)', 'gpxPortMonitor') def createListIns(self): super(logInspectorInternal, self).createListIns() diff --git a/python/inertialsense/logInspector/logPlotter.py b/python/inertialsense/logInspector/logPlotter.py index 76a14c949..bed721db7 100644 --- a/python/inertialsense/logInspector/logPlotter.py +++ b/python/inertialsense/logInspector/logPlotter.py @@ -1365,6 +1365,76 @@ def genFaultCodes(self, fig=None, axs=None): except: print(RED + "problem plotting insStatus: " + sys.exc_info()[0] + RESET) + def portMonitor(self, fig=None, axs=None): + if fig is None: + fig = plt.figure() + ax = fig.subplots(4,2, sharex=True) + fig.suptitle('Port Monitor - ' + os.path.basename(os.path.normpath(self.log.directory))) + + ax[0, 0].set_ylabel('Bytes/Sec (TX)') + ax[1, 0].set_ylabel('Bytes (TX)') + ax[2, 0].set_ylabel('Overflows (TX)') + ax[3, 0].set_ylabel('Bytes Dropped (TX)') + ax[0, 1].set_ylabel('Bytes/Sec (RX)') + ax[1, 1].set_ylabel('Bytes (RX)') + ax[2, 1].set_ylabel('Overflows (RX)') + ax[3, 1].set_ylabel('ChecksumErrors (RX)') + + for d in self.active_devs: + activePorts = self.getData(d, DID_PORT_MONITOR, 'activePorts')[0] + port_sets = self.getData(d, DID_PORT_MONITOR, 'port') + for i in range(activePorts): + data = port_sets[:,i] + ax[0,0].plot(data['txBytesPerSec'], label=f'{self.log.serials[d]}:{i}') + ax[0,1].plot(data['rxBytesPerSec'], label=f'{self.log.serials[d]}:{i}') + ax[1,0].plot(data['txBytes'], label=f'{self.log.serials[d]}:{i}') + ax[1,1].plot(data['rxBytes'], label=f'{self.log.serials[d]}:{i}') + ax[2,0].plot(np.diff(data['txOverflows']), label=f'{self.log.serials[d]}:{i}') + ax[2,1].plot(np.diff(data['rxOverflows']), label=f'{self.log.serials[d]}:{i}') + ax[3,0].plot(np.diff(data['txBytesDropped']), label=f'{self.log.serials[d]}:{i}') + ax[3,1].plot(np.diff(data['rxChecksumErrors']), label=f'{self.log.serials[d]}:{i}') + + + self.legends_add(ax[0,0].legend(ncol=2)) + for b in ax: + for a in b: + a.grid(True) + + def gpxPortMonitor(self, fig=None, axs=None): + if fig is None: + fig = plt.figure() + ax = fig.subplots(4,2, sharex=True) + fig.suptitle('Port Monitor - ' + os.path.basename(os.path.normpath(self.log.directory))) + + ax[0, 0].set_ylabel('Bytes/Sec (TX)') + ax[1, 0].set_ylabel('Bytes (TX)') + ax[2, 0].set_ylabel('Overflows (TX)') + ax[3, 0].set_ylabel('Bytes Dropped (TX)') + ax[0, 1].set_ylabel('Bytes/Sec (RX)') + ax[1, 1].set_ylabel('Bytes (RX)') + ax[2, 1].set_ylabel('Overflows (RX)') + ax[3, 1].set_ylabel('ChecksumErrors (RX)') + + for d in self.active_devs: + activePorts = self.getData(d, DID_GPX_PORT_MONITOR, 'activePorts')[0] + port_sets = self.getData(d, DID_GPX_PORT_MONITOR, 'port') + for i in range(activePorts): + data = port_sets[:,i] + ax[0,0].plot(data['txBytesPerSec'], label=f'{self.log.serials[d]}:{i}') + ax[0,1].plot(data['rxBytesPerSec'], label=f'{self.log.serials[d]}:{i}') + ax[1,0].plot(data['txBytes'], label=f'{self.log.serials[d]}:{i}') + ax[1,1].plot(data['rxBytes'], label=f'{self.log.serials[d]}:{i}') + ax[2,0].plot(np.diff(data['txOverflows']), label=f'{self.log.serials[d]}:{i}') + ax[2,1].plot(np.diff(data['rxOverflows']), label=f'{self.log.serials[d]}:{i}') + ax[3,0].plot(np.diff(data['txBytesDropped']), label=f'{self.log.serials[d]}:{i}') + ax[3,1].plot(np.diff(data['rxChecksumErrors']), label=f'{self.log.serials[d]}:{i}') + + + self.legends_add(ax[0,0].legend(ncol=2)) + for b in ax: + for a in b: + a.grid(True) + def gpxStatus(self, fig=None, axs=None): try: if fig is None: diff --git a/python/inertialsense/logs/include/log_reader.h b/python/inertialsense/logs/include/log_reader.h index cc1ffc11b..7eae4b014 100644 --- a/python/inertialsense/logs/include/log_reader.h +++ b/python/inertialsense/logs/include/log_reader.h @@ -107,7 +107,8 @@ struct DeviceLog std::vector rtkPhaseResidual; std::vector rtkDebug; // std::vector rtkDebug2; -// std::vector portMonitor; + std::vector portMonitor; + std::vector gpxPortMonitor; std::vector gpxDebugArray; }; diff --git a/python/inertialsense/logs/src/log_reader.cpp b/python/inertialsense/logs/src/log_reader.cpp index f79a0ec99..f1fcbe6e5 100644 --- a/python/inertialsense/logs/src/log_reader.cpp +++ b/python/inertialsense/logs/src/log_reader.cpp @@ -222,14 +222,14 @@ void LogReader::organizeData(shared_ptr devLog) HANDLE_MSG(DID_DIAGNOSTIC_MESSAGE, dev_log_->diagnosticMessage); HANDLE_MSG(DID_SURVEY_IN, dev_log_->surveyIn); // HANDLE_MSG(DID_EVB2, dev_log_->evb2); - // HANDLE_MSG(DID_PORT_MONITOR, dev_log_->portMonitor); + HANDLE_MSG(DID_PORT_MONITOR, dev_log_->portMonitor); // HANDLE_MSG(DID_RTK_STATE, dev_log_->rtkState); HANDLE_MSG(DID_RTK_CODE_RESIDUAL, dev_log_->rtkCodeResidual); HANDLE_MSG(DID_RTK_PHASE_RESIDUAL, dev_log_->rtkPhaseResidual); HANDLE_MSG(DID_RTK_DEBUG, dev_log_->rtkDebug); // HANDLE_MSG(DID_RTK_DEBUG_2, dev_log_->rtkDebug2); HANDLE_MSG(DID_GPX_DEBUG_ARRAY, dev_log_->gpxDebugArray); - // HANDLE_MSG(DID_GPX_PORT_MONITOR, dev_log_->portMonitor); + HANDLE_MSG(DID_GPX_PORT_MONITOR, dev_log_->gpxPortMonitor); default: // printf("Unhandled IS message DID: %d\n", message_type); @@ -315,7 +315,7 @@ void LogReader::forwardData(int device_id) forward_message(DID_DIAGNOSTIC_MESSAGE, dev_log_->diagnosticMessage, device_id); forward_message(DID_SURVEY_IN, dev_log_->surveyIn, device_id); // forward_message(DID_EVB2, dev_log_->evb2, device_id); - // forward_message(DID_PORT_MONITOR, dev_log_->portMonitor, device_id); + forward_message(DID_PORT_MONITOR, dev_log_->portMonitor, device_id); // forward_message(DID_RTK_STATE, dev_log_->rtkState, device_id); forward_message(DID_RTK_CODE_RESIDUAL, dev_log_->rtkCodeResidual, device_id); @@ -323,6 +323,7 @@ void LogReader::forwardData(int device_id) forward_message(DID_RTK_DEBUG, dev_log_->rtkDebug, device_id); // forward_message(DID_RTK_DEBUG_2, dev_log_->rtkDebug2, device_id); forward_message(DID_GPX_DEBUG_ARRAY, dev_log_->gpxDebugArray, device_id); + forward_message(DID_GPX_PORT_MONITOR, dev_log_->gpxPortMonitor, device_id); } bool LogReader::load() diff --git a/src/InertialSense.cpp b/src/InertialSense.cpp index a5bd0a66e..1e771af9f 100644 --- a/src/InertialSense.cpp +++ b/src/InertialSense.cpp @@ -189,6 +189,11 @@ void InertialSense::DisableLogging() } } +/** + * Registers a previously created ISDevice instance with the internal m_comManager instance. + * @param device + * @return + */ bool InertialSense::registerDevice(ISDevice* device) { if (!device) return NULL; @@ -205,6 +210,15 @@ bool InertialSense::registerDevice(ISDevice* device) { return true; } +/** + * Creates a new ISDevice instance by calling the newDeviceHandler function, with the port and dev_info_t that will + * be associated with the device. This attempt to avoid redundant entries by checking if any previously registered + * devices exists for the same HdwID and Serial No; if found, that existing device will be returned. + * If m_newDeviceHandler is null, then a generic ISDevice will be created. + * @param port the port that the new device is connected to + * @param devInfo the dev_info_t that describes the device + * @return a pointer to an ISDevice instance + */ ISDevice* InertialSense::registerNewDevice(port_handle_t port, dev_info_t devInfo) { if (!port) return NULL; @@ -227,6 +241,32 @@ ISDevice* InertialSense::registerNewDevice(port_handle_t port, dev_info_t devInf return m_comManagerState.devices.empty() ? NULL : (ISDevice*)m_comManagerState.devices.back(); } +/** + * Removes the specified device and associated port from being managed by the InertialSense's comManager instance. + * This does not free/delete/release the device or port, but the underlying call into comManagerRemovePort() will + * close the port. This is a special-use function as there is generally little utility is retaining an ISDevice + * instance which is not attached to the InertialSense class; you should probably be using releaseDevice() instead. + * NOTE: if you use RemoveDevice() it is the callers responsibility to delete/release the ISDevice instance, as + * the InertialSense class will no longer manage it. + */ +void InertialSense::RemoveDevice(ISDevice* device) +{ + for (auto cmsDevice : m_comManagerState.devices) { + if (cmsDevice == device) { + // m_serialPorts.erase(m_serialPorts.begin() + i); + if (device->port) { + comManagerRemovePort(device->port); + } + } + } + std::remove_if (m_comManagerState.devices.begin(), m_comManagerState.devices.end(), [&](const ISDevice* d){ + return d == device; + }); + // TODO: remove the device from m_comManagerState + // -- we don't really need to remove it, but we should + // -- we could end up with the same device listed more than once, with different ports if we don't, though only the most recent should have an active/open port +} + bool InertialSense::releaseDevice(ISDevice* device, bool closePort) { auto deviceIter = std::find(m_comManagerState.devices.begin(), m_comManagerState.devices.end(), device); @@ -269,27 +309,6 @@ bool InertialSense::HasReceivedDeviceInfoFromAllDevices() return true; } -void InertialSense::RemoveDevice(ISDevice* device) -{ - for (auto cmsDevice : m_comManagerState.devices) { - if (cmsDevice == device) { - // m_serialPorts.erase(m_serialPorts.begin() + i); - if (device->port) { - serialPortClose(device->port); - comManagerRemovePort(device->port); - //delete (serial_port_t *) device->port; - //device->port = NULL; - } - } - } - std::remove_if (m_comManagerState.devices.begin(), m_comManagerState.devices.end(), [&](const ISDevice* d){ - return d == device; - }); - // TODO: remove the device from m_comManagerState - // -- we don't really need to remove it, but we should - // -- we could end up with the same device listed more than once, with different ports if we don't, though only the most recent should have an active/open port -} - void InertialSense::LoggerThread(void* info) { bool running = true; @@ -1553,7 +1572,7 @@ bool InertialSense::OpenSerialPorts(const char* portPattern, int baudRate) for (auto deadDevice : deadDevices) { if (deadDevice) { debug_message("[DBG] Deallocating device associated with port '%s'\n", portName(deadDevice->port)); - RemoveDevice(deadDevice); + releaseDevice(deadDevice); } } deadDevices.clear(); diff --git a/src/pybindMacros.h b/src/pybindMacros.h index 3d3c2da72..957cd384b 100644 --- a/src/pybindMacros.h +++ b/src/pybindMacros.h @@ -65,8 +65,9 @@ PYBIND11_NUMPY_DTYPE(inl2_ned_sigma_t, timeOfWeekMs, StdPosNed, StdVelNed, StdAt PYBIND11_NUMPY_DTYPE(strobe_in_time_t, week, timeOfWeekMs, pin, count); PYBIND11_NUMPY_DTYPE(diag_msg_t, timeOfWeekMs, messageLength, message); PYBIND11_NUMPY_DTYPE(survey_in_t, state, maxDurationSec, minAccuracy, elapsedTimeSec, hAccuracy, lla); -// PYBIND11_NUMPY_DTYPE(port_monitor_t, portNumber, txTimeMs, txBytesPerSec, rxTimeMs, rxBytesPerSec, status); -// PYBIND11_NUMPY_DTYPE(port_monitor_t, port); +PYBIND11_NUMPY_DTYPE(port_monitor_set_t, portInfo, status, txBytesPerSec, rxBytesPerSec, txBytes, rxBytes, txOverflows, rxOverflows, txBytesDropped, rxChecksumErrors); +PYBIND11_NUMPY_DTYPE(port_monitor_t, port, activePorts); + // PYBIND11_NUMPY_DTYPE(evb2_t, week, timeOfWeekMs, firmwareVer, comBridgeCfg, loggerMode, loggerElapsedTimeMs, wifiSSID, wifiPSK, wifiIpAddr, serverIpAddr, serverPort); // PYBIND11_NUMPY_DTYPE(evb_status_t, week, timeOfWeekMs, firmwareVer, evbStatus, loggerMode, loggerElapsedTimeMs, wifiIpAddr, sysCommand); // PYBIND11_NUMPY_DTYPE(evb_flash_cfg_t, size, checksum, key, cbPreset, reserved1, cbf, cbOptions, bits, radioPID, radioNID, radioPowerLevel, wifi, server, encoderTickToWheelRad, CANbaud_kbps, can_receive_address, uinsComPort, uinsAuxPort, reserved2, portOptions, h3sp330BaudRate, h4xRadioBaudRate, h8gpioBaudRate);