1 files changed, 171 insertions, 0 deletions
diff --git a/apps/sm-serial-read-all.cpp b/apps/sm-serial-read-all.cpp
new file mode 100644
index 0000000..bf5ef11
--- /dev/null
+++ b/apps/sm-serial-read-all.cpp
@@ -0,0 +1,171 @@
+#include "trygvis/sensor.h"
+#include "trygvis/sensor/io.h"
+#include "json.hpp"
+#include "apps.h"
+#include <thread>
+#include <boost/asio/serial_port.hpp>
+
+namespace trygvis {
+namespace apps {
+
+using namespace boost::asio;
+using namespace std;
+using namespace std::chrono;
+using namespace trygvis::apps;
+using namespace trygvis::sensor;
+using namespace trygvis::sensor::io;
+namespace po = boost::program_options;
+using json = nlohmann::json;
+
+namespace sm_serial_read_all_utils {
+
+class port_handler {
+public:
+    port_handler(string &port_name, serial_port &port, unique_ptr<SampleStreamParser> parser)
+        : port_name(port_name), port(port), parser(move(parser)) {}
+
+    void run() {
+        auto packet = make_shared<vector<uint8_t>>(1024);
+
+        while (port.is_open()) {
+            std::size_t some = port.read_some(buffer);
+
+            mutable_buffers_1 chunk = boost::asio::buffer(data, some);
+            parser->process(chunk);
+        }
+
+        cerr << "port closed" << endl;
+        dead_ = true;
+    }
+
+    bool dead() const {
+        return dead_;
+    }
+
+    const string port_name;
+
+private:
+    static const size_t size = 1024;
+    bool dead_ = false;
+    serial_port &port;
+    uint8_t data[size];
+    mutable_buffers_1 buffer = boost::asio::buffer(data, size);
+
+    unique_ptr<SampleStreamParser> parser;
+};
+}
+
+// This only supports Linux
+#if 1
+
+#include <sys/types.h>
+#include <dirent.h>
+
+vector<string> &&find_ports() {
+    DIR *dir = opendir("/dev");
+    vector<string> ports;
+
+    if (!dir) {
+        return move(ports);
+    }
+
+    struct dirent *entry;
+    while ((entry = readdir(dir)) != NULL) {
+        string name = entry->d_name;
+
+        if (name.find("ttyS") || name.find("ttyUSB") || name.find("ttyACM")) {
+            ports.emplace_back("/dev/" + name);
+        }
+    }
+
+    closedir(dir);
+
+    return move(ports);
+}
+
+#endif
+
+class sm_serial_read_all : public app {
+    typedef trygvis::apps::sm_serial_read_all_utils::port_handler port_handler;
+
+public:
+    sm_serial_read_all() : app("sm-serial-read-all") {}
+
+    ~sm_serial_read_all() = default;
+
+    bool run = true;
+    sample_format_type format;
+
+    void add_options(po::options_description_easy_init &options) override {
+        options("port", po::value<string>()->required(), "The serial port to read");
+        options("format", po::value<sample_format_type>(&format)->default_value(sample_format_type::KEY_VALUE),
+                "Output format");
+    }
+
+    static void handler_thread(port_handler *handler) {
+        handler->run();
+    }
+
+    int main(app_execution &execution) override {
+        auto desc = execution.desc;
+        auto vm = execution.vm;
+
+        io_service io_service;
+        uint32_t baud_rate = 115200;
+
+        map<string, port_handler *> active_ports;
+
+        KeyDictionary outputDict;
+        auto output_stream = shared_ptr<ostream>(&cout, noop_deleter);
+        auto output = open_sample_output_stream(output_stream, outputDict, format);
+        auto tso = thread_safe_sample_output_stream(move(output));
+        shared_ptr<SampleOutputStream> thread_safe_output(move(tso));
+
+        while (run) {
+            // Port cleanup
+            for (auto it = active_ports.begin(); it != active_ports.end(); ++it) {
+                if (it->second->dead()) {
+                    cerr << "Removing dead port " << it->second->port_name << endl;
+                    it = active_ports.erase(it);
+                }
+            }
+
+            // Discover new ports
+            auto ports = find_ports();
+
+            for (auto port_name : ports) {
+                if (active_ports.find(port_name) != active_ports.end()) {
+                    cerr << "New port " << port_name;
+
+                    serial_port port(io_service);
+                    port.open(port_name);
+                    port.set_option(serial_port_base::baud_rate(baud_rate));
+                    port.set_option(serial_port_base::character_size(8));
+                    port.set_option(serial_port_base::parity(serial_port_base::parity::none));
+                    port.set_option(serial_port_base::stop_bits(serial_port_base::stop_bits::one));
+                    port.set_option(serial_port_base::flow_control(serial_port_base::flow_control::none));
+
+                    KeyDictionary parserDict; // TODO: dette feiler
+                    unique_ptr<SampleStreamParser> parser =
+                        open_sample_stream_parser(thread_safe_output, parserDict, sample_format_type::KEY_VALUE);
+
+                    auto handler = new port_handler(port_name, port, move(parser));
+                    active_ports[port_name] = handler;
+                    std::thread thread(handler_thread, handler);
+                }
+            }
+
+            sleep(1);
+        }
+
+        return EXIT_SUCCESS;
+    }
+};
+}
+}
+
+int main(int argc, const char *argv[]) {
+    using namespace trygvis::apps;
+
+    return real_main(new sm_serial_read_all(), argc, argv);
+}