diff --git a/src/ISDevice.h b/src/ISDevice.h index cd43ce8cd..136fa11fe 100644 --- a/src/ISDevice.h +++ b/src/ISDevice.h @@ -44,15 +44,18 @@ class ISDevice { static std::string getFirmwareInfo(const dev_info_t& devInfo, int detail = 1, eHdwRunStates hdwRunState = eHdwRunStates::HDW_STATE_APP); ISDevice() { + //std::cout << "Creating empty ISDevice: " << this << std::endl; flashCfg.checksum = (uint32_t)-1; } ISDevice(port_handle_t _port) { + // std::cout << "Creating ISDevice for port " << portName(_port) << " " << this << std::endl; flashCfg.checksum = (uint32_t)-1; port = _port; } ISDevice(const ISDevice& src) : devLogger(src.devLogger) { + // std::cout << "Creating ISDevice copy from " << ISDevice::getIdAsString(src.devInfo) << " " << this << std::endl; port = src.port; hdwId = src.hdwId; hdwRunState = src.hdwRunState; @@ -67,11 +70,15 @@ class ISDevice { } ~ISDevice() { +// if (((hdwId != IS_HARDWARE_TYPE_UNKNOWN) && (hdwId != IS_HARDWARE_ANY)) || (devInfo.serialNumber != 0)) +// std::cout << "Destroying ISDevice " << getDescription() << ". " << this << std::endl; + //if (port && serialPortIsOpen(port)) // serialPortClose(port); - port = 0; + devInfo = {}; hdwId = IS_HARDWARE_TYPE_UNKNOWN; hdwRunState = HDW_STATE_UNKNOWN; + port = 0; } ISDevice& operator=(const ISDevice& src) { @@ -92,7 +99,9 @@ class ISDevice { /** * @return true is this ISDevice has a valid, and open port */ - bool isConnected() { return (port && serialPortIsOpen(port)); } + bool isConnected() { std::lock_guard lock(portMutex); return (port && serialPortIsOpen(port)); } + + std::string getPortName() { std::lock_guard lock(portMutex); return (port ? portName(port) : ""); } bool Update(); bool step(); @@ -107,8 +116,8 @@ class ISDevice { // Convenience Functions bool BroadcastBinaryData(uint32_t dataId, int periodMultiple); void BroadcastBinaryDataRmcPreset(uint64_t rmcPreset, uint32_t rmcOptions) { std::lock_guard lock(portMutex); comManagerGetDataRmc(port, rmcPreset, rmcOptions); } - void GetData(eDataIDs dataId, uint16_t length=0, uint16_t offset=0, uint16_t period=0) { std::lock_guard lock(portMutex); comManagerGetData(port, dataId, length, offset, period); } - int SendData(eDataIDs dataId, const uint8_t* data, uint32_t length, uint32_t offset = 0) { std::lock_guard lock(portMutex); return comManagerSendData(port, data, dataId, length, offset); } + void GetData(eDataIDs dataId, uint16_t length=0, uint16_t offset=0, uint16_t period=0) { std::lock_guard lock(portMutex); comManagerGetData(port, dataId, length, offset, period); } + int SendData(eDataIDs dataId, const uint8_t* data, uint32_t length, uint32_t offset = 0) { std::lock_guard lock(portMutex); return comManagerSendData(port, data, dataId, length, offset); } int SendRaw(const uint8_t* data, uint32_t length) { std::lock_guard lock(portMutex); return comManagerSendRaw(port, data, length); } int SendNmea(const std::string& nmeaMsg); diff --git a/src/InertialSense.cpp b/src/InertialSense.cpp index b434300a7..a5bd0a66e 100644 --- a/src/InertialSense.cpp +++ b/src/InertialSense.cpp @@ -233,6 +233,9 @@ bool InertialSense::releaseDevice(ISDevice* device, bool closePort) if (deviceIter == m_comManagerState.devices.end()) return false; + auto dl = Logger()->getDeviceLogByPort(device->port); + if (dl) dl->CloseAllFiles(); + if (closePort && device->port) { serialPortClose(device->port); freeSerialPort(device->port, false); @@ -240,7 +243,7 @@ bool InertialSense::releaseDevice(ISDevice* device, bool closePort) m_comManagerState.devices.erase(deviceIter); // erase only remove the ISDevice* from the list, but doesn't release/free the instance itself device->port = NULL; - delete device; + delete device; // causes a double free?? return true; } @@ -575,8 +578,24 @@ bool InertialSense::Update() return anyOpen; } +/** + * TCP Server primary handler - parses data received via the first connected device, looking for RTCM3/UBLOX protocol + * and sends that same data out to the underlying ISTCPServer's connected clients + * @return always returns true, though should probably return false if the m_tcpServer has no active clients (or something) + */ bool InertialSense::UpdateServer() { + // As I understand it, this function is responsible for reading RTCM3, and other useful data sets from connected IMX, + // and publishing it to connected clients (because it is the server). + + // This is a little different, kind-of, because we don't actually let the ISDevice parse any data (but maybe we should). + // Rather, we parse data directly from the COMM buffer, so we can determine what type of data it is (though we should + // already know this). then, based on the packet type (RTCM3/UBLOX, etc) we'll send that data out to the socket. + // + // Ideally, the TCP socket would also be a port_handle_t, and we'd essentially plumb up a passthrough: Let the ISDevice + // parse data FROM the device, call a custom callback for the data types we're interested in, and then when those are + // received, we'd send them right back out the TCP port_handle_t. Perhaps one day; not today. + // as a tcp server, only the first serial port is read from port_handle_t port = m_comManagerState.devices.front()->port; is_comm_instance_t *comm = &(COMM_PORT(port)->comm); @@ -630,6 +649,10 @@ bool InertialSense::UpdateServer() return true; } +/** + * + * @return + */ bool InertialSense::UpdateClient() { if (m_clientStream == NULLPTR) @@ -1469,17 +1492,18 @@ bool InertialSense::OpenSerialPorts(const char* portPattern, int baudRate) break; } + ISDevice device; // we'll reuse this device for querying each port for (auto port : m_serialPorts) { - ISDevice device(port); + device.port = port; switch (checkType) { case 0 : - comManagerSendRaw(port, (uint8_t *) NMEA_CMD_QUERY_DEVICE_INFO, NMEA_CMD_SIZE); + device.SendRaw((uint8_t *) NMEA_CMD_QUERY_DEVICE_INFO, NMEA_CMD_SIZE); break; case 1 : - comManagerGetData(port, DID_DEV_INFO, 0, 0, 0); + device.GetData(DID_DEV_INFO, 0, 0, 0); break; case 2 : - // comManagerGetData(port, DID_DEV_INFO, 0, 0, 0); + // device.GetData(DID_DEV_INFO, 0, 0, 0); break; case 3 : device.queryDeviceInfoISbl(); @@ -1489,12 +1513,12 @@ bool InertialSense::OpenSerialPorts(const char* portPattern, int baudRate) for (int i = 0; i < 3; i++) { SLEEP_MS(5); - comManagerStep(); + device.step(); } if ((device.hdwId != 0) && (device.hdwRunState != 0)) { debug_message("[DBG] Received response from serial port '%s'. Registering device.\n", SERIAL_PORT(port)->portName); - registerDevice(&device); + registerNewDevice(port, device.devInfo); } else if (SERIAL_PORT(port)->errorCode) { // there was some other janky issue with the requested port; even though the device technically exists, its in a bad state. Let's just drop it now. debug_message("[DBG] There was an error accessing serial port '%s': %s\n", SERIAL_PORT(port)->portName, SERIAL_PORT(port)->error); @@ -1883,8 +1907,9 @@ std::vector InertialSense::checkForNewPorts(std::vector= (int)m_comManagerState.devices.size()) return NULLPTR; return &(m_comManagerState.devices[portId(port)]); } + bool registerDevice(ISDevice* device); + ISDevice* registerNewDevice(port_handle_t port, dev_info_t devInfo = {}); + bool freeSerialPort(port_handle_t port, bool releaseDevice = false); bool releaseDevice(ISDevice* device, bool closePort = true); @@ -751,8 +754,6 @@ class InertialSense : public iISTcpServerDelegate bool UpdateClient(); bool EnableLogging(const std::string& path, const cISLogger::sSaveOptions& options = cISLogger::sSaveOptions()); void DisableLogging(); - bool registerDevice(ISDevice* device); - ISDevice* registerNewDevice(port_handle_t port, dev_info_t devInfo); bool HasReceivedDeviceInfo(ISDevice* device); bool HasReceivedDeviceInfoFromAllDevices(); void RemoveDevice(size_t index); diff --git a/tests/gtest_helpers.h b/tests/gtest_helpers.h index a584afa76..ae9d9bd2c 100644 --- a/tests/gtest_helpers.h +++ b/tests/gtest_helpers.h @@ -1,6 +1,8 @@ #include +#ifndef GTEST_HELPERS_H +#define GTEST_HELPERS_H -#if defined(GTestColor) // ((ROS_VERSION_MAJOR == 1) && (ROS_VERSION_MINOR <= 13)) +#if defined(GTestColor) namespace testing { namespace internal @@ -15,19 +17,62 @@ namespace testing extern void ColoredPrintf(GTestColor color, const char* fmt, ...); } } -#define PRINTF(...) do { testing::internal::ColoredPrintf(testing::internal::COLOR_GREEN, "[ ] "); testing::internal::ColoredPrintf(testing::internal::COLOR_YELLOW, __VA_ARGS__); } while (0) + +#define ANSI_BLACK "\u001b[30m" +#define ANSI_DARKGRAY "\u001b[1,30m" +#define ANSI_DARKRED "\u001b[31m" +#define ANSI_RED "\u001b[1,31m" +#define ANSI_DARKGREEN "\u001b[32m" +#define ANSI_GREEN "\u001b[1,32m" +#define ANSI_BROWN "\u001b[33m" +#define ANSI_YELLOW "\u001b[1,33m" +#define ANSI_DARKBLUE "\u001b[34m" +#define ANSI_BLUE "\u001b[1,34m" +#define ANSI_DARKMAGENTA "\u001b[35m" +#define ANSI_MAGENTA "\u001b[1,35m" +#define ANSI_DARKCYAN "\u001b[36m" +#define ANSI_CYAN "\u001b[1,36m" +#define ANSI_GRAY "\u001b[37m" +#define ANSI_WHITE "\u001b[1,37m" +#define ANSI_DEFAULT "\u001b[39m" +#define ANSI_RESET "\u001b[0m" + +#define TEST_PRINTF(...) do { testing::internal::ColoredPrintf(testing::internal::COLOR_GREEN, "[ ] "); testing::internal::ColoredPrintf(testing::internal::COLOR_YELLOW, __VA_ARGS__); } while (0) #else -#define PRINTF(...) +#define ANSI_BLACK "" +#define ANSI_DARKGRAY "" +#define ANSI_DARKRED "" +#define ANSI_RED "" +#define ANSI_DARKGREEN "" +#define ANSI_GREEN "" +#define ANSI_BROWN "" +#define ANSI_YELLOW "" +#define ANSI_DARKBLUE "" +#define ANSI_BLUE "" +#define ANSI_DARKMAGENTA "" +#define ANSI_MAGENTA "" +#define ANSI_DARKCYAN "" +#define ANSI_CYAN "" +#define ANSI_GRAY "" +#define ANSI_WHITE "" +#define ANSI_DEFAULT "" +#define ANSI_RESET "" + +#define TEST_PRINTF(...) do { printf("[ ] "); printf(__VA_ARGS__); fflush(stdout); } while (0); #endif + + // C++ stream interface class TestCout : public std::stringstream { public: ~TestCout() { - PRINTF("%s",str().c_str()); + TEST_PRINTF("%s",str().c_str()); } }; #define TEST_COUT TestCout() + +#endif // GTEST_HELPERS_H \ No newline at end of file