AP_HAL_SITL: use AP_CSVReader to read from Log serial-async dump

libraries/AP_HAL_SITL/UARTDriver.cpp

@@ -87,6 +87,7 @@ void UARTDriver::begin(uint32_t baud, uint16_t rxSpace, uint16_t txSpace)
              uart:/dev/ttyUSB0:57600
              sim:ParticleSensor_SDS021:
              file:/tmp/my-device-capture.BIN
+             logic_async_csv:/tmp/logic_async.csv:
          */
         char *saveptr = nullptr;
         char *s = strdup(path);
@@ -165,6 +166,17 @@ void UARTDriver::begin(uint32_t baud, uint16_t rxSpace, uint16_t txSpace)
                 AP_HAL::panic("Failed to open (%s): %m", args1);
             }
             _connected = true;
+        } else if (strcmp(devtype, "logic_async_csv") == 0) {
+            if (_connected) {
+                AP::FS().close(_fd);
+            }
+            ::printf("logic_async_csv connection %s\n", args1);
+            _fd = AP::FS().open(args1, O_RDONLY);
+            if (_fd == -1) {
+                AP_HAL::panic("Failed to open (%s): %m", args1);
+            }
+            _connected = true;
+            logic_async_csv.active = true;
         } else {
             AP_HAL::panic("Invalid device path: %s", path);
         }
@@ -712,6 +724,98 @@ void UARTDriver::_check_reconnect(void)
     _uart_start_connection();
 }
 
+uint16_t UARTDriver::read_from_async_csv(uint8_t *buffer, uint16_t space)
+{
+    if (_fd == -1) {
+        return 0;
+    }
+    const uint32_t micros = AP_HAL::micros();
+    if (micros < 5000000) {
+        // don't inject for the first several seconds
+        return 0;
+    }
+
+    static uint32_t frame_number;
+    frame_number++;
+
+    uint8_t i;
+    for (i=0; i<space; i++) {
+        static uint32_t count;
+        if (logic_async_csv.loaded) {
+            const uint32_t emit_timestamp_us = micros - logic_async_csv.first_emit_micros_us;
+            const uint32_t data_timestamp_us = logic_async_csv.loaded_data.timestamp_us - logic_async_csv.first_timestamp_us;
+            if (data_timestamp_us > emit_timestamp_us) {
+                return i;
+            }
+            buffer[i] = logic_async_csv.loaded_data.b;
+            count++;
+            logic_async_csv.loaded = false;
+        }
+
+        while (!logic_async_csv.loaded) {
+            uint8_t c;
+            const ssize_t nread = ::read(_fd, &c, 1);
+            if (nread == 0) {
+                // EOF
+                close(_fd);
+                _fd = -1;
+                return i;
+            }
+
+            // feed data into CSV Reader, handle new state:
+            const auto retcode = logic_async_csv.csvreader.feed(c);
+            switch (retcode) {
+            case AP_CSVReader::RetCode::OK:
+                continue;
+            case AP_CSVReader::RetCode::ERROR:
+                AP_HAL::panic("Malformed CSV?");
+            case AP_CSVReader::RetCode::TERM_DONE:
+            case AP_CSVReader::RetCode::VECTOR_DONE:
+                switch (logic_async_csv.terms_seen) {
+                case 0:  // start_time
+                    if (!logic_async_csv.done_first_line) {
+                        break;
+                    }
+                    logic_async_csv.loaded_data.timestamp_us = atof((char*)logic_async_csv.term) * 1000000;  // seconds to microseconds
+                    break;
+                case 1:  // data
+                    if (!logic_async_csv.done_first_line) {
+                        break;
+                    }
+                    logic_async_csv.loaded_data.b = (char_to_hex(logic_async_csv.term[2]) << 4) | char_to_hex(logic_async_csv.term[3]);
+                    break;
+                case 2:  // error
+                case 3:  // framing error
+                    break;
+                case 4:
+                    AP_HAL::panic("Too many terms in CSV, want (name,type,start_time,duration,data");
+                }
+                logic_async_csv.terms_seen++;
+                if (retcode != AP_CSVReader::RetCode::VECTOR_DONE) {
+                    break;
+                }
+
+                // we've handled the last term, now handle the vector:
+                if (logic_async_csv.terms_seen != 4) {
+                    AP_HAL::panic("Incorrect number off terms in CSV, want (Time [s],Value,Parity Error,Framing Error)");
+                }
+                logic_async_csv.terms_seen = 0;
+                if (!logic_async_csv.done_first_line) {
+                    // skip the headers
+                    logic_async_csv.done_first_line = true;
+                    break;
+                }
+                if (logic_async_csv.first_timestamp_us == 0) {
+                    logic_async_csv.first_timestamp_us = logic_async_csv.loaded_data.timestamp_us;
+                    logic_async_csv.first_emit_micros_us = micros;
+                }
+                logic_async_csv.loaded = true;
+            }
+        }
+    }
+    return i;
+}
+
 void UARTDriver::_timer_tick(void)
 {
     if (!_connected) {
@@ -806,6 +910,8 @@ void UARTDriver::_timer_tick(void)
         }
     } else if (_sim_serial_device != nullptr) {
         nread = _sim_serial_device->read_from_device(buf, space);
+    } else if (logic_async_csv.active) {
+        nread = read_from_async_csv((uint8_t*)buf, space);
     } else if (!_use_send_recv) {
         if (!_select_check(_fd)) {
             return;

libraries/AP_HAL_SITL/UARTDriver.h

@@ -8,6 +8,7 @@
 #include "AP_HAL_SITL_Namespace.h"
 #include <AP_HAL/utility/Socket.h>
 #include <AP_HAL/utility/RingBuffer.h>
+#include <AP_CSVReader/AP_CSVReader.h>
 
 #include <SITL/SIM_SerialDevice.h>
 
@@ -130,6 +131,22 @@ private:
     uint32_t last_tick_us;
 
     SITL::SerialDevice *_sim_serial_device;
+
+    struct {
+        bool active;
+        uint8_t term[20];
+        AP_CSVReader csvreader{term, sizeof(term), ','};
+        struct {
+            uint32_t timestamp_us;
+            uint8_t b;  // the byte
+        } loaded_data;
+        bool loaded;  // true if data is all valid
+        bool done_first_line = false;
+        uint8_t terms_seen;
+        uint32_t first_timestamp_us;
+        uint32_t first_emit_micros_us;
+    } logic_async_csv;
+    uint16_t read_from_async_csv(uint8_t *buffer, uint16_t space);
 };
 
 #endif