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

#include <cstring>
#include <ethercatcpp/sensojoint.h>
#include <pid/log.h>
#include <pid/hashed_string.h>
#include "buffers_definitions.h"
#include "coe_definitions.h"
#include <algorithm>

#include "private_types_definitions.h"

using namespace std::chrono_literals;
using namespace pid::literals;

namespace ethercatcpp {

using namespace sensojoint;

void SensoJoint::bind_buffers() {
    // bind the buffers
    rx_buffer_.bind_physical_buffer(*this);
    // bind variables
    auto& control_word =
        rx_buffer_.map_memory<uint16_t>(rx_mapping_1_, "controlword");
    auto& control_mode =<--- Shadow variable
        rx_buffer_.map_memory<int8_t>(rx_mapping_1_, "set_mode");
    auto& target_output_position =
        rx_buffer_.map_memory<int32_t>(rx_mapping_1_, "target_output_position");
    auto& target_output_velocity =
        rx_buffer_.map_memory<int32_t>(rx_mapping_1_, "target_output_velocity");
    auto& target_output_torque =
        rx_buffer_.map_memory<int32_t>(rx_mapping_1_, "target_output_torque");
    auto& target_position =
        rx_buffer_.map_memory<int32_t>(rx_mapping_1_, "target_position");
    auto& target_velocity =
        rx_buffer_.map_memory<int32_t>(rx_mapping_1_, "target_velocity");
    auto& target_motor_torque =
        rx_buffer_.map_memory<int16_t>(rx_mapping_1_, "target_motor_torque");
    auto& velocity_offset = rx_buffer_.map_memory<int32_t>(
        rx_mapping_1_, "velocity_feedforward_offset");
    auto& torque_offset = rx_buffer_.map_memory<int16_t>(
        rx_mapping_1_, "torque_feedforward_offset");
    auto& digital_outputs = rx_buffer_.map_memory<uint32_t>(
        rx_mapping_2_, "digital_outputs_bit_set");

    sta_cmd_ = std::make_unique<OpaqueStateCommands>(
        control_word, control_mode, target_output_position,
        target_output_velocity, target_output_torque, target_position,
        target_velocity, target_motor_torque, velocity_offset, torque_offset,
        digital_outputs);

    set_digital_outputs(
        options_.digital_outputs_init_value); // set correct init value

    if (options_.enable_haptics) {
        // create internal structure to hold haptics parameters value
        auto& stiffness =
            rx_buffer_.map_memory<uint16_t>(rx_mapping_3_, "spring_stiffness");
        auto& damping =
            rx_buffer_.map_memory<uint16_t>(rx_mapping_3_, "spring_damping");
        auto& neutal_position_ = rx_buffer_.map_memory<int32_t>(
            rx_mapping_3_, "spring_neutral_position");
        auto& external_load_torque_offset = rx_buffer_.map_memory<int32_t>(
            rx_mapping_3_, "external_load_torque_offset");
        sta_cmd_->haptics_config_.emplace(stiffness, damping, neutal_position_,
                                          external_load_torque_offset);
    }
    initialize_state_variables( // need to cast to byte pointer to avoid packed
                                // references problems
        reinterpret_cast<uint8_t*>(&sta_cmd_->control_word_),
        reinterpret_cast<uint8_t*>(&sta_cmd_->status_word_),
        reinterpret_cast<uint8_t*>(&sta_cmd_->control_mode_),
        reinterpret_cast<uint8_t*>(&sta_cmd_->operation_mode_));
}

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

    uint32_t device_id{0};
    switch (model_) {
    case MODEL_7016:
        device_id = sensojoint_model_7016_id;
        break;
    case MODEL_5016:
        device_id = sensojoint_model_5016_id;
        break;
    case MODEL_4010:
        device_id = sensojoint_model_4010_id;
        break;
    case MODEL_3010:
        device_id = sensojoint_model_3010_id;
        break;
    }

    set_id("SensoJoint", sensodrive_manufacturer_id, device_id);

    //  memorize haptics usage
    if (options.has_value()) {
        options_.enable_haptics = options->enable_haptics;
    } else {
        options_.enable_haptics = false;
    }

    configure_at_init([this, options]() {
        pid_log << pid::info << "SensoJoint: CANOpen configuration..."
                << pid::flush;

        pdo_maps_configuration();

        // Alway reset fault before start the init step
        reset_fault();
        read_base_units_values();
        manage_ctrl_params(options);
    });

    configure_at_end([this]() { manage_digital_ios_end(); });

    // Mailboxes configuration
    define_physical_buffer<mailbox_out_t>(
        ASYNCHROS_OUT, sensodrive_mbx_out_addr, sensodrive_mbx_out_flag);
    define_physical_buffer<mailbox_in_t>(ASYNCHROS_IN, sensodrive_mbx_in_addr,
                                         sensodrive_mbx_in_flag);

    // Communication buffer config. (RxPDO / TxPDO)
    if (options_.enable_haptics) {
        manage_haptics(); // add haptic mapping to RxPDO
    }
    rx_buffer_.define_physical_buffer(*this);

    tx_buffer_.define_physical_buffer(*this);

    // the device has DC synchronization
    define_distributed_clock(true);

    set_quick_stop_reaction([this] {
        // situation where the quick stop is launched => automatic procedure
        // except in torque control mode
        if (torque_mode(sta_cmd_->control_mode_)) {
            manage_torque_based_quick_stop();
        }
    });

    set_leaving_enable_requires_quick_stop([this]() -> bool {
        return std::abs(velocity()) >
               options_.velocity_threshold; // not standstill
    });

    set_entering_switchon_reaction([this] {
        set_target_position(position());
        set_target_velocity(0.);
        set_target_output_torque(output_torque());
    });

    set_entering_enable_reaction([this] {
        if (sta_cmd_->first_cycle_) {
            // first time check that initial position is consistent with current
            // position (robot has not moved yet)
            sta_cmd_->first_cycle_ = false;
            return std::fabs(initial_position() - position()) < 0.001;
        }
        return true;
    });

    //----------------------------------------------------------------------------//
    //                     I N I T S     S T E P S //
    //----------------------------------------------------------------------------//

    // add init step to :
    // -> status => update all values (status_word, positions, velocity,
    // torque,
    // ...)

    add_init_step(
        [this]() {
            pid_log << pid::info << "SensoJoint: initialization" << pid::flush;
            bind_buffers();
            device_state_initialization();
        },
        [this]() {
            unpack_status_buffer();
            update_internal_state();
            pid_log << pid::info << "SensoJoint: initialization done"
                    << pid::flush;
        });

    //----------------------------------------------------------------------------//
    //                     R U N S     S T E P S //
    //----------------------------------------------------------------------------//

    add_run_step([]() {},
                 [this]() {
                     unpack_status_buffer();
                     update_internal_state();
                 });

    //----------------------------------------------------------------------------//
    //                     E N D S     S T E P S //
    //----------------------------------------------------------------------------//
    // Add ending step to force fsm to "Switch on disable" and stop
    // power stage.
    add_end_step(
        [this]() {
            pid_log << pid::info << "SensoJoint: ending" << pid::flush;
            set_target_output_torque(output_torque());
            set_target_position(position());
            set_target_velocity(0.);
            device_state_termination();
        },
        [this]() {
            unpack_status_buffer();
            pid_log << pid::info << "SensoJoint: end" << pid::flush;
        });
}

SensoJoint::SensoJoint(const Model& model, const Options& options)
    : coe::cia402::CIA402Device(),
      model_{model},
      internal_parameters_{std::make_unique<InternalParameters>()},
      conversion_{std::make_unique<ConversionRatios>()} {
    initialize(options);
}

SensoJoint::SensoJoint(const Model& model)
    : coe::cia402::CIA402Device(),
      model_{model},
      internal_parameters_{std::make_unique<InternalParameters>()},
      conversion_{std::make_unique<ConversionRatios>()} {
    initialize({});
}

SensoJoint::~SensoJoint() = default;

bool SensoJoint::brake_applied() const {
    switch (operation_state()) {
    case state_t::operation_enabled:
    case state_t::quick_stop_active:
    case state_t::fault_reaction_active:
        return false;
    default:
        return true;
    }
}

bool SensoJoint::sto_active() const {
    return sta_cmd_->sto_active_;
}
bool SensoJoint::sbc_active() const {
    return sta_cmd_->sbc_active_;
}
const SensoJoint::Options& SensoJoint::options() const {
    return options_;
}

const std::string& SensoJoint::fault() const {
    // need to remove trailing spaces for the hash to work
    sta_cmd_->error_report_.erase(
        std::remove_if(sta_cmd_->error_report_.begin(),
                       sta_cmd_->error_report_.end(),
                       [](unsigned char x) { return std::isspace(x); }),
        sta_cmd_->error_report_.end());

    switch (pid::hashed_string(sta_cmd_->error_report_)) {
    case "PuOcA"_hs:
        sta_cmd_->fault_description_ =
            "[User defined params Protection] A current "
            "was detected in phase A that "
            "exceeds the current limit set by the user.";
        break;
    case "PuOcB"_hs:
        sta_cmd_->fault_description_ =
            "[User Params Protection] A current "
            "was detected in phase B that "
            "exceeds the current limit set by the user.";
        break;
    case "PuOcC"_hs:
        sta_cmd_->fault_description_ =
            "[User Params Protection] A current "
            "was detected in phase C that "
            "exceeds the current limit set by the user.";
        break;
    case "PuUv"_hs:
        sta_cmd_->fault_description_ = "[User Params Protection] The "
                                       "measured voltage fell below the "
                                       "under-voltage limit set by the user.";
        break;
    case "PuOv"_hs:
        sta_cmd_->fault_description_ = "[User Params Protection] The "
                                       "measured voltage exceeds the "
                                       "over-voltage limit set by the user.";
        break;
    case "PosLmAct"_hs:
        sta_cmd_->fault_description_ = "[User Params Protection] The "
                                       "positive limit switch is active.";
        break;
    case "PhUdef"_hs:
        sta_cmd_->fault_description_ =
            "[Hardware Protection] Undefined fault in CIA "
            "402 standard.";
        break;
    case "PhFault"_hs:
        sta_cmd_->fault_description_ = "[Hardware Protection] generic fault.";
        break;
    case "PhWtdg"_hs:
        sta_cmd_->fault_description_ =
            "[Hardware Protection] Watchdog ticks fault.";
        break;
    case "PhDeadTA"_hs:
        sta_cmd_->fault_description_ =
            "[Hardware Protection] Deadtime fault in phase A.";
        break;
    case "PhDeadTB"_hs:
        sta_cmd_->fault_description_ =
            "[Hardware Protection] Deadtime fault in phase B.";
        break;
    case "PhDeadTC"_hs:
        sta_cmd_->fault_description_ =
            "[Hardware Protection] Deadtime fault in phase C.";
        break;
    case "PhDeadTD"_hs:
        sta_cmd_->fault_description_ =
            "[Hardware Protection] Deadtime fault in phase D.";
        break;
    case "PhOvUvOt"_hs:
        sta_cmd_->fault_description_ = "[Hardware Protection] Either "
                                       "over-voltage, under-voltage or "
                                       "over-temperature was triggered.";
        break;
    case "PhOc"_hs:
        sta_cmd_->fault_description_ =
            "[Hardware Protection] over-current was triggered.";
        break;
    case "SfeDiIvd"_hs:
        sta_cmd_->fault_description_ =
            "[Safety Fault] Invalid digital inputs on "
            "the safety board.";
        break;
    case "SfeFault"_hs:
        sta_cmd_->fault_description_ =
            "[Safety Fault] Internal fault on safety board.";
        break;
    case "BisErBit"_hs:
        sta_cmd_->fault_description_ =
            "[BiSS Sensor Fault] BiSS protocol error bit received.";
        break;
    case "BisWnBit"_hs:
        sta_cmd_->fault_description_ = "[BiSS Sensor Fault] BiSS protocol "
                                       "warning bit received.";
        break;
    case "BisAcBit"_hs:
        sta_cmd_->fault_description_ =
            "[BiSS Sensor Fault] Acknowledge bit not "
            "received. The slave "
            "does not respond correctly.";
        break;
    case "BisSrtBt"_hs:
        sta_cmd_->fault_description_ =
            "[BiSS Sensor Fault] Start bit not received.";
        break;

    case "BisSloLo"_hs:
        sta_cmd_->fault_description_ = "[BiSS Sensor Fault] SLO line is low "
                                       "(Slave not ready).";
        break;
    case "BisFrame"_hs:
        sta_cmd_->fault_description_ =
            "[BiSS Sensor Fault] BiSS frame configuration issue.";
        break;
    case "BisCrc"_hs:
        sta_cmd_->fault_description_ =
            "[BiSS Sensor Fault] Wrong CRC received.";
        break;
    case "BisRegEr"_hs:
        sta_cmd_->fault_description_ = "[BiSS Sensor Fault] BiSS register "
                                       "communication error.";
        break;
    case "SnsrAngl"_hs:
        sta_cmd_->fault_description_ =
            "[Sensor Fault] Wrong commutation angle.";
        break;
    case "SnsrCfg"_hs:
        sta_cmd_->fault_description_ =
            "[Sensor Fault] Sensor configuration issue.";
        break;
    case "HwRsrcEr"_hs:
        sta_cmd_->fault_description_ =
            "[Sensor Fault] Hardware sensor resource error.";
        break;
    case "SwLimOut"_hs:
        sta_cmd_->fault_description_ = "[Sensor Fault] Invalid software limit.";
        break;
    case "BrkNtRls"_hs:
        sta_cmd_->fault_description_ = "[Brake Fault] Brake release failed.";
        break;
    case "BkHiPull"_hs:
        sta_cmd_->fault_description_ =
            "[Brake Fault] Pull brake voltage higher than "
            "measured DC bus voltage.";
        break;
    case "BkHiHold"_hs:
        sta_cmd_->fault_description_ =
            "[Brake Fault] Hold brake voltage higher than "
            "measured DC bus voltage.";
        break;
    case "OpnTermA"_hs:
        sta_cmd_->fault_description_ =
            "[Open Terminal Fault] Drive terminal A opened.";
        break;
    case "OpnTermB"_hs:
        sta_cmd_->fault_description_ =
            "[Open Terminal Fault] Drive terminal B opened.";
        break;
    case "OpnTermC"_hs:
        sta_cmd_->fault_description_ =
            "[Open Terminal Fault] Drive terminal C opened.";
        break;
    case "OpnFetAH"_hs:
        sta_cmd_->fault_description_ =
            "[Open FET Fault] Drive FET  (Field-Effect "
            "Transistor) A (high side) opened.";
        break;
    case "OpnFetBH"_hs:
        sta_cmd_->fault_description_ =
            "[Open FET Fault] Drive FET  (Field-Effect "
            "Transistor) B (high side) opened.";
        break;
    case "OpnFetCH"_hs:
        sta_cmd_->fault_description_ =
            "[Open FET Fault] Drive FET  (Field-Effect "
            "Transistor) C (high side) opened.";
        break;
    case "OpnFetAL"_hs:
        sta_cmd_->fault_description_ =
            "[Open FET Fault] Drive FET  (Field-Effect "
            "Transistor) A (low side) opened.";
        break;
    case "OpnFetBL"_hs:
        sta_cmd_->fault_description_ =
            "[Open FET Fault] Drive FET  (Field-Effect "
            "Transistor) B (low side) opened.";
        break;
    case "OpnFetCL"_hs:
        sta_cmd_->fault_description_ =
            "[Open FET Fault] Drive FET  (Field-Effect "
            "Transistor) C (low side) opened.";
        break;
    case "IvldOfst"_hs:
        sta_cmd_->fault_description_ =
            "[Parameter Fault] Commutation offset is not valid.";
        break;
    case "IvldOpmd"_hs:
        sta_cmd_->fault_description_ =
            "[Parameter Fault] Requested opmode is not supported.";
        break;
    case "ZeroMxI"_hs:
        sta_cmd_->fault_description_ =
            "[Parameter Fault] User limit on max current is 0.";
        break;
    case "ZeroMxT"_hs:
        sta_cmd_->fault_description_ =
            "[Parameter Fault] User limit on max torque is 0.";
        break;
    case "ZeroMxMS"_hs:
        sta_cmd_->fault_description_ =
            "[Parameter Fault] User limit on max motor speed is 0.";
        break;
    case "IvldPara"_hs:
        sta_cmd_->fault_description_ = "[Parameter Fault] Invalid parameter.";
        break;
    case "SiVelPfx"_hs:
        sta_cmd_->fault_description_ =
            "[Parameter Fault] Invalid velocity scaling factor.";
        break;
    case "SiVelTim"_hs:
        sta_cmd_->fault_description_ =
            "[Parameter Fault] Invalid SI velocity time factor.";
        break;
    case "IvldFeed"_hs:
        sta_cmd_->fault_description_ =
            "[Parameter Fault] Invalid Feed constant.";
        break;
    case "MxPwrLmt"_hs:
        sta_cmd_->fault_description_ =
            "[Parameter Fault] Maximum mechanical output "
            "power of the motor is reached.";
        break;
    case "FollwErr"_hs:
        sta_cmd_->fault_description_ =
            "[Parameter Fault] Following error: actual position is "
            "outside "
            "following error window.";
        break;
    case "MxTorLmt"_hs:
        sta_cmd_->fault_description_ =
            "[Parameter Fault] Maximum torque limit reached.";
        break;
    case "MxMSLmt"_hs:
        sta_cmd_->fault_description_ =
            "[Parameter Fault] Maximum motor speed limit reached.";
        break;
    case "SwPosLmt"_hs:
        sta_cmd_->fault_description_ =
            "[Parameter Fault] Software position limit reached.";
        break;
    case "PosRgLmt"_hs:
        sta_cmd_->fault_description_ =
            "[Parameter Fault] Encoder range limit reached.";
        break;
    case "IntgrLmt"_hs:
        sta_cmd_->fault_description_ =
            "[Parameter Fault] Integral limit of position/velocity "
            "basic "
            "control loop has been reached.";
        break;
    case "CyclicHb"_hs:
        sta_cmd_->fault_description_ = "[Communication Fault] A "
                                       "communication keep-alive heart-beat "
                                       "was dropped. Check that the PDO "
                                       "messages are being sent by "
                                       "the master cyclically, and within "
                                       "100 ms of each other.";
        break;
    case "OsCmdCol"_hs:
        sta_cmd_->fault_description_ =
            "[Communication Fault] OS command collision.";
        break;
    case "I2tActv"_hs:
        sta_cmd_->fault_description_ =
            "[I2t Protection] I2t protection is active.";
        break;
    case "VeFiFcLo"_hs:
        sta_cmd_->fault_description_ =
            "[Filter Bounds Fault] Velocity feedback "
            "filter cutoff frequency is "
            "lower than the minimum limit.";
        break;
    case "VeFiFcHi"_hs:
        sta_cmd_->fault_description_ =
            "[Filter Bounds Fault] Velocity feedback "
            "filter cutoff frequency is "
            "higher than the maximum limit.";
        break;
    case "PoFiFcLo"_hs:
        sta_cmd_->fault_description_ =
            "[Filter Bounds Fault] Position feedback "
            "filter cutoff frequency is "
            "lower than the minimum limit.";
        break;
    case "PoFiFcHi"_hs:
        sta_cmd_->fault_description_ =
            "[Filter Bounds Fault] Position feedback "
            "filter cutoff frequency is "
            "higher than the maximum limit.";
        break;
    case "VffFcLo"_hs:
        sta_cmd_->fault_description_ =
            "[Filter Bounds Fault] Velocity feed forward cutoff "
            "frequency is lower than the minimum limit.";
        break;
    case "VffFcHi"_hs:
        sta_cmd_->fault_description_ =
            "[Filter Bounds Fault] Velocity feed forward cutoff "
            "frequency is higher than the maximum limit.";
        break;
    case "NhFiFcHi"_hs:
        sta_cmd_->fault_description_ =
            "[Filter Bounds Fault] Notch filter Center "
            "frequency is higher than the maximum limit.";
        break;
    case "NhFiPara"_hs:
        sta_cmd_->fault_description_ =
            "[Filter Bounds Fault] Invalid configuration was "
            "detected for the notch filter.";
        break;
    case "SynDifHi"_hs:
        sta_cmd_->fault_description_ =
            "[Distributed Clock Fault] Sync time variance "
            "is too high (value is ignored).";
        break;
    case "TmpCore"_hs:
        sta_cmd_->fault_description_ = "[Temperature Sensor Fault] The "
                                       "temperature of the core "
                                       "becomes too high andcrosses the "
                                       "error threshold (> 100 °C).";
        break;
    case "TmpDrive"_hs:
        sta_cmd_->fault_description_ = "[Temperature Sensor Fault] The "
                                       "temperature of the drive "
                                       "becomes too high and crosses the "
                                       "error threshold(> 100 °C).";
        break;
    case "ExAnSnsr"_hs:
        sta_cmd_->fault_description_ =
            "[Temperature Sensor Fault] The temperature "
            "of the motor becomes too high and crosses "
            "the error threshold(> 120 °C or < -10).";
        break;
    default:
        sta_cmd_->fault_description_ = "[Unknown Fault]";
        break;
    }
    std::ostringstream oss;
    oss << std::hex << sta_cmd_->error_code_;
    sta_cmd_->fault_description_ +=
        "(report: " + sta_cmd_->error_report_ + ", code: " + oss.str() + ")";
    return sta_cmd_->fault_description_;
}

uint16_t SensoJoint::error_code() const {
    return sta_cmd_->error_code_;
}

bool SensoJoint::warning() const {
    return sta_cmd_->status_word_ & flag_warning_emitted;
}

void SensoJoint::pdo_maps_configuration() {

    // automatically configure mappings and buffers for RX PDO
    if (not rx_buffer_.configure(*this)) {
        pid_log << pid::critical
                << "Cannot configure sensojoint buffers and mappings. The "
                   "SensoJoint is probably in an internal error state. Please "
                   "shutdown and reboot the system."
                << pid::flush;
        throw std::runtime_error(
            "SensoJoint buffer corrupted. Reboot the system");
    }
    // NOTE: we keep the original TX mappings as they give all the necessary
    // info we need
}

void SensoJoint::unpack_status_buffer() {

    auto [base, torque, safety] =
        input_buffer<buffer_in_status_base_t, buffer_in_status_torque_t,
                     buffer_in_status_safety_t>(sensodrive_pdo_in_addr);
    sta_cmd_->status_word_ = base->status_word;
    sta_cmd_->operation_mode_ = base->operation_modes;
    sta_cmd_->error_code_ = base->error_code;

    sta_cmd_->error_report_.clear();
    sta_cmd_->error_report_ = std::string_view(base->error_report, 8);
    sta_cmd_->position_ =
        base->current_position * conversion_->from_position_unit_;
    sta_cmd_->velocity_ =
        base->current_velocity * conversion_->from_velocity_unit_;
    sta_cmd_->output_torque_ =
        base->current_output_torque * conversion_->from_output_torque_unit_;
    sta_cmd_->motor_torque_ =
        torque->current_motor_torque * conversion_->from_rated_torque_unit_;
    sta_cmd_->friction_torque_ =
        torque->current_friction_torque * conversion_->from_output_torque_unit_;

    sta_cmd_->motor_temperature_ = safety->current_motor_temperature;
    sta_cmd_->drive_temperature_ = safety->current_drive_temperature *
                                   conversion_->from_drive_temperatue_unit_;
    sta_cmd_->sensor_temperature_ =
        safety->current_sensor_temperature *
        conversion_->from_torque_sensor_temperature_unit_;

    sta_cmd_->sto_active_ = (safety->sto & 0x01) == 1;
    sta_cmd_->sbc_active_ = (safety->sbc & 0x01) == 1;

    // std::cout << "status: " << sta_cmd_->status_word_ << std::endl;
}

void SensoJoint::update_internal_state() {
    // update setpoint management
    managed_set_point();
}

//****************************************************************************//
//              E N D   U S E R    F U N C T I O N
//****************************************************************************//

void SensoJoint::force_target_output_torque(double target_torque) {
    sta_cmd_->target_output_torque_ = static_cast<int32_t>(
        std::round(target_torque * conversion_->to_output_torque_unit_));
    // automatic conversion to motor torque using the gear ratio
    force_target_motor_torque(target_torque / internal_parameters_->gear_ratio);
}

void SensoJoint::set_target_output_torque(double target_torque) {
    auto state = operation_state();
    if (state == state_t::quick_stop_active or
        state == state_t::fault_reaction_active) {
        return;
    }
    force_target_output_torque(target_torque);
}

void SensoJoint::force_target_motor_torque(double target_torque) {
    sta_cmd_->target_motor_torque_ = static_cast<int16_t>(
        std::round(target_torque * conversion_->to_rated_torque_unit_));
    // target_motor_torque_ in rated torque/1000 and rated_torque is in
    // mNm
}

void SensoJoint::set_target_motor_torque(double target_torque) {
    auto state = operation_state();
    if (state == state_t::quick_stop_active or
        state == state_t::fault_reaction_active) {
        return;
    }
    force_target_motor_torque(target_torque);
}

void SensoJoint::set_motor_torque_feedforward(double torque_offset) {
    sta_cmd_->torque_offset_ = static_cast<int16_t>(
        std::round(torque_offset * conversion_->to_rated_torque_unit_));
    // target_motor_torque_ in rated torque/1000 and rated_torque is in
    // uNm
}

void SensoJoint::set_output_torque_feedforward(double torque_offset) {
    set_motor_torque_feedforward(torque_offset /
                                 internal_parameters_->gear_ratio);
}

void SensoJoint::set_spring(double neutral_position, double stiffness,
                            double damping) {
    if (not options_.enable_haptics) {
        return;
    }
    // conversion
    sta_cmd_->haptics_config_->stiffness_ = static_cast<uint16_t>(
        std::round(stiffness * conversion_->to_spring_stiffness_unit_));
    sta_cmd_->haptics_config_->damping_ = static_cast<uint16_t>(
        std::round(damping * conversion_->to_damping_unit_));
    sta_cmd_->haptics_config_->neutral_position_ = static_cast<uint16_t>(
        std::round(neutral_position * conversion_->to_position_unit_));
}

void SensoJoint::set_digital_outputs(uint8_t outputs) {
    if (options_.controlled_digital_outputs == 0) {
        return;
    }
    for (auto [index, number, pin] : gpio_index_number_pin) {
        if ((options_.controlled_digital_outputs & (1U << index)) != 0) {
            // GPIO is controlled
            uint32_t mask_for_io = 1U << (number + 15); // to set the bit value
            if ((outputs & (1U << index)) != 0) {       // set to High
                sta_cmd_->digital_outputs_ |= mask_for_io;
            } else { // set to Low
                sta_cmd_->digital_outputs_ &= ~mask_for_io;
            }
        }
    }
}

uint8_t SensoJoint::digital_inputs() const {
    if (options_.read_digital_inputs == 0) {
        return 0; // nothing read
    }
    uint8_t inputs{0};
    for (auto [index, number, pin] : gpio_index_number_pin) {
        if ((options_.read_digital_inputs & (1U << index)) != 0) {
            // GPIO is read
            uint32_t mask_for_io = 1U << (number + 15); // to read the bit value
            if ((sta_cmd_->digital_inputs_ & mask_for_io) != 0) { // bit is high
                inputs |= 1U << index;
            } // otherwise, bit is low (let it at 0)
        }
    }
    return inputs;
}

void SensoJoint::set_external_load_torque_offset(double offset) {
    if (not options_.enable_haptics) {
        return;
    }
    sta_cmd_->haptics_config_->external_load_torque_offset_ =
        static_cast<int32_t>(
            std::round(offset * conversion_->to_output_torque_unit_));
}

void SensoJoint::set_target_position(double target_position) {
    sta_cmd_->target_position_ = static_cast<int32_t>(
        std::round(target_position * conversion_->to_position_unit_));
    sta_cmd_->target_output_position_ = sta_cmd_->target_position_;
}

void SensoJoint::set_target_velocity(double target_velocity) {
    sta_cmd_->target_velocity_ = static_cast<int32_t>(
        std::round(target_velocity * conversion_->to_velocity_unit_));
    sta_cmd_->target_output_velocity_ = sta_cmd_->target_velocity_;
}

void SensoJoint::set_velocity_feedforward(double velocity_offset) {
    sta_cmd_->velocity_offset_ = static_cast<int32_t>(
        std::round(velocity_offset * conversion_->to_velocity_unit_));
}

bool SensoJoint::can_swap_mode(control_mode_t current, control_mode_t next) {
    // modes cannot be swapped between advanced and basic control modes
    return (is_advanced_mode(current) and is_advanced_mode(next)) or
           (not is_advanced_mode(current) and not is_advanced_mode(next));
}

bool SensoJoint::set_control_mode(control_mode_t control_mode) {
    auto code = control_mode_to_code(control_mode);
    if (sta_cmd_->control_mode_ != code) {
        if (control_mode == control_mode_t::monitor) {
            if (operation_state() == state_t::operation_enabled) {
                // setting monitor mode from operation enabled is forbidden !!!
                // because this is an unknown behavior
                return false;
            }
        }
        if (operation_state() != state_t::switch_on_disabled) {
            // in switched on disabled every change is possible
            if (control_mode != control_mode_t::monitor) {
                // target is different than monitor
                auto curr = control_code_to_mode(sta_cmd_->control_mode_);
                if (not can_swap_mode(curr, control_mode)) {
                    return false;
                }
            }
        }

        sta_cmd_->control_mode_ = code;
        return true;
    }
    return false;
}

SensoJoint::control_mode_t SensoJoint::control_mode() const {
    return control_code_to_mode(current_control_mode());
}

std::string_view SensoJoint::control_mode_str() const {
    return control_mode_to_str(control_mode());
}

std::string_view SensoJoint::control_mode_to_str(control_mode_t mode) {
    auto code = control_mode_to_code(mode);
    auto found = control_mode_decode_.find(code);
    if (found != control_mode_decode_.end()) {
        return found->second;
    }
    return CIA402Device::control_mode_to_str(code);
}

double SensoJoint::position() const {
    return sta_cmd_->position_;
}

double SensoJoint::initial_position() const {
    return options_.initial_adjusted_position + options_.zero_pos_offset;
}

double SensoJoint::velocity() const {
    return sta_cmd_->velocity_;
}

double SensoJoint::motor_torque() const {
    return sta_cmd_->motor_torque_;
}

double SensoJoint::motor_current() const {
    return sta_cmd_->motor_torque_ / internal_parameters_->torque_constant;
}

double SensoJoint::output_torque() const {
    return sta_cmd_->output_torque_;
}

double SensoJoint::friction_torque() const {
    return sta_cmd_->friction_torque_;
}
double SensoJoint::motor_temperature() const {
    return static_cast<double>(sta_cmd_->motor_temperature_);
}

double SensoJoint::drive_temperature() const {
    return sta_cmd_->drive_temperature_;
}

double SensoJoint::sensor_temperature() const {
    return sta_cmd_->sensor_temperature_;
}

void SensoJoint::manage_torque_based_quick_stop() {
    // NOTE: goal is to try to decelerate as fast as possible while avoiding
    // oscillations at the end
    // 1) reset all commands (e.g. offsets) that could generate torques
    set_output_torque_feedforward(0.);
    set_external_load_torque_offset(0.);
    set_spring(0., 0., 0.);
    // 2) compute the output torque command to control deceleration
    auto required_decel = std::abs(velocity() / options_.control_period_);
    double gain = 1.0;
    if (required_decel < internal_parameters_->quick_stop_deceleration) {
        gain = required_decel / internal_parameters_->quick_stop_deceleration;
    }
    // NOTE: we consider that max output torque is proportional to quick stop
    // acceleration (which is true by construction)
    // target torque is limited once device is close to 0 velocity,
    // proportionaly to the velocity "distance" to 0
    // this is just to avoid instability before stopping
    if (velocity() > 0.) {
        force_target_output_torque(
            -gain * internal_parameters_->absolute_max_output_torque);
    } else {
        force_target_output_torque(
            gain * internal_parameters_->absolute_max_output_torque);
    }
}

//****************************************************************************//
//              PROFILES   M O D E   F U N C T I O N
//****************************************************************************//

bool SensoJoint::following_error() const {
    if (profile_mode(sta_cmd_->operation_mode_)) {
        if (not motion_halted()) {
            return (sta_cmd_->status_word_ & flag_following_error);
        }
    } else {
        // NOTE: the user may have ask to change mode but mode is not
        // already changed -> target not reached BUT no error
        if (not profile_mode(sta_cmd_->control_mode_)) {
            // generate an error message only if user made a mistake
            pid_log << pid::warning
                    << "SenbsoJoint : invalid command "
                       "\"following_error()\" is "
                       "only used in profile modes "
                    << pid::flush;
        }
    }
    return false;
}

//****************************************************************************//
//              P V M   M O D E   F U N C T I O N S
//****************************************************************************//

bool SensoJoint::standstill() const {
    if (profile_mode(sta_cmd_->operation_mode_) and
        velocity_mode(sta_cmd_->operation_mode_)) {
        return (sta_cmd_->status_word_ & flag_standstill_motion);
    } else {
        // NOTE: the user may have ask to change mode but mode is not
        // already changed -> target not reached BUT no error
        if (not profile_mode(sta_cmd_->control_mode_) or
            not velocity_mode(sta_cmd_->control_mode_)) {
            // generate an error message only if user made a mistake
            pid_log << pid::warning
                    << "SenbsoJoint : invalid command \"standstill()\" is "
                       "only used in profile velocity modes "
                    << pid::flush;
        }
    }
    return false;
}

} // namespace ethercatcpp