jhu-cisst · adeguet1 · Jul 9, 2025 · Jan 7, 2024 · Jan 8, 2024 · Jan 9, 2024
diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -1,5 +1,5 @@
 #
-# (C) Copyright 2011-2024 Johns Hopkins University (JHU), All Rights Reserved.
+# (C) Copyright 2011-2025 Johns Hopkins University (JHU), All Rights Reserved.
 #
 # --- begin cisst license - do not edit ---
 #
@@ -9,7 +9,7 @@
 #
 # --- end cisst license ---
 
-cmake_minimum_required (VERSION 3.10)
+cmake_minimum_required (VERSION 3.16)
 
 project (Amp1394 VERSION 2.2.0)
 

diff --git a/lib/CMakeLists.txt b/lib/CMakeLists.txt
@@ -1,5 +1,5 @@
 #
-# (C) Copyright 2011-2021 Johns Hopkins University (JHU), All Rights Reserved.
+# (C) Copyright 2011-2025 Johns Hopkins University (JHU), All Rights Reserved.
 #
 # --- begin cisst license - do not edit ---
 #
@@ -65,29 +65,31 @@ if (Amp1394_BUILD_SWIG)
   find_package(SWIG REQUIRED)
   include(${SWIG_USE_FILE})
 
-  find_package(PythonLibs)
-  include_directories(${PYTHON_INCLUDE_PATH})
+  # Find Python and NumPy
+  set (AMP1394_PYTHON_VERSION_REQUIRED "" CACHE STRING "Required Python version (if not empty)")
+  if (AMP1394_PYTHON_VERSION_REQUIRED)
+    message (STATUS "Looking for Python ${AMP1394_PYTHON_VERSION_REQUIRED}")
+    find_package (Python ${AMP1394_PYTHON_VERSION_REQUIRED} EXACT REQUIRED COMPONENTS Development NumPy)
+  else ()
+    find_package (Python REQUIRED COMPONENTS Interpreter Development NumPy)
+  endif ()
 
-  find_package(PythonInterp)
-  if (PYTHON_EXECUTABLE)
-    execute_process(COMMAND ${PYTHON_EXECUTABLE} -c "import numpy; print(numpy.get_include())"
-                    ERROR_QUIET
-                    OUTPUT_VARIABLE Amp1394_NUMPY_INCLUDE_DIR
-                    OUTPUT_STRIP_TRAILING_WHITESPACE)
-    include_directories(${Amp1394_NUMPY_INCLUDE_DIR} "${Amp1394_BINARY_DIR}/Amp1394")
-  endif()
+  include_directories(${Python_INCLUDE_DIRS} ${Python_NumPy_INCLUDE_DIRS})
+  include_directories("${Amp1394_BINARY_DIR}/Amp1394")
 
   set_source_files_properties(Amp1394.i PROPERTIES CPLUSPLUS ON)
-  swig_add_module(Amp1394Python python Amp1394.i ${HEADERS} ${SOURCE_FILES})
-  swig_link_libraries(Amp1394Python ${PYTHON_LIBRARIES} ${Amp1394_EXTRA_LIBRARIES})
+  swig_add_library(Amp1394Python
+                   LANGUAGE python
+                   SOURCES Amp1394.i ${HEADERS} ${SOURCE_FILES})
+  target_link_libraries(Amp1394Python ${Python_LIBRARIES} ${Amp1394_EXTRA_LIBRARIES})
 
   if (WIN32)
-    set_target_properties (_Amp1394Python PROPERTIES SUFFIX .pyd)
-    set_target_properties (_Amp1394Python PROPERTIES DEBUG_POSTFIX "_d")
+    set_target_properties (Amp1394Python PROPERTIES SUFFIX .pyd)
+    set_target_properties (Amp1394Python PROPERTIES DEBUG_POSTFIX "_d")
   endif (WIN32)
 
   # post build command
-  add_custom_command(TARGET _Amp1394Python POST_BUILD
+  add_custom_command(TARGET Amp1394Python POST_BUILD
                      COMMAND ${CMAKE_COMMAND}
                      ARGS -E copy_if_different
                              ${CMAKE_CURRENT_BINARY_DIR}/Amp1394Python.py
@@ -104,7 +106,7 @@ if (Amp1394_BUILD_SWIG)
 
   # install library and python file
   install (
-    TARGETS _Amp1394Python
+    TARGETS Amp1394Python
     RUNTIME DESTINATION bin
     LIBRARY DESTINATION lib
     COMPONENT Amp1394)

diff --git a/lib/FpgaIO.h b/lib/FpgaIO.h
@@ -361,6 +361,9 @@ class FpgaIO : public BoardIO
     //    data    Data to write to register
     bool WriteFirewirePhy(unsigned char addr, unsigned char data);
 
+    // Print FireWire debug data
+    static void PrintFirewireDebug(std::ostream &debugStream, const quadlet_t *data);
+
 protected:
 
     // Accumulated firmware time

diff --git a/lib/ZynqEmioPort.h b/lib/ZynqEmioPort.h
@@ -74,6 +74,14 @@ class ZynqEmioPort : public BasePort {
     ZynqEmioPort(int portNum = 0, std::ostream &debugStream = std::cerr);
     ~ZynqEmioPort();
 
+    // Get/set EMIO timeout in microseconds
+    double GetTimeout_us(void) const { return emio->GetTimeout_us(); }
+    void SetTimeout_us(double time_uSec) { emio->SetTimeout_us(time_uSec); }
+
+    // Get/set EMIO verbose flag
+    bool GetVerbose() const { return emio->GetVerbose(); }
+    void SetVerbose(bool newState) { emio->SetVerbose(newState); }
+
     //****************** BasePort pure virtual methods ***********************
 
     PortType GetPortType(void) const { return PORT_ZYNQ_EMIO; }

diff --git a/lib/code/Amp1394Console.cpp b/lib/code/Amp1394Console.cpp
@@ -4,7 +4,7 @@
 /*
   Author(s):  Peter Kazanzides
 
-  (C) Copyright 2021 Johns Hopkins University (JHU), All Rights Reserved.
+  (C) Copyright 2021-2025 Johns Hopkins University (JHU), All Rights Reserved.
 
 --- begin cisst license - do not edit ---
 
@@ -29,6 +29,7 @@ no warranty.  The complete license can be found in license.txt and
 #include <curses.h>
 #else
 #include <termios.h>
+#include <sys/select.h>
 #endif
 #endif
 
@@ -87,6 +88,19 @@ bool Amp1394Console::GetString(char *str, int n)
 struct Amp1394Console::ConsoleInternals {
     struct termios savedAttr;
 };
+
+// Implementation of _kbhit for non-Windows systems
+int _kbhit()
+{
+    struct timeval tv;
+    fd_set fds;
+    tv.tv_sec = 0;
+    tv.tv_usec = 0;
+    FD_ZERO(&fds);
+    FD_SET(STDIN_FILENO, &fds);
+    select(STDIN_FILENO+1, &fds, 0, 0, &tv);
+    return FD_ISSET(STDIN_FILENO, &fds);
+}
 #endif
 
 bool Amp1394Console::Init()
@@ -169,14 +183,15 @@ void Amp1394Console::Refresh()
 
 int Amp1394Console::GetChar()
 {
-#ifdef _MSC_VER
     int c = -1;   // ERR in curses
-    // If blocking, or if key pressed, call _getch
-    if (!noBlock || _kbhit())
+    // If blocking, or if key pressed, get a character
+    if (!noBlock || _kbhit()) {
+#ifdef _MSC_VER
         c = noEcho ? _getch() : _getche();
 #else
-    int c = getchar();
+        c = getchar();
 #endif
+    }
     return c;
 }
 

diff --git a/lib/code/AmpIO.cpp b/lib/code/AmpIO.cpp
@@ -266,23 +266,27 @@ std::string AmpIO::GetQLASerialNumber(unsigned char chan)
     // Format: QLA 1234-56 or QLA 1234-567.
     // String is terminated by 0 or 0xff.
     uint16_t address = 0x0000;
-    uint8_t data[20];
+    // QLASNSize must be a multiple of 4
     const size_t QLASNSize = 12;
+    uint8_t data[QLASNSize+1];
     std::string sn;
 
     data[QLASNSize] = 0;  // make sure null-terminated
-    for (size_t i = 0; i < QLASNSize; i++) {
-        if (!PromReadByte25AA128(address, data[i], chan)) {
-            if (chan == 0)
-                std::cerr << "AmpIO::GetQLASerialNumber: failed to get QLA Serial Number" << std::endl;
-            else
-                std::cerr << "AmpIO::GetQLASerialNumber: failed to get QLA " << static_cast<unsigned int>(chan)
-                          << " Serial Number" << std::endl;
-            break;
+    if (!PromReadBlock25AA128(address, reinterpret_cast<quadlet_t *>(data), QLASNSize/sizeof(quadlet_t), chan)) {
+        if (chan == 0)
+            std::cerr << "AmpIO::GetQLASerialNumber: failed to get QLA Serial Number" << std::endl;
+        else
+            std::cerr << "AmpIO::GetQLASerialNumber: failed to get QLA " << static_cast<unsigned int>(chan)
+                      << " Serial Number" << std::endl;
+    }
+    else {
+        // Convert any trailing 0xff characters to null (0)
+        for (int i = QLASNSize-1; i >= 0; i--) {
+            if (data[i] == 0xff)
+                data[i] = 0;
+            else if (data[i] != 0)
+                break;
         }
-        if (data[i] == 0xff)
-            data[i] = 0;
-        address += 1;
     }
     if (strncmp((char *)data, "QLA ", 4) == 0 || strncmp((char *)data, "dRA ", 4) == 0)
         sn.assign((char *)data+4);
@@ -1862,10 +1866,10 @@ void AmpIO::CheckCollectCallback()
 
 //******* Following methods are for dRA1 ********
 
-std::string AmpIO::ExplainSiFault() const
+std::string AmpIO::ExplainSiFault(void) const
 {
     if (GetHardwareVersion() != dRA1_String) {
-        return "Not a Si controller";
+        return "Sorry, no detailed error code available for QLA based controllers.";
     }
     std::stringstream ss;
     const char* amp_fault_text[16] = {"-", "ADC saturated", "Current deviation", "HW overcurrent", "HW overtemp", "Undefined", "Undefined", "Undefined", "Undefined", "Undefined", "Undefined", "Undefined", "Undefined", "Undefined", "Undefined", "Undefined"};
@@ -1876,7 +1880,7 @@ std::string AmpIO::ExplainSiFault() const
     if (!(status & (1 << 0))) ss << "ESPM->dVRK communication failed. Robot not programmed? Cables?" << std::endl;
     if ((status & (1 << 0)) && !(status & (1 << 1))) ss << "ESII/CC->ESPM communication failed. The problem is inside the robot." << std::endl;
     if (!(status & (1 << 3))) ss << "Encoder preload is out of sync. You must preload encoder at least once." << std::endl;
-    if (!GetPowerStatus()) ss << "48V bad. E-stop pressed?" << std::endl;
+    if (!GetPowerStatus()) ss << "48V bad. E-stop disconnected or button pressed?" << std::endl;
     if (GetWatchdogTimeoutStatus()) ss << "Watchdog timeout." << std::endl;
     ss << "(end)" << std::endl;
     ss << std::endl;

diff --git a/lib/code/FpgaIO.cpp b/lib/code/FpgaIO.cpp
@@ -435,6 +435,10 @@ bool FpgaIO::PromReadBlock25AA128(uint16_t addr, quadlet_t *data, unsigned int n
     if (!port->WriteQuadlet(BoardId, address, write_data))
         return false;
 
+    // TODO: rather than PromDelay, can implement WaitProm25AA128, which can check
+    //       the lowest 3 bits of the status register (0 -> IDLE)
+    port->PromDelay();
+
     // get result
     if (!port->ReadBlock(BoardId, (address|0x0100), data, nquads * 4))
         return false;
@@ -760,3 +764,59 @@ bool FpgaIO::WriteFirewirePhy(unsigned char addr, unsigned char data)
     quadlet_t write_data = 0x00001000 | static_cast<quadlet_t>(addr) << 8 | static_cast<quadlet_t>(data);
     return port->WriteQuadlet(BoardId, BoardIO::FW_PHY_REQ, write_data);
 }
+
+
+void FpgaIO::PrintFirewireDebug(std::ostream &debugStream, const quadlet_t *data)
+{
+    // Following structure must match DebugData in Firewire.v
+    struct DebugData {
+        char     header[4];        // Quad 0
+        uint8_t  misc_info;        // Quad 1
+        uint8_t  bus_info;
+        uint8_t  lreq_info;
+        uint8_t  state;
+        uint8_t  numPhyStatus;     // Quad 2
+        uint8_t  numTxGrant;
+        uint8_t  numRecv;
+        uint8_t  numHubSendReq;
+        uint16_t stcount;          // Quad 3
+        uint8_t  numPktRecv;
+        uint8_t  numEthSendReq;
+    };
+    const DebugData *p = reinterpret_cast<const DebugData *>(data);
+    if (strncmp(p->header, "DBG", 3) != 0) {
+        debugStream << "  PrintFirewireDebug: Unexpected header string: " << p->header[0]
+                    << p->header[1] << p->header[2] << " (should be DBG)" << std::endl;
+        return;
+    }
+    debugStream << "  State: " << static_cast<uint16_t>(p->state >> 4)
+                << ", Next: " << static_cast<uint16_t>(p->state & 0xf0) << std::endl;
+    if (p->lreq_info & 0x80)
+        debugStream << "  lreq_type: " << static_cast<uint16_t>((p->lreq_info & 0x70) >> 4) << std::endl;
+    if (p->lreq_info & 0x08)
+        debugStream << "  lreq_busy" << std::endl;
+    debugStream << "  tx_type: " << static_cast<uint16_t>(p->lreq_info & 0x07) << std::endl;
+    debugStream << "  ";
+    if (p->bus_info & 0x80)
+        debugStream << "req_write_bus ";
+    if (p->bus_info & 0x40)
+        debugStream << "grant_write_bus ";
+    debugStream << "node_id: " << static_cast<uint16_t>(p->bus_info & 0x3f) << std::endl;
+    debugStream << "  ";
+    if (p->misc_info & 0x80)
+        debugStream << "data_block ";
+    if (p->misc_info & 0x08)
+        debugStream << "recvCtlInvalid ";
+    if (p->misc_info & 0x04)
+        debugStream << "recvPktNull ";
+    if (p->misc_info & 0x02)
+        debugStream << "link_active ";
+    debugStream << "  rx_speed: " << static_cast<uint16_t>((p->misc_info & 0x70)>>4) << std::endl;
+    debugStream << "  numHubSendReq: " << static_cast<uint16_t>(p->numHubSendReq) << std::endl;
+    debugStream << "  numRecv: " << static_cast<uint16_t>(p->numRecv) << std::endl;
+    debugStream << "  numPktRecv: " << static_cast<uint16_t>(p->numPktRecv) << std::endl;
+    debugStream << "  numTxGrant: " << static_cast<uint16_t>(p->numTxGrant) << std::endl;
+    debugStream << "  numPhyStatus: " << static_cast<uint16_t>(p->numPhyStatus) << std::endl;
+    debugStream << "  numEthSendReq: " << static_cast<uint16_t>(p->numEthSendReq) << std::endl;
+    debugStream << "  stcount: " << p->stcount << std::endl;
+}
diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt
@@ -41,8 +41,8 @@ target_link_libraries(qlacloserelays ${Amp1394_LIBRARIES} ${Amp1394_EXTRA_LIBRAR
 add_executable(qlacommand qlacommand.cpp)
 target_link_libraries(qlacommand ${Amp1394_LIBRARIES} ${Amp1394_EXTRA_LIBRARIES})
 
-add_executable(eth1394Test mainEth1394.cpp MotorVoltage.h MotorVoltage.cpp)
-target_link_libraries(eth1394Test ${Amp1394_LIBRARIES} ${Amp1394_EXTRA_LIBRARIES})
+add_executable(fpgatest fpgatest.cpp MotorVoltage.h MotorVoltage.cpp)
+target_link_libraries(fpgatest ${Amp1394_LIBRARIES} ${Amp1394_EXTRA_LIBRARIES})
 
 add_executable(instrument instrument.cpp)
 target_link_libraries(instrument ${Amp1394_LIBRARIES} ${Amp1394_EXTRA_LIBRARIES})
@@ -67,7 +67,7 @@ install (PROGRAMS ${EXECUTABLE_OUTPUT_PATH}/quad1394eth
          COMPONENT Amp1394-utils
          DESTINATION bin)
 
-install (TARGETS qlacloserelays qlacommand eth1394Test instrument block1394eth enctest ethswitch
+install (TARGETS qlacloserelays qlacommand fpgatest instrument block1394eth enctest ethswitch
          COMPONENT Amp1394-utils
          RUNTIME DESTINATION bin)