Issue/iridiumsbd

src/DUNE/Hardware/BasicModem.cpp

@@ -45,8 +45,6 @@ namespace DUNE
 {
   namespace Hardware
   {
-    //! Default command timeout.
-    static const double c_timeout = 5.0;
     //! Default input line termination.
     static std::string c_line_term_in = "\r\n";
     //! Default output line termination.
@@ -176,52 +174,31 @@ namespace DUNE
     {
       {
         Concurrency::ScopedMutex l(m_mutex);
+
+        if (m_read_mode == mode)
+          return;
+
         m_read_mode = mode;
       }
 
-      if (mode == READ_MODE_LINE)
+      switch (mode)
       {
-        if (m_bytes.size() > 0)
-        { // if have bytes in queue, transfer to lines
-          getTask()->spew("There are %d bytes in the queue bytes. Convert to queue of lines.",
-                          m_bytes.size());
-          while (m_bytes.size() > 0)
-          {
-            uint8_t byte = 0;
-            m_bytes.pop(byte);
-            m_chars.push(byte);
-          }
-          std::string line = "";
-          while (!m_chars.empty())
-          {
-            if (!processInput(line))
-              continue;
-
-            if (line.empty())
-              continue;
-
-            if (!handleUnsolicited(line))
-            {
-              m_lines.push(line);
-              line = "";
-            }
-          }
-        }
-      }
-      else
+      case READ_MODE_LINE:
       {
-        if (m_lines.size())
-        {
-          getTask()->war("[BasicModem]:There are %d lines in the queue. Convert to queue of bytes.",
-                         m_lines.size());
-          while (m_lines.size() > 0)
-          {
-            std::string line = m_lines.pop();
-            getTask()->war("[BasicModem]:line: %s", line.c_str());
-            for (size_t i = 0; i < line.size(); ++i)
-              m_bytes.push(line[i]);
-          }
-        }
+        if (!converBytesToLines())
+          break;
+
+        std::string line;
+        handleIncomingCharacters(line);
+        break;
+      }
+
+      case READ_MODE_RAW:
+        convertLinesToBytes();
+        break;
+
+      default:
+        break;
       }
     }
 
@@ -295,6 +272,8 @@ namespace DUNE
     bool
     BasicModem::processInput(std::string& str)
     {
+      Concurrency::ScopedMutex l(m_ingestion_mtx);
+
       bool got_line = false;
 
       while (!m_chars.empty())
@@ -368,7 +347,14 @@ namespace DUNE
     std::string
     BasicModem::readLine(Time::Counter<double>& timer)
     {
-      if (m_lines.waitForItems(timer.getRemaining()))
+      const auto remaining = timer.getRemaining();
+      if (remaining <= 0)
+      {
+        getTask()->war("[BasicModem]:timeout while reading line");
+        throw ReadTimeout();
+      }
+
+      if (m_lines.waitForItems(remaining))
       {
         std::string line = m_lines.pop();
         if (line != m_last_cmd)
@@ -386,6 +372,96 @@ namespace DUNE
       m_handle->flushInput();
     }
 
+    void
+    BasicModem::handleIncomingCharacters(std::string& str)
+    {
+      while (!incomingCharsQueueEmpty())
+      {
+        if (!processInput(str))
+          continue;
+
+        if (str.empty())
+          continue;
+
+        if (!handleUnsolicited(str))
+          pushLine(str);
+
+        str = "";
+      }
+    }
+
+    void
+    BasicModem::ingestIncomingDataRaw(const char* data, const size_t len)
+    {
+      Concurrency::ScopedMutex l(m_ingestion_mtx);
+      for (size_t i = 0; i < len; ++i)
+        m_bytes.push(data[i]);
+    }
+
+    void
+    BasicModem::ingestIncomingDataLine(const char* data, const size_t len)
+    {
+      Concurrency::ScopedMutex l(m_ingestion_mtx);
+      for (size_t i = 0; i < len; ++i)
+        m_chars.push(data[i]);
+    }
+
+    void
+    BasicModem::pushLine(const std::string& line)
+    {
+      Concurrency::ScopedMutex l(m_ingestion_mtx);
+      m_lines.push(line);
+    }
+
+    bool
+    BasicModem::incomingCharsQueueEmpty(void)
+    {
+      Concurrency::ScopedMutex l(m_ingestion_mtx);
+      return m_chars.empty();
+    }
+
+    bool
+    BasicModem::converBytesToLines(void)
+    {
+      Concurrency::ScopedMutex l(m_ingestion_mtx);
+
+      if (m_bytes.size() <= 0)
+        return false;
+
+      // if have bytes in queue, transfer to lines
+      getTask()->spew("There are %u bytes in the queue bytes. Convert to queue of lines.",
+                      m_bytes.size());
+      while (m_bytes.size() > 0)
+      {
+        uint8_t byte = 0;
+        m_bytes.pop(byte);
+        m_chars.push(byte);
+      }
+
+      return true;
+    }
+
+    bool
+    BasicModem::convertLinesToBytes(void)
+    {
+      Concurrency::ScopedMutex l(m_ingestion_mtx);
+
+      if (m_lines.size() <= 0)
+        return false;
+
+      getTask()->war("[BasicModem]:There are %u lines in the queue. Convert to queue of bytes.",
+                      m_lines.size());
+      while (m_lines.size() > 0)
+      {
+        std::string line = m_lines.pop();
+        getTask()->war("[BasicModem]:line: %s", line.c_str());
+        for (const auto& c: line)
+          m_bytes.push(c);
+      }
+
+      return true;
+    }
+
     void
     BasicModem::run(void)
     {
@@ -401,7 +477,9 @@ namespace DUNE
         try
         {
           rv = m_handle->read(bfr, sizeof(bfr));
-        } catch (std::runtime_error &e) {
+        }
+        catch (std::runtime_error& e)
+        {
           m_task->war("[BasicModem]:%s: %s", Status::getString(Status::CODE_IO_ERROR), e.what());
           break;
         }
@@ -417,32 +495,27 @@ namespace DUNE
           break;
         }
 
-        if (getReadMode() == READ_MODE_RAW)
+        const auto mode = getReadMode();
+
+        switch (mode)
+        {
+        case READ_MODE_RAW:
         {
-          for (size_t i = 0; i < rv; ++i)
-            m_bytes.push(bfr[i]);
+          ingestIncomingDataRaw(bfr, rv);
+          break;
         }
-        else
+
+        case READ_MODE_LINE:
         {
           bfr[rv] = 0;
           m_task->spew("%s", Streams::sanitize(bfr).c_str());
-
-          for (size_t i = 0; i < rv; ++i)
-          {
-            m_chars.push(bfr[i]);
-          }
-
-          while (!m_chars.empty())
-          {
-            if (!processInput(line))
-              continue;
-
-            if (line.empty())
-              continue;
-
-            if (!handleUnsolicited(line))
-              m_lines.push(line);
-          }
+          ingestIncomingDataLine(bfr, rv);
+          handleIncomingCharacters(line);
+          break;
+        }
+
+        default:
+          break;
         }
       }
     }

src/DUNE/Hardware/BasicModem.hpp

@@ -102,6 +102,9 @@ namespace DUNE
       setBusy(bool value);
 
     protected:
+      //! Default command timeout.
+      static constexpr double c_timeout = 5.0;
+
       //! Read mode.
       enum ReadMode
       {
@@ -113,6 +116,8 @@ namespace DUNE
 
       //! Concurrency lock.
       Concurrency::Mutex m_mutex;
+      //! Lock for data ingestion.
+      Concurrency::Mutex m_ingestion_mtx;
 
       //! Handle unsolicited or asynchronous commands.
       //! @param[in] str command string.
@@ -223,6 +228,29 @@ namespace DUNE
       //! True to trim white-space.
       bool m_line_trim;
 
+
+
+      void
+      handleIncomingCharacters(std::string& str);
+
+      void
+      ingestIncomingDataRaw(const char* data, const size_t len);
+
+      void
+      ingestIncomingDataLine(const char* data, const size_t len);
+
+      void
+      pushLine(const std::string& line);
+
+      bool
+      incomingCharsQueueEmpty(void);
+
+      bool
+      converBytesToLines(void);
+
+      bool
+      convertLinesToBytes(void);
+
       bool
       processInput(std::string& str);
 

src/DUNE/Hardware/HayesModem.cpp

@@ -157,6 +157,36 @@ namespace DUNE
       return str;
     }
 
+    std::string
+    HayesModem::readValue(const std::string& cmd, const std::string& rly, const double tmt)
+    {
+      sendAT(cmd);
+      std::string str = waitForReply(rly, tmt);
+      expectOK();
+      return str;
+    }
+
+    std::string
+    HayesModem::waitForReply(const std::string& rly, const double tmt)
+    {
+      Time::Counter<double> timer(tmt);
+      std::string str;
+
+      do
+      {
+        str = readLine(timer);
+
+        if (Utils::String::startsWith(str, rly))
+          return str;
+      }
+      while(!timer.overflow());
+
+      if (str.empty())
+        throw ReadTimeout();
+      else
+        throw UnexpectedReply(rly, str);
+    }
+
     void
     HayesModem::sendAT(const std::string& str)
     {
@@ -175,11 +205,22 @@ namespace DUNE
     }
 
     void
-    HayesModem::expect(const std::string& str)
+    HayesModem::expect(const std::string& str, const bool persistent)
     {
-      std::string rv = readLine();
-      if (rv != str)
-        throw UnexpectedReply(str, rv);
+      Time::Counter<double> timer(getTimeout());
+
+      do
+      {
+        std::string rv = readLine(timer);
+        if (rv == str)
+          return;
+        else if (!persistent || isErrorReply(rv))
+          throw UnexpectedReply(str, rv);
+      }
+      while (!timer.overflow());
+
+      getTask()->war("[BasicModem]:timeout while reading line");
+      throw ReadTimeout();
     }
 
     void
@@ -193,5 +234,17 @@ namespace DUNE
     {
       expect("READY");
     }
+
+    bool
+    HayesModem::isErrorReply(const std::string& str)
+    {
+      return m_error_replies.find(str) != m_error_replies.end();
+    }
+
+    void
+    HayesModem::addErrorReply(const std::string& str)
+    {
+      m_error_replies.insert(str);
+    }
   }
 }