实现了 optoforce_dqp 待测试

src/core/sophiar_manager.h

@@ -115,6 +115,9 @@ namespace sophiar {
 #define UPDATE_GLOBAL_OBJ_VALUE(obj_type, obj_index, obj_value) \
     global_sophiar_manager.update_global_obj<obj_type>(obj_index, obj_type::new_instance(obj_value))
 
+#define UPDATE_GLOBAL_OBJ_VALUE_TS(obj_type, obj_index, obj_value, ts) \
+    global_sophiar_manager.update_global_obj<obj_type>(obj_index, obj_type::new_instance(obj_value), ts)
+
 }
 
 #endif //SOPHIAR2_SOPHIAR_MANAGER_H

src/sensor/optoforce/optoforce_daq.cpp

@@ -0,0 +1,237 @@
+#include "optoforce_daq.h"
+
+#include "core/basic_obj_types.hpp"
+#include "utility/coro_worker.hpp"
+#include "utility/versatile_buffer2.hpp"
+
+#include <boost/asio/serial_port.hpp>
+#include <boost/asio/use_awaitable.hpp>
+#include <boost/crc.hpp>
+#include <boost/smart_ptr/scoped_ptr.hpp>
+
+#include <spdlog/spdlog.h>
+
+#include <numeric>
+
+namespace sophiar {
+
+    using boost::asio::async_read;
+    using boost::asio::async_write;
+    using boost::asio::awaitable;
+    using boost::asio::serial_port;
+    using boost::asio::use_awaitable;
+    using boost::scoped_ptr;
+
+    struct optoforce_daq::impl {
+
+        static constexpr auto daq_endian = boost::endian::order::big;
+        static constexpr uint8_t header_magic_1[] = {170, 7, 8, 10};
+        static constexpr uint8_t header_magic_2[] = {170, 7, 8, 28};
+        static constexpr uint8_t header_magic_3[] = {170, 7, 8, 16};
+        static constexpr uint8_t header_magic_config[] = {170, 0, 50, 3};
+        static constexpr auto message_length_1 = 16;
+        static constexpr auto message_length_2 = 34;
+        static constexpr auto message_length_3 = 22;
+        static constexpr auto message_length_config = 9;
+
+        optoforce_daq *q_this = nullptr;
+
+        scoped_ptr<serial_port> conn;
+        uint8_t device_type = 3;
+        coro_worker::pointer worker;
+
+        double force_resolution, torque_resolution;
+
+        global_obj_index_type force_obj_index;
+        global_obj_index_type torque_obj_index;
+
+        template<size_t MessageLength>
+        uint16_t calc_checksum(const char *data) {
+            return std::accumulate(data, data + MessageLength - 2, static_cast<uint16_t>(0));
+        }
+
+        template<size_t MessageLength>
+        uint16_t get_checksum_val(const char *data) {
+            uint16_t checksum_val = *reinterpret_cast<const uint16_t *>(data + MessageLength - 2);
+            swap_net_loc_endian<daq_endian>(checksum_val);
+            return checksum_val;
+        }
+
+        template<size_t MessageLength>
+        bool check_checksum(const char *data) {
+            auto checksum_val = calc_checksum<MessageLength>(data);
+            auto checksum = get_checksum_val<MessageLength>(data);
+            return checksum_val == checksum;
+        }
+
+        awaitable<bool> handle_type_3_message() {
+            using memory_type = static_memory<message_length_3>;
+            auto reader = versatile_readable_buffer<daq_endian, memory_type>();
+            co_await reader.async_read_from(*conn, message_length_3);
+
+            // check header and checksum
+            assert(memcmp(header_magic_3, reader.cur_data(), 4) == 0);
+            assert(check_checksum<message_length_3>(reader.cur_data()));
+
+            reader.manual_offset(4); // ignore header
+            uint16_t sample_count, status;
+            reader >> sample_count >> status;
+            if (status != 0) {
+                // TODO show log for abnormal status
+            }
+
+            // force component
+            auto ts = current_timestamp();
+            uint16_t fx, fy, fz;
+            reader >> fx >> fy >> fz;
+            auto force_value = Eigen::Vector3d(
+                    fx * force_resolution,
+                    fy * force_resolution,
+                    fz * force_resolution);
+            UPDATE_GLOBAL_OBJ_VALUE_TS(scalarxyz_obj, force_obj_index, std::move(force_value), ts);
+
+            // torque component
+            uint16_t tx, ty, tz;
+            reader >> tx >> ty >> tz;
+            auto torque_value = Eigen::Vector3d(
+                    tx * torque_resolution,
+                    ty * torque_resolution,
+                    tz * torque_resolution);
+            UPDATE_GLOBAL_OBJ_VALUE_TS(scalarxyz_obj, torque_obj_index, std::move(torque_value), ts);
+
+            co_return true;
+        }
+
+        awaitable<void> send_config_message(uint8_t speed, uint8_t filter, uint8_t zero = 0) {
+            assert(zero == 0); // zero will be done manually
+            using memory_type = static_memory<message_length_config>;
+            auto writer = versatile_writable_buffer<daq_endian, memory_type>();
+            memcpy(writer.cur_data(), header_magic_config, 4);
+            writer.manual_offset(4);
+            writer << speed << filter << zero;
+            auto checksum = calc_checksum<message_length_config>(writer.get_buffer().data());
+            writer << checksum;
+            co_await writer.async_write_to(*conn);
+            co_return;
+        }
+
+        awaitable<void> load_device_config(const nlohmann::json &config) {
+            // device type
+            assert(config.contains("device_type"));
+            assert(config["device_type"].is_number_unsigned());
+            device_type = config["device_type"].get<uint8_t>();
+            assert(device_type == 3); // TODO only type 3 is supported
+
+            // global objects
+            switch (device_type) {
+                case 3: {
+                    assert(config.contains("force_output_name"));
+                    assert(config.contains("torque_output_name"));
+                    assert(config["force_output_name"].is_string());
+                    assert(config["torque_output_name"].is_string());
+                    auto force_output_name = config["force_output_name"].get<std::string>();
+                    auto torque_output_name = config["torque_output_name"].get<std::string>();
+                    force_obj_index = get_manager().
+                            register_global_obj<scalarxyz_obj>(force_output_name);
+                    torque_obj_index = get_manager().
+                            register_global_obj<scalarxyz_obj>(torque_output_name);
+                    break;
+                }
+                default: {
+                    assert(false);
+                    break;
+                }
+            }
+
+            // resolution
+            assert(config.contains("force_resolution"));
+            assert(config.contains("torque_resolution"));
+            assert(config["force_resolution"].is_number_float());
+            assert(config["torque_resolution"].is_number_float());
+            force_resolution = config["force_resolution"].get<double>();
+            torque_resolution = config["torque_resolution"].get<double>();
+
+            // report config
+            assert(config.contains("report_frequency"));
+            assert(config.contains("filter_type"));
+            assert(config["report_frequency"].is_number_unsigned());
+            assert(config["filter_type"].is_number_unsigned());
+            auto freq = config["report_frequency"].get<uint16_t>();
+            assert(freq >= 10);
+            auto filter = config["filter_type"].get<uint8_t>();
+            co_await send_config_message(1000 / freq, filter);
+        }
+
+        void setup_connection(const nlohmann::json &config) {
+            assert(conn == nullptr);
+            conn.reset(new serial_port(get_context()));
+
+            // open serial port
+            assert(config.contains("com_port"));
+            assert(config["com_port"].is_string());
+            auto com_port_name = config["com_port"].get<std::string>();
+            conn->open(com_port_name);
+            assert(conn->is_open());
+
+            // config
+            conn->set_option(serial_port::baud_rate(115200));
+            conn->set_option(serial_port::stop_bits(serial_port::stop_bits::one));
+            conn->set_option(serial_port::parity(serial_port::parity::none));
+            conn->set_option(serial_port::character_size(8));
+            conn->set_option(serial_port::flow_control(serial_port::flow_control::none));
+        }
+
+        void create_worker() {
+            auto exit_func = stop_on_exit_func(q_this);
+            switch (device_type) {
+                case 3: {
+                    auto func_wrapper = make_noexcept_func(
+                            std::bind(&impl::handle_type_3_message, this));
+                    worker = make_infinite_coro_worker(
+                            get_context(), std::move(func_wrapper), std::move(exit_func));
+                    break;
+                }
+                default: {
+                    assert(false);
+                    break;
+                }
+            }
+            worker->run();
+        }
+
+    };
+
+    optoforce_daq::optoforce_daq()
+            : pimpl(std::make_unique<impl>()) {
+        pimpl->q_this = this;
+    }
+
+    optoforce_daq::~optoforce_daq() = default;
+
+    awaitable<bool> optoforce_daq::on_init(const nlohmann::json &config) {
+        co_return true;
+    }
+
+    awaitable<bool> optoforce_daq::on_start(const nlohmann::json &config) {
+        try {
+            pimpl->setup_connection(config);
+            co_await pimpl->load_device_config(config);
+            pimpl->create_worker();
+        } catch (std::exception &e) {
+            // TODO show log
+            co_return false;
+        }
+        co_return true;
+    }
+
+    awaitable<void> optoforce_daq::on_stop() {
+        pimpl->worker->cancel();
+        co_await pimpl->worker->coro_wait_stop();
+        pimpl->worker = nullptr;
+        co_return;
+    }
+
+    awaitable<void> optoforce_daq::on_reset() {
+        co_return;
+    }
+}

src/sensor/optoforce/optoforce_daq.h

@@ -0,0 +1,8 @@
+#ifndef SOPHIAR2_OPTOFORCE_DAQ_H
+#define SOPHIAR2_OPTOFORCE_DAQ_H
+
+#include "core/tristate_obj.h"
+
+DEFAULT_TRISTATE_OBJ_DEF(optoforce_daq)
+
+#endif //SOPHIAR2_OPTOFORCE_DAQ_H

src/utility/coro_worker.hpp

@@ -115,7 +115,7 @@ namespace sophiar {
 
     template<typename Executor, typename FuncType,
             typename ExitFuncType = decltype(coro_worker::empty_func) const &>
-    auto make_infinite_coro_worker(Executor &executor, FuncType &&func,
+    inline auto make_infinite_coro_worker(Executor &executor, FuncType &&func,
                                    ExitFuncType &&exit_func = coro_worker::empty_func) {
         return coro_worker::pointer(new coro_worker_impl<Executor, FuncType, ExitFuncType>(
                 executor, std::forward<FuncType>(func), std::forward<ExitFuncType>(exit_func))
@@ -124,7 +124,7 @@ namespace sophiar {
 
     template<typename Executor, typename FuncType,
             typename ExitFuncType = decltype(coro_worker::empty_func) const &>
-    auto make_interval_coro_worker(Executor &executor, std::chrono::milliseconds interval,
+    inline auto make_interval_coro_worker(Executor &executor, std::chrono::milliseconds interval,
                                    FuncType &&func, ExitFuncType &&exit_func = coro_worker::empty_func) {
         auto worker_func = [
                 interval,
@@ -144,7 +144,7 @@ namespace sophiar {
 
     template<typename FuncType,
             typename ErrorHandlerType = decltype(empty_exception_handler) const &>
-    auto make_noexcept_func(FuncType &&func,
+    inline auto make_noexcept_func(FuncType &&func,
                             ErrorHandlerType &&error_handler = empty_exception_handler) {
         static_assert(std::is_convertible_v<
                 decltype(std::declval<FuncType>()()),
@@ -169,6 +169,28 @@ namespace sophiar {
         return std::move(noexcept_func);
     }
 
+    inline auto stop_on_exit_func(tristate_obj *obj) {
+        auto func = [obj]() {
+            assert(obj != nullptr);
+            boost::asio::co_spawn(
+                    global_context,
+                    std::bind(&tristate_obj::stop, obj),
+                    boost::asio::detached);
+        };
+        return std::move(func);
+    }
+
+    inline auto reset_on_exit_func(tristate_obj *obj) {
+        auto func = [obj]() {
+            assert(obj != nullptr);
+            boost::asio::co_spawn(
+                    global_context,
+                    std::bind(&tristate_obj::reset, obj),
+                    boost::asio::detached);
+        };
+        return std::move(func);
+    }
+
 }
 
 #endif //SOPHIAR2_CORO_WORKER_HPP

src/utility/debug_utility.hpp

@@ -43,10 +43,13 @@ inline awaitable<void> coro_sleep(std::chrono::milliseconds t) {
 class string_writer {
 public:
 
+    explicit string_writer(std::string _sep = ", ")
+            : sep(std::move(_sep)) {}
+
     template<typename T>
     string_writer &operator<<(const T &val) {
         if (!is_empty) {
-            ss << ", ";
+            ss << sep;
         }
         ss << val;
         is_empty = false;
@@ -63,6 +66,7 @@ public:
 
 private:
     std::stringstream ss;
+    std::string sep;
     bool is_empty = true;
 };
 
@@ -75,7 +79,7 @@ namespace sophiar {
         auto obj_name = config["obj_name"].get<std::string>();
         auto obj_index = global_sophiar_manager.register_global_obj<SmallObjType>(obj_name);
         auto worker = make_infinite_coro_worker(global_context, [
-                obj_name, buffer = string_writer{},
+                obj_name, buffer = string_writer(),
                 obj_helper = GLOBAL_OBJ_AUTO_DELEGATE(SmallObjType, obj_index)]() mutable
                 -> awaitable<bool> {
             co_await obj_helper.coro_wait_update();

src/utility/simple_tristate_obj.hpp

@@ -52,16 +52,32 @@ namespace sophiar {
         }
 
     private:
-        coro_worker::pointer worker;
+        std::shared_ptr<coro_worker> worker;
+
+        void create_watchdog() {
+            auto watchdog_func = [this, handler = worker]() -> boost::asio::awaitable<void> {
+                co_await handler->coro_wait_stop();
+                if constexpr (Flag) {
+                    co_await stop();
+                } else {
+                    co_await reset();
+                }
+                co_return;
+            };
+            boost::asio::co_spawn(global_context, std::move(watchdog_func), boost::asio::detached);
+        }
 
         void create_worker(const nlohmann::json &config) {
             worker = CreateFunc(config);
+            create_watchdog();
             worker->run();
+            assert(worker.use_count() == 2);
         }
 
         boost::asio::awaitable<void> stop_worker() {
             worker->cancel();
             co_await worker->coro_wait_stop();
+            worker = nullptr;
             co_return;
         }
 

src/utility/versatile_buffer2.hpp

@@ -285,6 +285,10 @@ namespace sophiar {
             cur_pos += offset;
         }
 
+        const buffer_type &get_buffer() const {
+            return buffer;
+        }
+
         size_t get_cur_pos() const {
             return cur_pos;
         }
@@ -296,7 +300,7 @@ namespace sophiar {
         template<typename AsyncWriteStream>
         boost::asio::awaitable<void> async_write_to(AsyncWriteStream &s) {
             co_await boost::asio::async_write(s,
-                                              boost::asio::buffer(buffer, cur_pos),
+                                              boost::asio::buffer(buffer.data(), cur_pos),
                                               boost::asio::use_awaitable);
             cur_pos = 0;
             co_return;

tests/data/optoforce_daq_config.json

@@ -0,0 +1,19 @@
+{
+  "listen_port": 5277,
+  "object_list": [
+    {
+      "type": "optoforce_daq",
+      "name": "optoforce_dqp",
+      "start_config": {
+        "com_port": "COM5",
+        "device_type": 3,
+        "force_output_name": "flange_force",
+        "torque_output_name": "flange_torque",
+        "force_resolution": 0.01,
+        "torque_resolution": 0.001,
+        "report_frequency": 100,
+        "filter_type": 4
+      }
+    }
+  ]
+}