Sophiar2/src/sensor/optoforce/optoforce_daq.cpp

#include "optoforce_daq.h"
#include "core/basic_obj_types.hpp"
#include "utility/config_utility.hpp"
#include "utility/coro_worker.hpp"
#include "utility/coro_worker_helper_func.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;

        variable_index_type force_var_index;
        variable_index_type torque_var_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() {
            auto buf = static_memory<message_length_3>{};
            co_await async_fill_memory_from(*conn, buf);
            auto reader = versatile_reader<daq_endian>(buf);

            // check header and checksum
            assert(memcmp(header_magic_3, buf.data(), 4) == 0);
            assert(check_checksum<message_length_3>(buf.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_VARIABLE_VAL_WITH_TS(scalarxyz_obj, force_var_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_VARIABLE_VAL_WITH_TS(scalarxyz_obj, torque_var_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
            auto buf = static_memory<message_length_config>{};
            auto writer = versatile_writer<daq_endian>(buf);
            memcpy(buf.data(), header_magic_config, 4);
            writer.manual_offset(4);
            writer << speed << filter << zero;
            auto checksum = calc_checksum<message_length_config>(buf.data());
            writer << checksum;
            co_await async_write_memory_to(*conn, buf);
            co_return;
        }

        awaitable<void> load_device_config(const nlohmann::json &config) {
            // device type
            device_type = LOAD_UINT_ITEM("device_type");
            assert(device_type == 3); // TODO only type 3 is supported

            // global objects
            switch (device_type) {
                case 3: {
                    force_var_index = LOAD_VARIABLE_INDEX(scalarxyz_obj, "force_output_name");
                    torque_var_index = LOAD_VARIABLE_INDEX(scalarxyz_obj, "torque_output_name");
                    break;
                }
                default: {
                    assert(false);
                    break;
                }
            }

            // resolution
            force_resolution = LOAD_FLOAT_ITEM("force_resolution");
            torque_resolution = LOAD_FLOAT_ITEM("torque_resolution");

            // report config
            auto freq = LOAD_UINT_ITEM("report_frequency");
            assert(freq >= 10);
            auto filter = LOAD_UINT_ITEM("filter_type");
            co_await send_config_message(1000 / freq, filter);
        }

        void setup_connection(const nlohmann::json &config) {
            assert(conn == nullptr);
            conn.reset(new serial_port(*global_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(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) noexcept {
        co_return true;
    }

    awaitable<bool> optoforce_daq::on_start(const nlohmann::json &config) noexcept {
        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() noexcept {
        pimpl->worker->cancel();
        co_await pimpl->worker->coro_wait_stop();
        pimpl->worker = nullptr;
        co_return;
    }

    awaitable<void> optoforce_daq::on_reset() noexcept {
        co_return;
    }
}