[ASCII protocol] fix corrupted responses

CHANGELOG.md

@@ -1,6 +1,11 @@
 # Unreleased Features
 Please add a note of your changes below this heading if you make a Pull Request.
 
+# Releases
+## [0.5.3] - unreleased
+### Fixed
+* ASCII protocol commands with multiline responses (`i`, `h`) now return the expected response (in v0.5.2 the response was corrupted)
+
 # Releases
 ## [0.5.2] - 2021-05-21
 

Firmware/communication/ascii_protocol.cpp

@@ -42,25 +42,27 @@ static Introspectable root_obj = ODrive4TypeInfo<ODrive>::make_introspectable(od
 // @brief Sends a line on the specified output.
 template<typename ... TArgs>
 void AsciiProtocol::respond(bool include_checksum, const char * fmt, TArgs&& ... args) {
-    size_t len = snprintf(tx_buf_, sizeof(tx_buf_), fmt, std::forward<TArgs>(args)...);
+    char tx_buf[64];
+
+    size_t len = snprintf(tx_buf, sizeof(tx_buf), fmt, std::forward<TArgs>(args)...);
 
     // Silently truncate the output if it's too long for the buffer.
-    len = std::min(len, sizeof(tx_buf_));
+    len = std::min(len, sizeof(tx_buf));
 
     if (include_checksum) {
         uint8_t checksum = 0;
         for (size_t i = 0; i < len; ++i)
-            checksum ^= tx_buf_[i];
-        len += snprintf(tx_buf_ + len, sizeof(tx_buf_) - len, "*%u", checksum);
+            checksum ^= tx_buf[i];
+        len += snprintf(tx_buf + len, sizeof(tx_buf) - len, "*%u", checksum);
     } else {
-        len += snprintf(tx_buf_ + len, sizeof(tx_buf_) - len, "\r\n");
+        len += snprintf(tx_buf + len, sizeof(tx_buf) - len, "\r\n");
     }
 
     // Silently truncate the output if it's too long for the buffer.
-    len = std::min(len, sizeof(tx_buf_));
+    len = std::min(len, sizeof(tx_buf));
 
-    tx_end_ = (const uint8_t*)tx_buf_ + len;
-    tx_channel_->start_write({(const uint8_t*)tx_buf_, tx_end_}, &tx_handle_, MEMBER_CB(this, on_write_finished));
+    sink_.write({(const uint8_t*)tx_buf, len});
+    sink_.maybe_start_async_write();
 }
 
 
@@ -407,24 +409,6 @@ void AsciiProtocol::cmd_unknown(char * pStr, bool use_checksum) {
     respond(use_checksum, "unknown command");
 }
 
-
-
-void AsciiProtocol::on_write_finished(WriteResult result) {
-    tx_handle_ = 0;
-
-    if (result.status == kStreamOk && result.end < tx_end_) {
-        // Not everything was written. Try again.
-        tx_channel_->start_write({result.end, tx_end_}, &tx_handle_, MEMBER_CB(this, on_write_finished));
-        return;
-    }
-
-    if (rx_end_) {
-        uint8_t* rx_end = rx_end_;
-        rx_end_ = nullptr;
-        on_read_finished({kStreamOk, rx_end});
-    }
-}
-
 void AsciiProtocol::on_read_finished(ReadResult result) {
     if (result.status != kStreamOk) {
         return;
@@ -440,13 +424,6 @@ void AsciiProtocol::on_read_finished(ReadResult result) {
         }
 
         if (read_active_) {
-            if (tx_handle_) {
-                // TX is busy - inhibit processing of the incoming data until
-                // on_write_finished() is invoked.
-                rx_end_ = result.end;
-                return;
-            }
-
             process_line({rx_buf_, end_of_line});
         } else {
             // Ignoring this line cause it didn't start at a new-line character

Firmware/communication/ascii_protocol.hpp

@@ -2,13 +2,14 @@
 #define __ASCII_PROTOCOL_HPP
 
 #include <fibre/async_stream.hpp>
+#include <fibre/../../stream_utils.hpp>
 
 #define MAX_LINE_LENGTH ((size_t)256)
 
 class AsciiProtocol {
 public:
     AsciiProtocol(fibre::AsyncStreamSource* rx_channel, fibre::AsyncStreamSink* tx_channel)
-        : rx_channel_(rx_channel), tx_channel_(tx_channel) {}
+        : rx_channel_(rx_channel), sink_(*tx_channel) {}
 
     void start();
 
@@ -34,16 +35,12 @@ private:
     void on_read_finished(fibre::ReadResult result);
 
     fibre::AsyncStreamSource* rx_channel_ = nullptr;
-    fibre::AsyncStreamSink* tx_channel_ = nullptr;
-
-    fibre::TransferHandle tx_handle_ = 0; // non-zero while a TX operation is in progress
     uint8_t* rx_end_ = nullptr; // non-zero if an RX operation has finished but wasn't handled yet because the TX channel was busy
-    const uint8_t* tx_end_ = nullptr;
 
     uint8_t rx_buf_[MAX_LINE_LENGTH];
     bool read_active_ = true;
 
-    char tx_buf_[64];
+    fibre::BufferedStreamSink<512> sink_;
 };
 
 #endif // __ASCII_PROTOCOL_HPP

Firmware/fibre-cpp/include/fibre/cpp_utils.hpp

@@ -82,6 +82,7 @@ public:
 #include <tuple>
 #include <functional>
 #include <unordered_map>
+#include <stdlib.h>
 //#include <ostream>
 
 /* Backport features from C++14 and C++17 ------------------------------------*/

Firmware/fibre-cpp/stream_utils.hpp

@@ -19,11 +19,17 @@ public:
      * (TODO: this is not true yet, see comment in function)
      */
     void write(cbufptr_t buf) {
+        size_t read_idx = read_idx_; // read_idx_ could change during this function
+
+        if ((read_idx + 1) % I == write_idx_) {
+            return;
+        }
+
         // We subtract 1 from the read index because we never want the write
         // pointer to catch up with the read pointer, cause then
         // `write_idx_ == read_idx_` could mean both "full" and "empty".
 
-        size_t read_idx = (read_idx_ + I - 1) % I; // read_idx_ could change during this function
+        read_idx = (read_idx + I - 1) % I; 
 
         if (write_idx_ > read_idx) {
             size_t n_copy = std::min(I - write_idx_, buf.size());