Cppcheck report - ethercatcpp-core: /builds/ethercatcpp/ethercatcpp-core/src/ethercat_master/cia402_device.cpp

/*      File: cia402_device.cpp
 *       This file is part of the program ethercatcpp-core
 *       Program description : EtherCAT driver libraries for UNIX
 *       Copyright (C) 2017-2024 -  Robin Passama (LIRMM / CNRS) Arnaud Meline
 * (LIRMM / CNRS) Benjamin Navarro (LIRMM / CNRS). All Right reserved.
 *
 *       This software is free software: you can redistribute it and/or modify
 *       it under the terms of the CeCILL-C license as published by
 *       the CEA CNRS INRIA, either version 1
 *       of the License, or (at your option) any later version.
 *       This software is distributed in the hope that it will be useful,
 *       but WITHOUT ANY WARRANTY without even the implied warranty of
 *       MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 *       CeCILL-C License for more details.
 *
 *       You should have received a copy of the CeCILL-C License
 *       along with this software. If not, it can be found on the official
 * website of the CeCILL licenses family (http://www.cecill.info/index.en.html).
 */
#include <bitset>
#include <ethercatcpp/cia402_device.h>
#include "cia402_definitions.h"

#include <pid/log/ethercatcpp-core_ethercatcpp-core.h>
namespace ethercatcpp::coe::cia402 {

///////////////////// Object dictionary /////////////////////

CIA402ObjectDictionary::CIA402ObjectDictionary() : ObjectDictionary() {
    add_entries(
        {{"controlword", {CIA402Device::controlword_addr, 0x00, 0x10}},
         {"set_mode", {CIA402Device::set_modes_of_operation_addr, 0x00, 0x08}},
         {"statusword", {CIA402Device::statusword_addr, 0x00, 0x10}},
         {"read_mode",
          {CIA402Device::get_modes_of_operation_addr, 0x00, 0x08}}});
}

CIA402ObjectDictionary::CIA402ObjectDictionary(
    std::initializer_list<std::pair<std::string_view, entry>> entries)
    : CIA402ObjectDictionary() {
    add_entries(std::move(entries));
}

///////////////////// State Machine /////////////////////

const std::map<CIA402Device::state_t, std::string_view>
    CIA402Device::device_state_decode_ = {
        {unknown_state, "Invalid state"},
        {not_ready_to_switch_on, "Not ready to switch on"},
        {switch_on_disabled, "Switch on disabled"},
        {ready_to_switch_on, "Ready to switch ON"},
        {switched_on, "Switched ON"},
        {operation_enabled, "Operation enabled"},
        {quick_stop_active, "Quick stop active"},
        {fault_reaction_active, "Fault reaction active"},
        {faulty, "Fault"}};

// flags to set/unset bits in controlword

void CIA402Device::set_control_word(uint16_t& controlword, control_t ctrl) {
    switch (ctrl) {
    case control_t::shutdown:
        controlword |= flag_shutdown_set;
        controlword &= flag_shutdown_unset;
        break;
    case control_t::switch_on:
        controlword |= flag_switch_on_set;
        controlword &= flag_unset_reset_fault;
        break;
    case control_t::switch_on_and_enable_op:
        controlword |= (flag_switch_on_set | flag_enable_op_set);
        controlword &= (flag_unset_reset_fault & flag_enable_op_unset);
        break;
    case control_t::disable_voltage:
        controlword &= flag_disable_voltage_unset;
        break;
    case control_t::quickstop:
        controlword |= flag_quickstop_set;
        controlword &= flag_quickstop_unset;
        break;
    case control_t::disable_op:
        controlword |= flag_disable_op_set;
        controlword &= flag_disable_op_unset;
        break;
    case control_t::enable_op:
        controlword |= flag_enable_op_set;
        controlword &= flag_enable_op_unset;
        break;
    case control_t::fault_reset_op:
        controlword |= flag_set_reset_fault;
        break;
    };
}

CIA402Device::state_t CIA402Device::operation_state(uint16_t controlword) {<--- Function 'operation_state' argument 1 names different: declaration 'statusword' definition 'controlword'.
    // std::cout << "status word: " << word_bits(status_word_) << std::endl;
    switch (controlword & mask_state_device_status) {
    case CIA402Device::not_ready_to_switch_on_code:
        return state_t::not_ready_to_switch_on;
    case CIA402Device::switch_on_disabled_code:
    case (CIA402Device::switch_on_disabled_code | mask_quickstop_bit_status):
        return state_t::switch_on_disabled;
    case CIA402Device::ready_to_switch_on_code:
        return state_t::ready_to_switch_on;
    case CIA402Device::switched_on_code:
        return state_t::switched_on;
    case CIA402Device::operation_enabled_code:
        return state_t::operation_enabled;
    case CIA402Device::quick_stop_active_code:
        return state_t::quick_stop_active;
    case CIA402Device::fault_reaction_active_code:
    case CIA402Device::fault_reaction_active_code | mask_quickstop_bit_status:
        return state_t::fault_reaction_active;
    case CIA402Device::fault_code:
    case CIA402Device::fault_code | mask_quickstop_bit_status:
        return state_t::faulty;
    default:
        return state_t::unknown_state;
    }
}

///////////////////// Device /////////////////////

const std::map<int8_t, std::string_view> CIA402Device::std_control_mode_str =
    std::map<int8_t, std::string_view>{{{monitor_mode, "monitor"},
                                        {PP_mode, "profile_position"},
                                        {PV_mode, "profile_velocity"},
                                        {PT_mode, "profile_torque"},
                                        {HOMING_mode, "homing"},
                                        {IP_mode, "interpolated_position"},
                                        {CSP_mode, "cyclic_position"},
                                        {CSV_mode, "cyclic_velocity"},
                                        {CST_mode, "cyclic_torque"}}};

CIA402Device::CIA402Device()
    : SlaveDevice(),
      active_{true},
      paused_{false},
      go_back_to_monitor_mode_{false},
      setpoint_management_{READY_TO_ACCEPT},
      set_point_defined_{false},
      control_word_{nullptr},
      status_word_{nullptr},
      mode_set_{nullptr},
      mode_read_{nullptr} {
}

CIA402Device::CIA402Device(uint16_t& control_word, uint16_t& status_word,
                           int8_t& mode_set, int8_t& mode_read)
    : SlaveDevice(),
      active_{true},
      paused_{false},
      go_back_to_monitor_mode_{false},
      setpoint_management_{READY_TO_ACCEPT},
      set_point_defined_{false},
      control_word_{&control_word},
      status_word_{&status_word},
      mode_set_{&mode_set},
      mode_read_{&mode_read} {
}

CIA402Device::~CIA402Device() = default;

bool CIA402Device::is_configurable() const {
    return operation_state() == state_t::switch_on_disabled;
}

CIA402Device::state_t CIA402Device::operation_state() const {
    return CIA402Device::operation_state(*status_word_);
}

std::string_view CIA402Device::operation_state_str() const {
    return operation_state_to_str(operation_state());
}

std::string_view CIA402Device::operation_state_to_str(state_t state) {
    return device_state_decode_.at(state);
}

void CIA402Device::set_operation_state(control_t state) {
    CIA402Device::set_control_word(*control_word_, state);
}

void CIA402Device::acknowledge_fault() {
    if (operation_state() == state_t::faulty) {
        // the sensojoint is currently in a faulty state and waits for
        // aknowledge to start again
        set_operation_state(control_t::fault_reset_op);
        // ask to go back to monitor mode
        go_back_to_monitor_mode_ = true;
    }
}

void CIA402Device::quick_stop() {
    if (operation_state() == state_t::operation_enabled) {
        set_operation_state(control_t::quickstop);
        // ask to go back to monitor mode
        go_back_to_monitor_mode_ = true;
    }
}

bool CIA402Device::voltage_enabled() const {
    return *status_word_ & mask_voltage_enabled_bit_status;
}

bool CIA402Device::fault_alert() const {
    return *status_word_ & fault_code;
}

bool CIA402Device::warning_alert() const {
    return *status_word_ & mask_warning_bit_status;
}

void CIA402Device::set_quick_stop_reaction(std::function<void()> reaction) {
    quick_stop_reaction_ = reaction;
}

void CIA402Device::set_leaving_enable_requires_quick_stop(
    std::function<bool()> reaction) {
    leaving_enabled_requires_quick_stop_ = reaction;
}

void CIA402Device::set_entering_enable_reaction(
    std::function<bool()> reaction) {
    entering_enable_reaction_ = reaction;
}
void CIA402Device::set_entering_switchon_reaction(
    std::function<void()> reaction) {
    entering_switch_on_reaction_ = reaction;
}
void CIA402Device::set_entering_disabled_reaction(
    std::function<void()> reaction) {
    entering_switch_on_disabled_reaction_ = reaction;
}

// Change state of Sensojoint FSM to power on/off motors and activate brakes
bool CIA402Device::process_state() {
    switch (operation_state()) {
    case state_t::not_ready_to_switch_on: // only first time drive is powered on
        if (entering_switch_on_disabled_reaction_) {
            entering_switch_on_disabled_reaction_();
        }
        break;
    case state_t::switch_on_disabled:
        if (go_back_to_monitor_mode_) {
            *mode_set_ = monitor_mode;
            go_back_to_monitor_mode_ = false;
            // at this stage the drive restart after a quick stop or fault
            // reaction
            if (entering_switch_on_disabled_reaction_) {
                entering_switch_on_disabled_reaction_();
            }
        }
        if (active_) {
            if (paused_) {
                // stay in switch_on_disabled state
                set_operation_state(control_t::disable_voltage);
            } else {
                set_operation_state(control_t::shutdown);
            }
        } else {
            return true; // final state reached
        }
        break;
    case state_t::ready_to_switch_on:
        if (not paused_) {
            if (active_) {
                if (*mode_set_ == monitor_mode) { // target mode is monitor mode
                    // simply go to SWITCHED_ON state
                    set_operation_state(control_t::switch_on);
                    if (entering_switch_on_reaction_) {
                        entering_switch_on_reaction_();
                    }
                } else if (current_control_mode() == *mode_set_) {
                    // a target control mode is defined and current mode matches
                    // target -> directly go to ENABLED state
                    set_operation_state(control_t::switch_on_and_enable_op);
                    if (entering_switch_on_reaction_) {
                        entering_switch_on_reaction_();
                    }
                }
            } else {
                set_operation_state(control_t::disable_voltage);
                if (entering_switch_on_disabled_reaction_) {
                    entering_switch_on_disabled_reaction_();
                }
            }
        } else {
            set_operation_state(control_t::shutdown);
        }
        break;
    case state_t::switched_on:
        if (not paused_) {
            if (active_) {
                if (*mode_set_ != monitor_mode) {
                    // a target control mode is defined
                    if (current_control_mode() == *mode_set_) {
                        // current mode is target -> go to ENABLED state
                        set_operation_state(control_t::enable_op);
                        if (entering_enable_reaction_) {
                            if (not entering_enable_reaction_()) {
                                set_operation_state(control_t::switch_on);
                            }
                        }
                    } // otherwise wait for current mode change
                } else {
                    return true; // final state reached
                }
            } else {
                set_operation_state(control_t::disable_voltage);
                if (entering_switch_on_disabled_reaction_) {
                    entering_switch_on_disabled_reaction_();
                }
            }
        } else {
            set_operation_state(control_t::switch_on);
        }
        break;
    case state_t::quick_stop_active:
    case state_t::fault_reaction_active:
        if (active_) {
            // situation where the quick stop is launched => automatic procedure
            // except in torque control mode
            if (quick_stop_reaction_) {
                quick_stop_reaction_();
            }
        }
        // else (not active) transition to switch_on_disabled is automatic
        break;
    case state_t::operation_enabled:
        if (not active_) { // leaving enabled state
            if (leaving_enabled_requires_quick_stop_) {
                if (leaving_enabled_requires_quick_stop_()) {
                    quick_stop();
                } else { // no quick stop required
                    set_operation_state(control_t::disable_voltage);
                }
            } else {
                quick_stop();
            }
        } else {
            return true; // final state reached
        }
        break;
    case state_t::faulty:
        if (not active_) {
            // automatic acknowledge when finishing
            set_operation_state(control_t::fault_reset_op);
        }
        // else (active) nothing can be done automatically, waiting for
        // acknowledge to start control again
        break;
    default:
        break;
    }
    return false;
}

int8_t CIA402Device::current_control_mode() const {
    return *mode_read_;
}

void CIA402Device::finish() {
    active_ = false;
}

void CIA402Device::startup() {
    active_ = true;
}

void CIA402Device::pause(bool doit) {
    paused_ = doit;
}

bool CIA402Device::paused() const {
    return paused_;
}

bool CIA402Device::active() const {
    return active_;
}

void CIA402Device::initialize_state_variables(uint16_t& control_word,
                                              uint16_t& status_word,
                                              int8_t& mode_set,
                                              int8_t& mode_read) {
    initialize_state_variables(&control_word, &status_word, &mode_set,
                               &mode_read);
}

void CIA402Device::initialize_state_variables(uint16_t* control_word,
                                              uint16_t* status_word,
                                              int8_t* mode_set,
                                              int8_t* mode_read) {
    control_word_ = control_word;
    status_word_ = status_word;
    mode_set_ = mode_set;
    mode_read_ = mode_read;
}

void CIA402Device::initialize_state_variables(uint8_t* control_word_addr,
                                              uint8_t* status_word_addr,
                                              uint8_t* mode_set_addr,
                                              uint8_t* mode_read_addr) {
    initialize_state_variables(reinterpret_cast<uint16_t*>(control_word_addr),
                               reinterpret_cast<uint16_t*>(status_word_addr),
                               reinterpret_cast<int8_t*>(mode_set_addr),
                               reinterpret_cast<int8_t*>(mode_read_addr));
}

void CIA402Device::reset_fault() {
    uint16_t value = 0;
    // NOTE: do not need control_word_ to be bound when called since it is
    // direct SDO based communication
    read_sdo(controlword_addr, 0x00, value); // read control_word
    value &= flag_unset_reset_fault; // mask for unset the "reset fault" bit
    write_sdo(controlword_addr, 0x00, value);
    read_sdo(controlword_addr, 0x00, value);
    value |= flag_set_reset_fault; // mask for set the "reset fault" bit
    write_sdo(controlword_addr, 0x00, value);
    read_sdo(controlword_addr, 0x00, value);
    value &= flag_unset_reset_fault; // mask for unset the "reset fault" bit
    write_sdo(controlword_addr, 0x00, value);
}

void CIA402Device::device_state_termination() {
    set_operation_state(control_t::disable_voltage);
    *mode_set_ = monitor_mode;
}

void CIA402Device::device_state_initialization() {
    *mode_set_ = monitor_mode;
    // unlock any profile mode motion
    halt_motion(false);
    // allow profile control
    reset_set_point();
    active_ = true;
}

bool CIA402Device::profile_mode(int8_t mode) {
    switch (mode) {
    case PP_mode:
    case PV_mode:
    case PT_mode:
    case IP_mode:
        return true;
    default:
        return false;
    }
}

bool CIA402Device::cyclic_mode(int8_t mode) {
    switch (mode) {
    case CSP_mode:
    case CSV_mode:
    case CST_mode:
        return true;
    default:
        return false;
    }
}

bool CIA402Device::position_mode(int8_t mode) {
    switch (mode) {
    case PP_mode:
    case IP_mode:
    case CSP_mode:
        return true;
    default:
        return false;
    }
}

bool CIA402Device::velocity_mode(int8_t mode) {
    switch (mode) {
    case PV_mode:
    case CSV_mode:
        return true;
    default:
        return false;
    }
}

bool CIA402Device::torque_mode(int8_t mode) {
    switch (mode) {
    case PT_mode:
    case CST_mode:
        return true;
    default:
        return false;
    }
}

bool CIA402Device::profile_mode() const {
    return CIA402Device::profile_mode(*mode_read_);
}
bool CIA402Device::cyclic_mode() const {
    return CIA402Device::cyclic_mode(*mode_read_);
}
bool CIA402Device::position_mode() const {
    return CIA402Device::position_mode(*mode_read_);
}
bool CIA402Device::velocity_mode() const {
    return CIA402Device::velocity_mode(*mode_read_);
}
bool CIA402Device::torque_mode() const {
    return CIA402Device::torque_mode(*mode_read_);
}

bool CIA402Device::profile_mode_queried() const {
    return CIA402Device::profile_mode(*mode_set_);
}
bool CIA402Device::cyclic_mode_queried() const {
    return CIA402Device::cyclic_mode(*mode_set_);
}
bool CIA402Device::position_mode_queried() const {
    return CIA402Device::position_mode(*mode_set_);
}
bool CIA402Device::velocity_mode_queried() const {
    return CIA402Device::velocity_mode(*mode_set_);
}
bool CIA402Device::torque_mode_queried() const {
    return CIA402Device::torque_mode(*mode_set_);
}

std::string_view CIA402Device::control_mode_to_str(int8_t mode) {
    auto it = std_control_mode_str.find(mode);
    if (it != std_control_mode_str.end()) {
        return it->second;
    }
    return "";
}

///////////// STANDARD CONTROL MODES SPECIFIC ////////////////

bool CIA402Device::target_reached() const {
    if (profile_mode()) {
        if (not motion_halted()) {
            return (*status_word_ & flag_set_point_reached);
        }
    } else {
        if (not profile_mode_queried()) {
            // generate an error message only if user made a mistake
            pid_log << pid::warning
                    << "CIA402Device invalid command:  \"target_reached()\" is "
                       "only used in profile modes "
                    << pid::flush;
        }
        return false;
    }
    return false;
}

bool CIA402Device::motion_halted() const {
    return (*control_word_ & flag_stop_motion);
}

void CIA402Device::halt_motion(bool choise) {
    if (choise) {                           // 1-> stop axle.
        *control_word_ |= flag_stop_motion; // set bit 8 of control word
    } else {                                // 0 -> execute command
        *control_word_ &= static_cast<uint16_t>(~flag_stop_motion);
        // unset bit 8 of control word
    }
}

bool CIA402Device::manage_new_setpoint() {
    if (profile_mode() and position_mode()) {
        if (setpoint_management_ == READY_TO_ACCEPT) {
            // control word is ready to be changed again
            // ask device to follow new set points
            follow_set_point();
        } else { // NOT ready -> need to wait
            set_point_defined_ = true;
            // NOTE:: will be managed later
        }
        return true;
    } else {
        // NOTE: the user may have ask to change mode but mode is not
        // already changed -> target not reached BUT no error

        // generate an error message only if user made a mistake
        if (not profile_mode_queried() or not position_mode_queried()) {
            pid_log
                << pid::warning
                << "CIA402Device invalid command: \"manage_new_setpoint()\" is "
                   "only used in profile position modes "
                << pid::flush;
        }
        return false;
    }
}

void CIA402Device::reset_set_point() {
    *control_word_ &= static_cast<uint16_t>(
        ~flag_new_set_point_defined); // unset bit 4 of control word
    *control_word_ &= static_cast<uint16_t>(
        ~flag_change_set_point_now); // unset bit 5 of control word
}

void CIA402Device::follow_set_point() {
    *control_word_ |= flag_new_set_point_defined;
    *control_word_ |= flag_change_set_point_now;
    setpoint_management_ = SET_POINT_SET;
    // std::cout << "set point FOLLOWING" << std::endl;
}

void CIA402Device::managed_set_point() {
    // update setpoint management
    if (profile_mode() and position_mode()) {
        // std::cout << "managed_set_point ???? control: "
        //           << std::bitset<16>(*control_word_)
        //           << " status: " << std::bitset<16>(*status_word_) <<
        //           std::endl;
        switch (setpoint_management_) {
        case READY_TO_ACCEPT:
            if (set_point_defined_) {
                set_point_defined_ = false;
                follow_set_point();
            }
            break;
        case SET_POINT_SET:
            if (*status_word_ & flag_set_point_ack) {
                // std::cout << "managed_set_point -> ack new target" <<
                // std::endl; the new set point has been managed
                setpoint_management_ = ACCEPTED_SET_POINT;
                // make it possible to change setpoint again in next
                // cycles
                reset_set_point();
            }
            break;
        case ACCEPTED_SET_POINT:
            if (*status_word_ & ~flag_set_point_ack) {
                // std::cout << "managed_set_point -> accepting new target"
                //           << std::endl;
                setpoint_management_ = READY_TO_ACCEPT;
            }
            break;
        default: // READY_TO_ACCEPT
            break;
        }
    }
}

bool CIA402Device::new_setpoint_following() const {
    if (profile_mode() and position_mode()) {
        return setpoint_management_ == ACCEPTED_SET_POINT;
    } 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_queried() or not position_mode_queried()) {
            // generate an error message only if user made a mistake
            pid_log << pid::warning
                    << "CIA402Device : invalid query "
                       "new_setpoint_following()\" is "
                       "only used in profile position modes "
                    << pid::flush;
        }
    }
    return false;
}

bool CIA402Device::internal_limit() const {
    // internl limit is meaning full only of motion is not halted in profile
    // modes
    if (profile_mode() or not motion_halted()) {
        return (*status_word_ & flag_internal_limit);
    }
    return false;
}

} // namespace ethercatcpp::coe::cia402