Cppcheck report - ethercatcpp-sensodrive: /builds/ethercatcpp/ethercatcpp-sensodrive/src/sensodrive/configuration.cpp

#include <bitset>
#include <ethercatcpp/sensojoint.h>
#include "buffers_definitions.h"
#include "coe_definitions.h"
#include "private_types_definitions.h"
#include <iostream>

namespace ethercatcpp {

using namespace sensojoint;

SensoJoint::Options& SensoJoint::Options::offsets(double pos_offset,
                                                  double torque_offset) {
    zero_pos_offset = pos_offset;
    zero_torque_offset = torque_offset;
    return *this;
}
SensoJoint::Options& SensoJoint::Options::use_haptics() {
    enable_haptics = true;
    return *this;
}
SensoJoint::Options& SensoJoint::Options::inertia(double output) {
    output_inertia = output;
    return *this;
}
SensoJoint::Options& SensoJoint::Options::endstops(double stiffness,
                                                   double damping) {
    endstop_specs.stiffness = stiffness;
    endstop_specs.damping = damping;
    return *this;
}
SensoJoint::Options& SensoJoint::Options::max(double motor_torque,
                                              double motor_vel) {
    max_motor_torque = motor_torque;
    max_motor_velocity = motor_vel;
    return *this;
}

SensoJoint::Options& SensoJoint::Options::profile(double velocity, double accel,
                                                  double decel) {

    profile_velocity = velocity;
    profile_acceleration = accel;
    profile_deceleration = decel;
    return *this;
}
SensoJoint::Options& SensoJoint::Options::torque_profile(double slope) {
    torque_slope = slope;
    return *this;
}
SensoJoint::Options& SensoJoint::Options::position_reached(double window,
                                                           double time) {
    position_window = window;
    position_window_time = time;
    return *this;
}
SensoJoint::Options& SensoJoint::Options::velocity_reached(double window,
                                                           double time) {
    velocity_window = window;
    velocity_window_time = time;
    return *this;
}
SensoJoint::Options& SensoJoint::Options::standstill(double threshold,
                                                     double time) {
    velocity_threshold = threshold;
    velocity_threshold_time = time;
    return *this;
}

SensoJoint::Options&
SensoJoint::Options::controlled_outputs(uint8_t outputs, uint8_t init_value,
                                        uint8_t end_value) {
    outputs &= 0x0F;                            // considered bits = 0000 1111
    if ((outputs & read_digital_inputs) == 0) { // not in conflict with IN IOs
        controlled_digital_outputs = outputs;
        digital_outputs_init_value = init_value;
        digital_outputs_end_value = end_value;
    }
    return *this;
}

SensoJoint::Options& SensoJoint::Options::read_inputs(uint8_t inputs) {
    inputs &= 0x0D; // considered bits = 0000 1101
    if ((inputs & controlled_digital_outputs) == 0) {
        // not in conflict with OUT IOs
        read_digital_inputs = inputs;
    }
    return *this;
}

SensoJoint::Options SensoJoint::gen_options(double control_period,
                                            double min_target_pos,
                                            double max_target_pos) {
    SensoJoint::Options opts;
    opts.control_period_ = control_period;
    opts.max_target_position = max_target_pos;
    opts.min_target_position = min_target_pos;
    return opts.endstops(1000, 100);
}

// sensojoint functions to manage options and parameters

void SensoJoint::manage_ctrl_params(const std::optional<Options>& options) {
    find_initial_position();
    manage_control_period(options);
    //////////// offsets management ////////////
    manage_torque_offset(options);
    manage_position_offset(options);

    //////////// limits management ////////////
    manage_max_output_torque();
    manage_max_motor_torque();
    manage_max_output_velocity();
    manage_max_motor_current();
    manage_max_motor_velocity();
    manage_user_max_motor_torque(options);
    manage_user_max_motor_velocity(options);
    manage_user_position_limits(options);

    /////////////////// control parameters //////////////////
    manage_quickstop_config(options);
    manage_profile_config(options);
    manage_torque_slope(options);
    manage_endstop_config(options);

    //////////////// GPIOs /////////////////////////////
    manage_digital_ios(options);
}

void SensoJoint::read_base_units_values() {
    // important parameters defining base values of units used in the
    // torque/velocity/position controllers: from synapticon, see
    // https://doc.synapticon.com/node/sw5.1/actuator_config/motor_and_gear_settings.htm#motor-gear

    // see https://doc.synapticon.com/node/sw5.1/objects_html/2xxx/2003.html
    const auto& [tor_cte_addr, tor_cte_sub, tor_cte_s] =
        dictionary_.object("torque_constant");
    int32_t torque_constant; // in μNm/A
    read_sdo(tor_cte_addr, tor_cte_sub, torque_constant);
    internal_parameters_->torque_constant =
        static_cast<double>(torque_constant) / 1000000.;
    std::cout << "torque_constant raw (uNm/A): " << torque_constant
              << " user (Nm/A): " << internal_parameters_->torque_constant
              << std::endl;

    const auto& [in_inert_addr, in_inert_sub, in_inert_s] =
        dictionary_.object("input_inertia");
    uint32_t input_inertia; // in g.mm^2
    read_sdo(in_inert_addr, in_inert_sub, input_inertia);
    internal_parameters_->input_inertia =
        static_cast<double>(input_inertia) / 1000000000.;
    std::cout << "input inertia raw ( g.mm^2): " << input_inertia
              << " user (kg.m^2): " << internal_parameters_->input_inertia
              << std::endl;
    // see
    // https://doc.synapticon.com/circulo_safe_motion/sw5.4/objects_html/6xxx/6091.html
    const auto& [mot_rev_addr, mot_rev_sub, mot_rev_s] =
        dictionary_.object("gear_motor_revolutions"); // supposed to be 161
    const auto& [shaft_rev_addr, shaft_rev_sub, shaft_rev_s] =
        dictionary_.object("gear_shaft_revolutions"); // supposed to be 1
    uint32_t gear_motor_revs;
    uint32_t gear_shaft_revs;
    read_sdo(mot_rev_addr, mot_rev_sub, gear_motor_revs);
    read_sdo(shaft_rev_addr, shaft_rev_sub, gear_shaft_revs);
    internal_parameters_->gear_ratio = static_cast<double>(gear_motor_revs) /
                                       static_cast<double>(gear_shaft_revs);
    std::cout << "gear ratio, motor revs: " << gear_motor_revs
              << " shaft revs:" << gear_shaft_revs
              << " ratio: " << internal_parameters_->gear_ratio << std::endl;

    //  see
    //  https://doc.synapticon.com/node/sw5.1/objects_html/6xxx/6076.html
    const auto& [r_torque_addr, r_torque_sub, r_tor_s] =
        dictionary_.object("rated_torque");
    uint32_t rated_torque;
    read_sdo(r_torque_addr, r_torque_sub, rated_torque);
    // NOTE rated torque is in mNm -> need to get a factor for
    // converting Nm (user input) into per thousands of rated torque
    std::cout << "rated_torque raw (mNm): " << rated_torque
              << " user (Nm): " << rated_torque / 1000. << std::endl;
    conversion_->from_rated_torque_unit_ =
        static_cast<double>(rated_torque) / 1000000.0;
    conversion_->to_rated_torque_unit_ =
        1. / conversion_->from_rated_torque_unit_;

    // https://doc.synapticon.com/node/sw5.1/objects_html/6xxx/6075.html
    const auto& [r_curr_addr, r_curr_sub, r_curr_s] =
        dictionary_.object("rated_current");
    uint32_t rated_current;
    read_sdo(r_curr_addr, r_curr_sub, rated_current);
    // NOTE rated current is in mA -> need to get a factor for
    // converting A (user input) into per thousands of rated current
    conversion_->from_rated_current_unit_ =
        static_cast<double>(rated_current) / 1000000.0;
    std::cout << "rated_current raw (mA): " << rated_current
              << " user (A): " << rated_current / 1000. << std::endl;

    // see PDF documentation (mRPM <-> rad/s)
    conversion_->from_velocity_unit_ = coe::cia402::rpm2rads / 1000.0;
    conversion_->to_velocity_unit_ = 1 / conversion_->from_velocity_unit_;

    // see
    // https://doc.synapticon.com/node/sw5.1/objects_html/2xxx/2110.html
    const auto& [enc_res_addr, enc_res_sub, enc_res_s] =
        dictionary_.object("encoder_resolution");
    uint32_t encoder_resolution; // in Inc/revolution
    read_sdo(enc_res_addr, enc_res_sub, encoder_resolution);
    std::cout << "encoder_resolution, raw (inc/rev): " << encoder_resolution
              << " user (inc/rad): "
              << encoder_resolution / coe::cia402::rev_to_rad << std::endl;
    conversion_->from_position_unit_ =
        coe::cia402::rev_to_rad /                // revolution in radians
        static_cast<double>(encoder_resolution); // inc per rev
    conversion_->to_position_unit_ = 1. / conversion_->from_position_unit_;

    const auto& [tq_unit_addr, tq_unit_sub, tq_unit_s] =
        dictionary_.object("torque_units");
    uint8_t tq_unit; // in Inc/revolution
    read_sdo(tq_unit_addr, tq_unit_sub, tq_unit);
    std::cout << "torque units, raw (mNm): " << std::to_string(tq_unit)
              << " user (Nm): " << tq_unit / 1000. << std::endl;

    conversion_->to_output_torque_unit_ =
        1000. / tq_unit; // convert in mNm from Nm
    conversion_->from_output_torque_unit_ =
        0.001 * tq_unit; // convert in Nm from mNm

    // haptics related units
    // to_haptic_position_unit_ = rad2deg * 10.; // convert to unit in
    // 0.1 deg
    conversion_->to_damping_unit_ =
        conversion_->to_output_torque_unit_ * 10. /
        coe::cia402::rad2deg; // convert to 0.1 mNm s / deg
    conversion_->to_spring_stiffness_unit_ =
        conversion_->to_output_torque_unit_ /
        coe::cia402::rad2deg; // convert to mNm / deg
    conversion_->to_endstop_stiffness_unit_ =
        0.1 * conversion_->to_output_torque_unit_ /
        coe::cia402::rad2deg; // convert to 10 * mNm / deg

    conversion_->from_torque_sensor_temperature_unit_ = 0.001; // °C / 1000
    conversion_->from_drive_temperatue_unit_ = 0.001;          // °C / 1000

    // acceleration (for quick stop) mRPM/s -> rad/s²
    // conversion is the same as for velocities
    conversion_->to_acceleration_unit_ = conversion_->to_velocity_unit_;
    conversion_->from_acceleration_unit_ = conversion_->from_velocity_unit_;

    // time user: s raw: ms
    conversion_->to_time_unit_ = 1000.;
    conversion_->from_time_unit_ = 0.001;
}

void SensoJoint::find_initial_position() {
    // NOT DONE: using FoE
    // https://doc.synapticon.com/node/tutorials/foe_drive_operations.html?Highlight=FoE

    int32_t adjust_pos;
    read_sdo(encoder_params_addr, 0x01, adjust_pos);
    std::cout << "raw position, raw (inc/rev): " << adjust_pos
              << " user (rad): "
              << adjust_pos * conversion_->from_position_unit_ << std::endl;
    read_sdo(encoder_params_addr, 0x02, adjust_pos);
    options_.initial_adjusted_position =
        adjust_pos * conversion_->from_position_unit_;
    std::cout << "adjust position, raw (inc/rev): " << adjust_pos
              << " user (rad): " << options_.initial_adjusted_position
              << std::endl;
}

void SensoJoint::manage_torque_offset(const std::optional<Options>& options) {
    const auto& [addr, sub, _] = dictionary_.object("zero_torque_offset");
    int32_t torque_compensation_offset = 0;
    // torque offset
    if (options.has_value() and options->zero_torque_offset != 0.) {
        // an offset to set the zero postion is defined
        torque_compensation_offset = static_cast<int32_t>(std::round(
            options->zero_torque_offset * conversion_->to_output_torque_unit_));
        write_sdo(addr, sub, torque_compensation_offset);
        std::cout << "WRITE torque offset, raw: " << torque_compensation_offset
                  << " user: " << options->zero_torque_offset << std::endl;
    } // otherwise keep the offset 0
    read_sdo(addr, sub, torque_compensation_offset);
    options_.zero_torque_offset =
        torque_compensation_offset * conversion_->from_output_torque_unit_;
    std::cout << "READ torque offset, raw: " << torque_compensation_offset
              << " user: " << options_.zero_torque_offset << std::endl;
}
void SensoJoint::manage_position_offset(const std::optional<Options>& options) {
    // position offset
    const auto& [addr, sub, bits] = dictionary_.object("zero_position_offset");
    int32_t zero_position_offset = 0;
    if (options.has_value()) {
        // an offset to set the zero postion is defined
        zero_position_offset = static_cast<int32_t>(std::round(
            options->zero_pos_offset * conversion_->to_position_unit_));
        write_sdo(addr, sub, zero_position_offset);
        std::cout << "WRITE pos offset, raw: " << zero_position_offset
                  << " user: " << options->zero_pos_offset << std::endl;
    }
    read_sdo(addr, sub, zero_position_offset);
    options_.zero_pos_offset =
        zero_position_offset * conversion_->from_position_unit_;
    std::cout << "READ pos offset, raw: " << zero_position_offset
              << " user: " << options_.zero_pos_offset << std::endl;
}

void SensoJoint::manage_torque_slope(const std::optional<Options>& options) {
    const auto& [addr, sub, _] = dictionary_.object("torque_slope");
    // torque slope for torque profile
    uint32_t torque_slope = 0;
    if (options.has_value() and options->torque_slope != 0.) {
        torque_slope = static_cast<uint32_t>(std::round(
            options->torque_slope * conversion_->to_rated_torque_unit_));
        write_sdo(addr, sub, torque_slope);
        std::cout << "WRITE torque slope, raw: " << torque_slope
                  << " user: " << options->torque_slope << std::endl;
    } // otherwise keep the offset 0
    read_sdo(addr, sub, torque_slope);
    options_.torque_slope = torque_slope * conversion_->from_rated_torque_unit_;
    std::cout << "READ torque slope, raw: " << torque_slope
              << " user: " << options_.torque_slope << std::endl;
}

void SensoJoint::manage_max_output_torque() {
    // absolute max output torque (cannot be changed)
    const auto& [amot_addr, amot_sub, _] =
        dictionary_.object("absolute_max_output_torque");
    uint32_t max_torque;
    read_sdo(amot_addr, amot_sub, max_torque);
    internal_parameters_->absolute_max_output_torque =
        max_torque * conversion_->from_output_torque_unit_;
    std::cout << "absolute_max_output_torque, raw: " << max_torque
              << " user: " << internal_parameters_->absolute_max_output_torque
              << std::endl;
}

void SensoJoint::manage_max_output_velocity() {
    // absolute max output velocity (cannot be changed)
    uint32_t max_velocity;
    const auto& [amov_addr, amov_sub, _] =
        dictionary_.object("absolute_max_output_velocity");
    read_sdo(amov_addr, amov_sub, max_velocity);
    internal_parameters_->absolute_max_output_velocity =
        max_velocity * conversion_->from_velocity_unit_;
    std::cout << "absolute_max_output_velocity, raw: " << max_velocity
              << " user: " << internal_parameters_->absolute_max_output_velocity
              << std::endl;
}
void SensoJoint::manage_max_motor_torque() {
    // absolute max motor torque (cannot be changed)
    const auto& [ammt_addr, ammt_sub, _] =
        dictionary_.object("absolute_max_motor_torque");
    uint32_t max_torque;
    read_sdo(ammt_addr, ammt_sub, max_torque);
    internal_parameters_->absolute_max_motor_torque =
        max_torque * conversion_->from_rated_torque_unit_;
    std::cout << "absolute_max_motor_torque, raw: " << max_torque
              << " user: " << internal_parameters_->absolute_max_motor_torque
              << std::endl;
}
void SensoJoint::manage_max_motor_current() {
    // absolute max motor current (could be changed, but initialized
    // correctly by the firmware) see
    // https://doc.synapticon.com/node/sw5.1/objects_html/6xxx/6073.html
    const auto& [ammc_addr, ammc_sub, ammc_s] =
        dictionary_.object("absolute_max_motor_current");
    uint16_t max_current;
    read_sdo(ammc_addr, ammc_sub, max_current);
    internal_parameters_->absolute_max_motor_current =
        max_current * conversion_->from_rated_current_unit_;
    std::cout << "absolute_max_motor_current, raw: " << max_current
              << " user: " << internal_parameters_->absolute_max_motor_current
              << std::endl;
}
void SensoJoint::manage_max_motor_velocity() {
    // memorize the initial max motor velocity and consider it as absolute
    // max
    const auto& [mmv_addr, mmv_sub, mmv_s] =
        dictionary_.object("max_motor_velocity");
    uint32_t max_velocity;
    read_sdo(mmv_addr, mmv_sub, max_velocity);
    internal_parameters_->absolute_max_motor_velocity =
        max_velocity * conversion_->from_velocity_unit_;
    std::cout << "absolute_max_motor_velocity, raw: " << max_velocity
              << " user: " << internal_parameters_->absolute_max_motor_velocity
              << std::endl;
}

void SensoJoint::manage_user_max_motor_torque(
    const std::optional<Options>& options) {
    // motor torque, see
    // https://doc.synapticon.com/node/sw5.1/objects_html/6xxx/6072.html

    const auto& [mmt_addr, mmt_sub, mmt_s] =
        dictionary_.object("max_motor_torque");
    uint16_t max_motor_torque;
    if (options.has_value() and options->max_motor_torque != 0.) {
        // set the max motor torque only if not violating the absolute max
        // torque
        if (options->max_motor_torque <=
            internal_parameters_->absolute_max_motor_torque) {
            max_motor_torque = static_cast<uint16_t>(
                std::round(options->max_motor_torque *
                           conversion_->to_rated_torque_unit_));
            write_sdo(mmt_addr, mmt_sub, max_motor_torque);
            options_.max_motor_torque = options->max_motor_torque;
            std::cout << "WRITE max_motor_torque, raw: " << max_motor_torque
                      << " user: " << options_.max_motor_torque << std::endl;
            return;
        }
    }
    // else no max torque defined, keep the default value defined in
    // device
    read_sdo(mmt_addr, mmt_sub, max_motor_torque);
    options_.max_motor_torque =
        max_motor_torque * conversion_->from_rated_torque_unit_;
    std::cout << "READ max_motor_torque, raw: " << max_motor_torque
              << " user: " << options_.max_motor_torque << std::endl;
}
void SensoJoint::manage_user_max_motor_velocity(
    const std::optional<Options>& options) {
    // motor velocity
    const auto& [mmv_addr, mmv_sub, mmv_s] =
        dictionary_.object("max_motor_velocity");
    uint32_t max_velocity;
    if (options.has_value() and options->max_motor_velocity != 0.) {
        // set the max motor velocity only if not violating the absolute max
        // velocity
        if (options->max_motor_velocity <=
            internal_parameters_->absolute_max_motor_velocity) {
            max_velocity = static_cast<uint32_t>(std::round(
                options->max_motor_velocity * conversion_->to_velocity_unit_));
            write_sdo(mmv_addr, mmv_sub, max_velocity);
            options_.max_motor_velocity = options->max_motor_velocity;
            std::cout << "WRITE max_motor_velocity, raw: " << max_velocity
                      << " user: " << options_.max_motor_velocity << std::endl;
            return;
        }
    }
    options_.max_motor_velocity =
        internal_parameters_->absolute_max_motor_velocity;
}

void SensoJoint::manage_user_position_limits(
    const std::optional<Options>& options) {
    // min/max positions
    const auto& [minp_addr, minp_sub, minp_s] =
        dictionary_.object("min_target_position");
    const auto& [maxnp_addr, maxp_sub, maxp_s] =
        dictionary_.object("max_target_position");
    int32_t position;
    if (options.has_value() and
        (options->max_target_position != 0. and
         options->min_target_position != 0.) and<--- Syntax Error: AST broken, 'options' doesn't have a parent.
        options->max_target_position > options->min_target_position) {

        position = static_cast<int32_t>(std::round(
            options->min_target_position * conversion_->to_position_unit_));
        std::cout << "WRITE min_target_position, raw: " << position
                  << " user: " << options->min_target_position << std::endl;
        write_sdo(minp_addr, minp_sub, position);
        position = static_cast<int32_t>(std::round(
            options->max_target_position * conversion_->to_position_unit_));
        std::cout << "WRITE max_target_position, raw: " << position
                  << " user: " << options->max_target_position << std::endl;
        write_sdo(maxnp_addr, maxp_sub, position);
    }
    // else no min/max position defined, keep the default value defined
    // in device
    read_sdo(minp_addr, minp_sub, position);
    options_.min_target_position = position * conversion_->from_position_unit_;
    std::cout << "READ min_target_position raw: " << position
              << " user (rad): " << options_.min_target_position << std::endl;
    read_sdo(maxnp_addr, maxp_sub, position);
    options_.max_target_position = position * conversion_->from_position_unit_;

    std::cout << "READ max_target_position: " << position
              << " user (rad): " << options_.max_target_position << std::endl;
}

void SensoJoint::manage_quickstop_config(
    const std::optional<Options>& options) {
    // Quick stop deceleration
    const auto& [qsa_addr, qsa_sub, qsa_s] =
        dictionary_.object("quick_stop_deceleration");
    uint32_t acceleration;
    if (options.has_value()) {
        options_.output_inertia = options->output_inertia;
    } else {
        options_.output_inertia = 0.; // no external inertia defined
    }
    auto total_inertia =
        options->output_inertia + internal_parameters_->input_inertia *
                                      (internal_parameters_->gear_ratio *
                                       internal_parameters_->gear_ratio);

    auto max_decel =
        internal_parameters_->absolute_max_output_torque / total_inertia;

    acceleration = static_cast<uint32_t>(
        std::round(max_decel * conversion_->to_acceleration_unit_));
    write_sdo(qsa_addr, qsa_sub, acceleration);
    std::cout << "WRITE quick_stop_deceleration, raw: " << acceleration
              << " user: " << max_decel << std::endl;
    read_sdo(qsa_addr, qsa_sub, acceleration);
    internal_parameters_->quick_stop_deceleration =
        acceleration * conversion_->from_acceleration_unit_;
    std::cout << "READ quick_stop_deceleration, raw: " << acceleration
              << " user: " << internal_parameters_->quick_stop_deceleration
              << std::endl;
}

void SensoJoint::manage_profile_config(const std::optional<Options>& options) {

    // Profile velocity
    {
        const auto& [prv_addr, prv_sub, prv_s] =
            dictionary_.object("profile_velocity");
        uint32_t velocity;
        if (options.has_value() and options->profile_velocity != 0.) {
            if (options->profile_velocity <=
                internal_parameters_->absolute_max_output_velocity) {
                velocity = static_cast<uint32_t>(
                    std::round(options->profile_velocity *
                               conversion_->to_velocity_unit_));

                write_sdo(prv_addr, prv_sub, velocity);
                std::cout << "WRITE profile_velocity, raw: " << velocity
                          << " user: " << options->profile_velocity
                          << std::endl;
            }
        }
        // else no profile velocity defined, keep the default value
        // defined in device
        read_sdo(prv_addr, prv_sub, velocity);
        options_.profile_velocity = velocity * conversion_->from_velocity_unit_;
        std::cout << "READ profile_velocity, raw: " << velocity
                  << " user: " << options_.profile_velocity << std::endl;
    }
    // Profile acceleration
    {
        const auto& [pra_addr, pra_sub, pra_s] =
            dictionary_.object("profile_acceleration");
        uint32_t acceleration;
        if (options.has_value() and options->profile_acceleration != 0.) {
            acceleration = static_cast<uint32_t>(
                std::round(options->profile_acceleration *
                           conversion_->to_acceleration_unit_));
            write_sdo(pra_addr, pra_sub, acceleration);
            std::cout << "WRITE profile_acceleration, raw: " << acceleration
                      << " user: " << options->profile_acceleration
                      << std::endl;
        }
        // else no profile deceleration defined, keep the default value
        // defined in device
        read_sdo(pra_addr, pra_sub, acceleration);
        options_.profile_acceleration =
            acceleration * conversion_->from_acceleration_unit_;
        std::cout << "READ profile_acceleration, raw: " << acceleration
                  << " user: " << options_.profile_acceleration << std::endl;

        // Profile deceleration
        const auto& [prd_addr, prd_sub, prd_s] =
            dictionary_.object("profile_deceleration");
        if (options.has_value() and options->profile_deceleration != 0.) {
            acceleration = static_cast<uint32_t>(
                std::round(options->profile_deceleration *
                           conversion_->to_acceleration_unit_));
            write_sdo(prd_addr, prd_sub, acceleration);
            std::cout << "WRITE profile_deceleration, raw: " << acceleration
                      << " user: " << options->profile_deceleration
                      << std::endl;
        }
        // else no profile deceleration defined, keep the default value
        // defined in device
        read_sdo(prd_addr, prd_sub, acceleration);
        options_.profile_deceleration =
            acceleration * conversion_->from_acceleration_unit_;
        std::cout << "READ profile_deceleration, raw: " << acceleration
                  << " user: " << options_.profile_deceleration << std::endl;
    }
    // position window (PPM) -> defining target reached
    {
        const auto& [pow_addr, pow_sub, pow_s] =
            dictionary_.object("position_window");
        uint32_t position_window;
        if (options.has_value() and options->position_window > 0.) {
            position_window = static_cast<uint32_t>(std::round(
                options->position_window * conversion_->to_position_unit_));
            write_sdo(pow_addr, pow_sub, position_window);
            std::cout << "WRITE position_window, raw: " << position_window
                      << " user: " << options->position_window << std::endl;
        }
        // else no profile position_window defined, keep the default value
        // defined in device
        read_sdo(pow_addr, pow_sub, position_window);
        options_.position_window =
            position_window * conversion_->from_position_unit_;
        std::cout << "READ position_window, raw: " << position_window
                  << " user: " << options_.position_window << std::endl;

        const auto& [pwt_addr, pwt_sub, pwt_s] =
            dictionary_.object("position_window_time");
        uint16_t position_window_time;
        if (options.has_value() and options->position_window_time > 0.) {
            position_window_time = static_cast<uint16_t>(std::round(
                options->position_window_time * conversion_->to_time_unit_));
            write_sdo(pwt_addr, pwt_sub, position_window_time);
            std::cout << "WRITE position_window_time, raw: "
                      << position_window_time
                      << " user: " << options->position_window_time
                      << std::endl;
        }
        // else no profile position_window_time defined, keep the default value
        // defined in device
        read_sdo(pwt_addr, pwt_sub, position_window_time);
        options_.position_window_time =
            position_window_time * conversion_->from_time_unit_;
        std::cout << "READ position_window_time, raw: " << position_window_time
                  << " user: " << options_.position_window_time << std::endl;
    }
    // velocity threshold (PVM)-> defining conditions for standstill
    {
        const auto& [vth_addr, vth_sub, vth_s] =
            dictionary_.object("velocity_threshold");
        uint16_t velocity_threshold;
        if (options.has_value() and options->velocity_threshold > 0.) {
            velocity_threshold = static_cast<uint16_t>(std::round(
                options->velocity_threshold * conversion_->to_velocity_unit_));
            write_sdo(vth_addr, vth_sub, velocity_threshold);
            std::cout << "WRITE velocity_threshold, raw: " << velocity_threshold
                      << " user: " << options->velocity_threshold << std::endl;
        }
        read_sdo(vth_addr, vth_sub, velocity_threshold);
        options_.velocity_threshold =
            velocity_threshold * conversion_->from_velocity_unit_;
        std::cout << "READ velocity_threshold, raw: " << velocity_threshold
                  << " user: " << options_.velocity_threshold << std::endl;

        const auto& [vtt_addr, vtt_sub, vtt_s] =
            dictionary_.object("velocity_threshold_time");
        uint16_t velocity_threshold_time;
        if (options.has_value() and options->velocity_threshold_time > 0.) {
            velocity_threshold_time = static_cast<uint16_t>(std::round(
                options->velocity_threshold_time * conversion_->to_time_unit_));
            write_sdo(vtt_addr, vtt_sub, velocity_threshold_time);
            std::cout << "WRITE velocity_threshold_time, raw: "
                      << velocity_threshold_time
                      << " user: " << options->velocity_threshold_time
                      << std::endl;
        }
        read_sdo(vtt_addr, vtt_sub, velocity_threshold_time);
        options_.velocity_threshold_time =
            velocity_threshold_time * conversion_->from_time_unit_;
        std::cout << "READ velocity_threshold_time, raw: "
                  << velocity_threshold_time
                  << " user: " << options_.velocity_threshold_time << std::endl;
    }
    // velocity window (PVM) -> defining conditions for target reached
    {
        const auto& [vwi_addr, vwi_sub, vwi_s] =
            dictionary_.object("velocity_window");
        uint16_t velocity_window;
        if (options.has_value() and options->velocity_window > 0.) {
            velocity_window = static_cast<uint16_t>(std::round(
                options->velocity_window * conversion_->to_velocity_unit_));
            write_sdo(vwi_sub, vwi_s, velocity_window);
            std::cout << "WRITE velocity_window, raw: " << velocity_window
                      << " user: " << options->velocity_window << std::endl;
        }
        read_sdo(vwi_addr, vwi_sub, velocity_window);
        options_.velocity_window =
            velocity_window * conversion_->from_velocity_unit_;
        std::cout << "READ velocity_window, raw: " << velocity_window
                  << " user: " << options_.velocity_window << std::endl;

        const auto& [vwt_addr, vwt_sub, vwt_s] =
            dictionary_.object("velocity_window_time");
        uint16_t velocity_window_time;
        if (options.has_value() and options->velocity_window_time > 0.) {
            velocity_window_time = static_cast<uint16_t>(std::round(
                options->velocity_window_time * conversion_->to_time_unit_));
            write_sdo(vwt_addr, vwt_sub, velocity_window_time);
            std::cout << "WRITE velocity_window_time, raw: "
                      << velocity_window_time
                      << " user: " << options->velocity_window_time
                      << std::endl;
        }
        read_sdo(vwt_addr, vwt_sub, velocity_window_time);
        options_.velocity_window_time =
            velocity_window_time * conversion_->from_time_unit_;
        std::cout << "READ velocity_window_time, raw: " << velocity_window_time
                  << " user: " << options_.velocity_window_time << std::endl;
    }
}

void SensoJoint::manage_endstop_config(const std::optional<Options>& options) {

    // enstops for advanced control loops (these are limits but
    // functionning in a different way than software limits)
    uint16_t endstop_stiffness = 0; // in 10* torque units (mNm) / deg
    uint16_t endstop_damping = 0;   // in 0.1* torque units (mNm) * s /deg
    uint32_t endstop_position = 0;  // in inc
    int32_t endstop_offset = 0;     // in inc

    // endstop cannot be correctly defined if no stiffness and damping
    // values are defined (could lead to undesired behavior)
    if (options.has_value() and (options->endstop_specs.stiffness > 0 and
                                 options->endstop_specs.damping > 0.)) {

        options_.endstop_specs = options->endstop_specs;
        // defining the offset based on min and max joints limits
        // in order to keep consistency between basic and advanced
        // control modes
        options_.endstop_specs.offset =
            (options_.max_target_position + options_.min_target_position) / 2.;
        // then defining the endstop position as difference between max
        // and offset position
        options_.endstop_specs.position = std::abs(
            options_.max_target_position - options_.endstop_specs.offset);
        // damping and stiffness are given by user

        // now options are correctly set, convert to sensojoint units
        endstop_stiffness = static_cast<uint16_t>(
            std::round(options_.endstop_specs.stiffness *
                       conversion_->to_endstop_stiffness_unit_));
        endstop_damping = static_cast<uint16_t>(std::round(
            options_.endstop_specs.damping * conversion_->to_damping_unit_));
        endstop_position = static_cast<uint32_t>(std::round(
            options_.endstop_specs.position * conversion_->to_position_unit_));
        endstop_offset = static_cast<int32_t>(std::round(
            options_.endstop_specs.offset * conversion_->to_position_unit_));
    } else { // deactivate endstops
        options_.endstop_specs.stiffness = 0.;
        options_.endstop_specs.damping = 0.;
        options_.endstop_specs.position = 0.;
        options_.endstop_specs.offset = 0.;
    }
    std::cout << "WRITE endstop_stiffness, raw (10*): " << endstop_stiffness
              << " user: " << options_.endstop_specs.stiffness << std::endl;
    std::cout << "WRITE endstop_damping, raw: " << endstop_damping
              << " user: " << options_.endstop_specs.damping << std::endl;
    std::cout << "WRITE endstop_position, raw: " << endstop_position
              << " user: " << options_.endstop_specs.position << std::endl;
    std::cout << "WRITE endstop_offset, raw: " << endstop_offset
              << " user: " << options_.endstop_specs.offset << std::endl;

    // finally set the values into sensojoint memory
    const auto& [es_stiff_addr, es_stiff_sub, es_stiff_s] =
        dictionary_.object("endstop_stiffness");
    const auto& [es_damp_addr, es_damp_sub, es_damp_s] =
        dictionary_.object("endstop_damping");
    const auto& [es_pos_addr, es_pos_sub, es_pos_s] =
        dictionary_.object("endstop_position");
    const auto& [es_off_addr, es_off_sub, es_off_s] =
        dictionary_.object("endstop_offset");
    write_sdo(es_stiff_addr, es_stiff_sub, endstop_stiffness);
    write_sdo(es_damp_addr, es_damp_sub, endstop_damping);
    write_sdo(es_off_addr, es_off_sub, endstop_offset);
    write_sdo(es_pos_addr, es_pos_sub, endstop_position);
}

void SensoJoint::manage_control_period(const std::optional<Options>& options) {
    // memorize control period
    if (options.has_value()) {
        options_.control_period_ = options->control_period_;
    } else {
        options_.control_period_ = 0.001; // 1 ms
    }
    uint32_t cycle_time_ns = static_cast<uint32_t>(
        std::round(options_.control_period_ * 1000000000.0));
    // see
    // https://doc.synapticon.com/circulo/sw5.1/motion_control/advanced_control_options/distributed_clocks.html
    // for more explanations
    configure_dc_sync0(cycle_time_ns, 0);
}

void SensoJoint::manage_digital_ios(const std::optional<Options>& options) {
    // get digital outputs mask
    const auto& [dout_addr, dout_sub, dout_s] =
        dictionary_.object("digital_outputs_mask_set");
    uint32_t digital_outputs_mask{0};
    read_sdo(dout_addr, dout_sub, digital_outputs_mask);
    std::cout << "READ digital outputs mask: "
              << std::bitset<32>(digital_outputs_mask) << std::endl;

    // reset memorized options == no digital IO used by default
    options_.controlled_digital_outputs = 0;
    options_.read_digital_inputs = 0;
    if (options.has_value() and (options->controlled_digital_outputs != 0 or
                                 options->read_digital_inputs != 0)) {
        // get configuration of the GPIOs, see
        // https://doc.synapticon.com/circulo/sw5.1/objects_html/2xxx/2210.html?Highlight=0x2210
        uint8_t gpio_configuration{0};
        for (auto [index, number, pin] : gpio_index_number_pin) {
            read_sdo(gpio_configuration_addr, number, gpio_configuration);
            std::cout << "READ GPIO " << number << " config (pin " << pin
                      << "): "
                      << (gpio_configuration == gpio_disabled
                              ? "disabled"
                              : (gpio_configuration == gpio_config_in
                                     ? "IN"
                                     : (gpio_configuration == gpio_config_out
                                            ? "OUT"
                                            : "Other")))
                      << std::endl;
            // NOTE: create the mask for setting adequate bit in mask
            // corresponding bit index in mask is : number + 16 - 1 (IO number
            // start at 1)
            uint32_t mask_for_io = 1U << (number + 15);
            if ((options->controlled_digital_outputs & (1 << index)) != 0) {
                // check if the pin is configured as output
                if (gpio_configuration != gpio_config_out) {
                    // pin not configured as output => change its config
                    gpio_configuration = gpio_config_out;
                    write_sdo(gpio_configuration_addr, number,
                              gpio_configuration);
                }
                digital_outputs_mask |= mask_for_io;

            } else {
                digital_outputs_mask &= ~mask_for_io;
            }

            if ((options->read_digital_inputs & (1 << index)) != 0) {
                // check if the pin is configured as output
                if (gpio_configuration != gpio_config_in) {
                    // pin not configured as input => change its config
                    gpio_configuration = gpio_config_in;
                    write_sdo(gpio_configuration_addr, number,
                              gpio_configuration);
                }
            }
        }
        if (options->controlled_digital_outputs != 0) {
            // set the digital outputs mask to allow control of selected outputs
            write_sdo(dout_addr, dout_sub, digital_outputs_mask);
            std::cout << "WRITE digital outputs mask, raw: "
                      << std::bitset<32>(digital_outputs_mask) << std::endl;
            // then sets the initial value for each controlled output
            uint32_t digital_outputs_init_val{0};
            // set the initial value of the digital outputs
            for (auto [index, shift, pin] : gpio_index_number_pin) {
                if ((options->controlled_digital_outputs & (1U << index)) !=
                    0) {
                    // GPIO is controlled
                    uint32_t mask_for_io =
                        1U << (shift + 15); // to set the bit value
                    if ((options->digital_outputs_init_value & (1U << index)) !=
                        0) { // set to High
                        digital_outputs_init_val |= mask_for_io;
                    } else { // set to Low
                        digital_outputs_init_val &= ~mask_for_io;
                    }
                }
            }

            const auto& [dout_val__addr, dout_val_sub, dout_val_s] =
                dictionary_.object("digital_outputs_bit_set");
            write_sdo(dout_val__addr, dout_val_sub, digital_outputs_init_val);
        }
        options_.controlled_digital_outputs =
            options->controlled_digital_outputs;
        options_.digital_outputs_init_value =
            options->digital_outputs_init_value;
        options_.digital_outputs_end_value = options->digital_outputs_end_value;
        options_.read_digital_inputs = options->read_digital_inputs;
    }
}

void SensoJoint::manage_digital_ios_end() {
    if (options_.controlled_digital_outputs != 0) {
        // then sets the initial value for each controlled output
        uint32_t digital_outputs_end_val{0};
        // set the initial value of the digital outputs
        for (auto [index, shift, pin] : gpio_index_number_pin) {
            if ((options_.controlled_digital_outputs & (1U << index)) != 0) {
                // GPIO is controlled
                uint32_t mask_for_io = 1U
                                       << (shift + 15); // to set the bit value
                if ((options_.digital_outputs_end_value & (1U << index)) !=
                    0) { // set to High
                    digital_outputs_end_val |= mask_for_io;
                } else { // set to Low
                    digital_outputs_end_val &= ~mask_for_io;
                }
            }
        }
        const auto& [dout_val__addr, dout_val_sub, dout_val_s] =
            dictionary_.object("digital_outputs_bit_set");
        write_sdo(dout_val__addr, dout_val_sub, digital_outputs_end_val);
    }
}

} // namespace ethercatcpp