SVHController.cpp

Go to the documentation of this file.

//
// © Copyright 2022 SCHUNK Mobile Greifsysteme GmbH, Lauffen/Neckar Germany
// © Copyright 2022 FZI Forschungszentrum Informatik, Karlsruhe, Germany
//
// This file is part of the Schunk SVH Library.
//
// The Schunk SVH Library is free software: you can redistribute it and/or
// modify it under the terms of the GNU General Public License as published by
// the Free Software Foundation, either version 3 of the License, or (at your
// option) any later version.
//
// The Schunk SVH Library 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 GNU General
// Public License for more details.
//
// You should have received a copy of the GNU General Public License along with
// the Schunk SVH Library. If not, see <https://www.gnu.org/licenses/>.
//
 
//----------------------------------------------------------------------
//----------------------------------------------------------------------
#include "schunk_svh_library/control/SVHController.h"
 
#include <chrono>
#include <functional>
#include <schunk_svh_library/Logger.h>
#include <schunk_svh_library/serial/ByteOrderConversion.h>
#include <thread>
 
using driver_svh::ArrayBuilder;
 
namespace driver_svh {
 
 
const char* SVHController::m_channel_description[] = {"Thumb_Flexion",
                                                      "Thumb_Opposition", // wrist
                                                      "Index_Finger_Distal",
                                                      "Index_Finger_Proximal",
                                                      "Middle_Finger_Distal",
                                                      "Middle_Finger_Proximal",
                                                      "Ring_Finger",
                                                      "Pinky",
                                                      "Finger_Spread",
                                                      NULL};
 
const float SVHController::CHANNEL_EFFORT_CONSTANTS[][2] = {
  // a, b         // y = a*x + b with y = effort and x = current
  0.015,
  -1.543, // Thumb Flexion
  0,
  0, // Thumb Opposition
  0.014,
  -0.07, // Index Finger Distal
  0.015,
  0.282, // Index Finger Proximal
  0.014,
  -0.026, // Middle Finger Distal
  0.015,
  0.336, // Middle Finger Proximal
  0.007,
  -0.073, // Ring Finger
  0.005,
  0.081, // Pinky
  0,
  0 // Finger Spread
};
 
SVHController::SVHController()
  : m_current_settings(SVH_DIMENSION)
  , // Vectors have to be filled with objects for correct deserialization
  m_position_settings(SVH_DIMENSION)
  , m_controller_feedback(SVH_DIMENSION)
  , m_serial_interface(new SVHSerialInterface(std::bind(
      &SVHController::receivedPacketCallback, this, std::placeholders::_1, std::placeholders::_2)))
  , m_enable_mask(0)
  , m_received_package_count(0)
{
  SVH_LOG_DEBUG_STREAM("SVHController", "SVH Controller started");
  m_firmware_info.version_major = 0;
  m_firmware_info.version_minor = 0;
}
 
SVHController::~SVHController()
{
  if (m_serial_interface != NULL)
  {
    disconnect();
    delete m_serial_interface;
    m_serial_interface = NULL;
  }
 
  SVH_LOG_DEBUG_STREAM("SVHController", "SVH Controller terminated");
}
 
bool SVHController::connect(const std::string& dev_name)
{
  SVH_LOG_DEBUG_STREAM("SVHController", "Connect was called, starting the serial interface...");
  if (m_serial_interface != NULL)
  {
    bool success = m_serial_interface->connect(dev_name);
    SVH_LOG_DEBUG_STREAM("SVHController",
                         "Connect finished " << ((success) ? "succesfully" : "with an error"));
    return success;
  }
  else
  {
    SVH_LOG_DEBUG_STREAM("SVHController", "Connect failed");
    return false;
  }
}
 
void SVHController::disconnect()
{
  SVH_LOG_DEBUG_STREAM("SVHController",
                       "Disconnect called, disabling all channels and closing interface...");
  if (m_serial_interface != NULL && m_serial_interface->isConnected())
  {
    // Disable all channels
    disableChannel(SVH_ALL);
    m_serial_interface->close();
  }
  // Reset the Firmware version, so we get always the current version on a reconnect or 0.0 on
  // failure
  m_firmware_info.version_major = 0;
  m_firmware_info.version_minor = 0;
 
  SVH_LOG_DEBUG_STREAM("SVHController", "Disconnect finished");
}
 
void SVHController::setControllerTarget(const SVHChannel& channel, const int32_t& position)
{
  // No Sanity Checks for out of bounds positions at this point as the finger manager has already
  // handled these
  if ((channel != SVH_ALL) && (channel >= 0 && channel < SVH_DIMENSION))
  {
    // The channel is encoded in the index byte
    SVHSerialPacket serial_packet(0, SVH_SET_CONTROL_COMMAND | static_cast<uint8_t>(channel << 4));
    SVHControlCommand control_command(position);
    // Note the 40 byte ArrayBuilder initialization -> this is needed to get a zero padding in the
    // serialpacket. Otherwise it would be shorter
    ArrayBuilder ab(40);
    ab << control_command;
    serial_packet.data = ab.array;
    m_serial_interface->sendPacket(serial_packet);
 
    // Debug Disabled as it is way to noisy
    SVH_LOG_DEBUG_STREAM("SVHController",
                         "Control command was given for channel: "
                           << channel << "Driving motor to position: " << position);
  }
  else
  {
    SVH_LOG_WARN_STREAM("SVHController",
                        "Control command was given for unknown (or all) channel: "
                          << channel << "- ignoring request");
  }
}
 
void SVHController::setControllerTargetAllChannels(const std::vector<int32_t>& positions)
{
  if (positions.size() >= SVH_DIMENSION)
  {
    SVHSerialPacket serial_packet(0, SVH_SET_CONTROL_COMMAND_ALL);
    SVHControlCommandAllChannels control_command(positions);
    ArrayBuilder ab(40);
    ab << control_command;
    serial_packet.data = ab.array;
    m_serial_interface->sendPacket(serial_packet);
 
    // Debug Disabled as it is way to noisy
    SVH_LOG_DEBUG_STREAM("SVHController",
                         "Control command was given for all channels: Driving motors to positions: "
                           << positions[0] << " , " << positions[1] << " , " << positions[2]
                           << " , " << positions[3] << " , " << positions[4] << " , "
                           << positions[5] << " , " << positions[6] << " , " << positions[7]
                           << " , " << positions[8] << " , ");
  }
  // We could theoretically allow fewer channels but this leaves some questions. Are the given
  // channels in right order? was it realy intented to just give fewer positions? What to do with
  // the ones that did not get anything? Just disallowing it seems to be the most understandable
  // decision.
  else
  {
    SVH_LOG_WARN_STREAM(
      "SVHController",
      "Control command was given for all channels but with to few points. Expected at least "
        << SVH_DIMENSION << " values but only got " << positions.size()
        << "use the individual setTarget function for this");
  }
}
 
void SVHController::enableChannel(const SVHChannel& channel)
{
  SVHSerialPacket serial_packet(0, SVH_SET_CONTROLLER_STATE);
  SVHControllerState controller_state;
  ArrayBuilder ab(40);
 
  SVH_LOG_DEBUG_STREAM("SVHController", "Enable of channel " << channel << " requested.");
 
  // In case no channel was enabled we need to enable the 12V dc drivers first
  if (m_enable_mask == 0)
  {
    SVH_LOG_DEBUG_STREAM("SVHController",
                         "Enable was called and no channel was previously activated, commands are "
                         "sent individually......");
    SVH_LOG_DEBUG_STREAM("SVHController",
                         "Sending pwm_fault and pwm_otw...(0x001F) to reset software warnings");
    // Reset faults and overtemperature warnings saved in the controller
    controller_state.pwm_fault = 0x001F;
    controller_state.pwm_otw   = 0x001F;
    ab << controller_state;
    serial_packet.data = ab.array;
    m_serial_interface->sendPacket(serial_packet);
    ab.reset(40);
 
    // Small delays seem to make communication at this point more reliable although they SHOULD NOT
    // be necessary
    std::this_thread::sleep_for(std::chrono::microseconds(2000));
 
    SVH_LOG_DEBUG_STREAM("SVHController",
                         "Enabling 12V Driver (pwm_reset and pwm_active = 0x0200)...");
    // enable +12v supply driver
    controller_state.pwm_reset  = 0x0200;
    controller_state.pwm_active = 0x0200;
    ab << controller_state;
    serial_packet.data = ab.array;
    m_serial_interface->sendPacket(serial_packet);
    ab.reset(40);
 
    std::this_thread::sleep_for(std::chrono::microseconds(2000));
 
    SVH_LOG_DEBUG_STREAM("SVHController", "Enabling pos_ctrl and cur_ctrl...");
    // enable controller
    controller_state.pos_ctrl = 0x0001;
    controller_state.cur_ctrl = 0x0001;
    ab << controller_state;
    serial_packet.data = ab.array;
    m_serial_interface->sendPacket(serial_packet);
    ab.reset(40);
 
    std::this_thread::sleep_for(std::chrono::microseconds(2000));
 
    SVH_LOG_DEBUG_STREAM("SVHController", "...Done");
  }
 
  // enable actual channels (again we only accept individual channels for safety)
  if (channel >= 0 && channel < SVH_DIMENSION)
  {
    SVH_LOG_DEBUG_STREAM("SVHController", "Enabling motor: " << channel);
    // oring all channels to create the activation mask-> high = channel active
    m_enable_mask |= (1 << channel);
 
    // SENDING EVERYTHING AT ONCE WILL LEAD TO "Jumping" behaviour of the controller on faster
    // Systems ---> this has to do with the initialization of the hardware controllers. If we split
    // it in two calls we will reset them first and then activate making sure that all values are
    // initialized properly effectively preventing any jumping behaviour
    ab.reset(40);
    controller_state.pwm_fault  = 0x001F;
    controller_state.pwm_otw    = 0x001F;
    controller_state.pwm_reset  = (0x0200 | (m_enable_mask & 0x01FF));
    controller_state.pwm_active = (0x0200 | (m_enable_mask & 0x01FF));
    ab << controller_state;
    serial_packet.data = ab.array;
    m_serial_interface->sendPacket(serial_packet);
    ab.reset(40);
 
    // WARNING: DO NOT ! REMOVE THESE DELAYS OR THE HARDWARE WILL! FREAK OUT! (see reason above)
    std::this_thread::sleep_for(std::chrono::microseconds(500));
 
    controller_state.pos_ctrl = 0x0001;
    controller_state.cur_ctrl = 0x0001;
    ab << controller_state;
    serial_packet.data = ab.array;
    m_serial_interface->sendPacket(serial_packet);
    ab.reset(40);
 
    SVH_LOG_DEBUG_STREAM("SVHController", "Enabled channel: " << channel);
  }
  else
  {
    SVH_LOG_ERROR_STREAM("SVHController",
                         "Activation request for ALL or unknown channel: " << channel
                                                                           << "- ignoring request");
  }
}
 
void SVHController::disableChannel(const SVHChannel& channel)
{
  SVH_LOG_DEBUG_STREAM("SVHController", "Disable of channel " << channel << " requested.");
 
  if (m_serial_interface != NULL && m_serial_interface->isConnected())
  {
    // prepare general packet
    SVHSerialPacket serial_packet(0, SVH_SET_CONTROLLER_STATE);
    SVHControllerState controller_state;
    ArrayBuilder ab(40);
 
    // we just accept it at this point because it makes no difference in the calls
    if (channel == SVH_ALL)
    {
      m_enable_mask              = 0;
      controller_state.pwm_fault = 0x001F;
      controller_state.pwm_otw   = 0x001F;
 
      // default initialization to zero -> controllers are deactivated
      ab << controller_state;
      serial_packet.data = ab.array;
      m_serial_interface->sendPacket(serial_packet);
 
      SVH_LOG_DEBUG_STREAM("SVHController", "Disabled all channels");
    }
    else if (channel >= 0 && channel < SVH_DIMENSION)
    {
      controller_state.pwm_fault = 0x001F;
      controller_state.pwm_otw   = 0x001F;
      // Disable the finger in the bitmask
      m_enable_mask &= ~(1 << channel);
 
      if (m_enable_mask != 0) // pos and current control stay on then
      {
        controller_state.pwm_reset  = (0x0200 | (m_enable_mask & 0x01FF));
        controller_state.pwm_active = (0x0200 | (m_enable_mask & 0x01FF));
        controller_state.pos_ctrl   = 0x0001;
        controller_state.cur_ctrl   = 0x0001;
      }
 
      ab << controller_state;
      serial_packet.data = ab.array;
      m_serial_interface->sendPacket(serial_packet);
 
      SVH_LOG_DEBUG_STREAM("SVHController", "Disabled channel: " << channel);
    }
    else
    {
      SVH_LOG_WARN_STREAM("SVHController",
                          "Disable was requestet for unknown channel: " << channel
                                                                        << "- ignoring request");
    }
  }
  else
  {
    SVH_LOG_ERROR_STREAM("SVHController",
                         "Disabling Channel not possible. Serial interface is not connected!");
  }
}
 
void SVHController::requestControllerState()
{
  SVH_LOG_DEBUG_STREAM("SVHController", "Requesting ControllerStatefrom Hardware");
  SVHSerialPacket serial_packet(40, SVH_GET_CONTROLLER_STATE);
  m_serial_interface->sendPacket(serial_packet);
}
 
void SVHController::requestControllerFeedback(const SVHChannel& channel)
{
  if ((channel != SVH_ALL) && (channel >= 0 && channel < SVH_DIMENSION))
  {
    SVHSerialPacket serial_packet(40,
                                  SVH_GET_CONTROL_FEEDBACK | static_cast<uint8_t>(channel << 4));
    m_serial_interface->sendPacket(serial_packet);
 
    // Disabled as it spams the output to much
    SVH_LOG_DEBUG_STREAM("SVHController",
                         "Controller feedback was requested for channel: " << channel);
  }
  else if (channel == SVH_ALL)
  {
    SVHSerialPacket serial_packet(40, SVH_GET_CONTROL_FEEDBACK_ALL);
    m_serial_interface->sendPacket(serial_packet);
 
    // Disabled as it spams the output to much
    SVH_LOG_DEBUG_STREAM("SVHController", "Controller feedback was requested for all channels ");
  }
  else
  {
    SVH_LOG_WARN_STREAM(
      "SVHController",
      "Controller feedback was requestet for unknown channel: " << channel << "- ignoring request");
  }
}
 
void SVHController::requestPositionSettings(const SVHChannel& channel)
{
  SVH_LOG_DEBUG_STREAM("SVHController",
                       "Requesting PositionSettings from Hardware for channel: " << channel);
  SVHSerialPacket serial_packet(40,
                                (SVH_GET_POSITION_SETTINGS | static_cast<uint8_t>(channel << 4)));
  m_serial_interface->sendPacket(serial_packet);
}
 
void SVHController::setPositionSettings(const SVHChannel& channel,
                                        const SVHPositionSettings& position_settings)
{
  if ((channel != SVH_ALL) && (channel >= 0 && channel < SVH_DIMENSION))
  {
    SVHSerialPacket serial_packet(0,
                                  SVH_SET_POSITION_SETTINGS | static_cast<uint8_t>(channel << 4));
    ArrayBuilder ab;
    ab << position_settings;
    serial_packet.data = ab.array;
    m_serial_interface->sendPacket(serial_packet);
 
    // Save already in case we dont get immediate response
    m_position_settings[channel] = position_settings;
 
    SVH_LOG_DEBUG_STREAM("SVHController",
                         "Position controller settings where send to change channel: " << channel
                                                                                       << " : ");
    SVH_LOG_DEBUG_STREAM("SVHController",
                         "wmn " << position_settings.wmn << " "
                                << "wmx " << position_settings.wmx << " "
                                << "dwmx " << position_settings.dwmx << " "
                                << "ky " << position_settings.ky << " "
                                << "dt " << position_settings.dt << " "
                                << "imn " << position_settings.imn << " "
                                << "imx " << position_settings.imx << " "
                                << "kp " << position_settings.kp << " "
                                << "ki " << position_settings.ki << " "
                                << "kd " << position_settings.kd << " ");
  }
  else
  {
    SVH_LOG_WARN_STREAM("SVHController",
                        "Position controller settings where given for unknown channel: "
                          << channel << "- ignoring request");
  }
}
 
void SVHController::requestCurrentSettings(const SVHChannel& channel)
{
  SVH_LOG_DEBUG_STREAM("SVHController", "Requesting CurrentSettings for channel: " << channel);
 
  if ((channel != SVH_ALL) && (channel >= 0 && channel < SVH_DIMENSION))
  {
    SVHSerialPacket serial_packet(40,
                                  (SVH_GET_CURRENT_SETTINGS | static_cast<uint8_t>(channel << 4)));
    m_serial_interface->sendPacket(serial_packet);
  }
  else
  {
    SVH_LOG_WARN_STREAM(
      "SVHController",
      "Get Current Settings can only be requested with a specific channel, ALL or unknown channel:"
        << channel << "was selected ");
  }
}
 
void SVHController::setCurrentSettings(const SVHChannel& channel,
                                       const SVHCurrentSettings& current_settings)
{
  if ((channel != SVH_ALL) && (channel >= 0 && channel < SVH_DIMENSION))
  {
    SVHSerialPacket serial_packet(0, SVH_SET_CURRENT_SETTINGS | static_cast<uint8_t>(channel << 4));
    ArrayBuilder ab;
    ab << current_settings;
    serial_packet.data = ab.array;
    m_serial_interface->sendPacket(serial_packet);
 
    // Save already in case we dont get immediate response
    m_current_settings[channel] = current_settings;
 
    SVH_LOG_DEBUG_STREAM("SVHController",
                         "Current controller settings where send to change channel: " << channel
                                                                                      << " : ");
    SVH_LOG_DEBUG_STREAM("SVHController",
                         "wmn " << current_settings.wmn << " "
                                << "wmx " << current_settings.wmx << " "
                                << "ky " << current_settings.ky << " "
                                << "dt " << current_settings.dt << " "
                                << "imn " << current_settings.imn << " "
                                << "imx " << current_settings.imx << " "
                                << "kp " << current_settings.kp << " "
                                << "ki " << current_settings.ki << " "
                                << "umn " << current_settings.umn << " "
                                << "umx " << current_settings.umx);
  }
  else
  {
    SVH_LOG_WARN_STREAM("SVHController",
                        "Current controller settings where given for unknown channel: "
                          << channel << "- ignoring request");
  }
}
 
void SVHController::requestEncoderValues()
{
  SVH_LOG_DEBUG_STREAM("SVHController", "Requesting EncoderValues from hardware");
  SVHSerialPacket serial_packet(40, SVH_GET_ENCODER_VALUES);
  m_serial_interface->sendPacket(serial_packet);
}
 
void SVHController::setEncoderValues(const SVHEncoderSettings& encoder_settings)
{
  SVH_LOG_DEBUG_STREAM("SVHController", "Setting new Encoder values : ");
  for (size_t i = 0; i < encoder_settings.scalings.size(); i++)
  {
    // log stream unfortunately does not support the standard to stream operators yet
    SVH_LOG_DEBUG_STREAM("SVHController",
                         "[" << (int)i << "] "
                             << ": " << encoder_settings.scalings[i] << " ");
  }
 
  SVHSerialPacket serial_packet(0, SVH_SET_ENCODER_VALUES);
  ArrayBuilder ab;
  ab << encoder_settings;
  serial_packet.data = ab.array;
  m_serial_interface->sendPacket(serial_packet);
 
  // Save already in case we dont get imediate response
  m_encoder_settings = encoder_settings;
}
 
void SVHController::requestFirmwareInfo()
{
  SVH_LOG_DEBUG_STREAM("SVHController", "Requesting firmware Information from hardware");
 
  SVHSerialPacket serial_packet(40, SVH_GET_FIRMWARE_INFO);
  m_serial_interface->sendPacket(serial_packet);
}
 
void SVHController::receivedPacketCallback(const SVHSerialPacket& packet, unsigned int packet_count)
{
  // Extract Channel
  uint8_t channel = (packet.address >> 4) & 0x0F;
  // Prepare Data for conversion
  ArrayBuilder ab;
  ab.appendWithoutConversion(packet.data);
 
  SVHControllerFeedbackAllChannels feedback_all;
 
  m_received_package_count = packet_count;
 
  // Packet meaning is encoded in the lower nibble of the adress byte
  switch (packet.address & 0x0F)
  {
    case SVH_GET_CONTROL_FEEDBACK:
    case SVH_SET_CONTROL_COMMAND:
      if (channel >= 0 && channel < SVH_DIMENSION)
      {
        // std::cout << "Recieved: Controllerfeedback RAW Data: " << ab;
        ab >> m_controller_feedback[channel];
        // Disabled as this is spamming the output to much
        SVH_LOG_DEBUG_STREAM("SVHController",
                             "Received a Control Feedback/Control Command packet for channel "
                               << channel
                               << " Position: " << (int)m_controller_feedback[channel].position
                               << " Current: " << (int)m_controller_feedback[channel].current);
      }
      else
      {
        SVH_LOG_ERROR_STREAM(
          "SVHController",
          "Received a Control Feedback/Control Command packet for ILLEGAL channel "
            << channel << "- packet ignored!");
      }
      break;
    case SVH_GET_CONTROL_FEEDBACK_ALL:
    case SVH_SET_CONTROL_COMMAND_ALL:
      // We cannot just read them all into the vector (which would have been nice) because the
      // feedback of all channels is structured different from the feedback of one channel. So the
      // SVHControllerFeedbackAllChannels is used as an intermediary ( handles the deserialization)
      ab >> feedback_all;
      m_controller_feedback = feedback_all.feedbacks;
      // Disabled as this is spannimg the output to much
      SVH_LOG_DEBUG_STREAM(
        "SVHController",
        "Received a Control Feedback/Control Command packet for channel all channels ");
      break;
    case SVH_GET_POSITION_SETTINGS:
    case SVH_SET_POSITION_SETTINGS:
      if (channel >= 0 && channel < SVH_DIMENSION)
      {
        // std::cout << "Recieved: Postitionsettings RAW Data: " << ab; // for really intensive
        // debugging
        ab >> m_position_settings[channel];
        SVH_LOG_DEBUG_STREAM("SVHController",
                             "Received a get/set position setting packet for channel " << channel);
        SVH_LOG_DEBUG_STREAM("SVHController",
                             "wmn " << m_position_settings[channel].wmn << " "
                                    << "wmx " << m_position_settings[channel].wmx << " "
                                    << "dwmx " << m_position_settings[channel].dwmx << " "
                                    << "ky " << m_position_settings[channel].ky << " "
                                    << "dt " << m_position_settings[channel].dt << " "
                                    << "imn " << m_position_settings[channel].imn << " "
                                    << "imx " << m_position_settings[channel].imx << " "
                                    << "kp " << m_position_settings[channel].kp << " "
                                    << "ki " << m_position_settings[channel].ki << " "
                                    << "kd " << m_position_settings[channel].kd);
      }
      else
      {
        SVH_LOG_ERROR_STREAM("SVHController",
                             "Received a get/set position setting packet for ILLEGAL channel "
                               << channel << "- packet ignored!");
      }
      break;
    case SVH_GET_CURRENT_SETTINGS:
    case SVH_SET_CURRENT_SETTINGS:
      if (channel >= 0 && channel < SVH_DIMENSION)
      {
        // std::cout << "Recieved: Current Settings RAW Data: " << ab; // for really intensive
        // debugging
        ab >> m_current_settings[channel];
        SVH_LOG_DEBUG_STREAM("SVHController",
                             "Received a get/set current setting packet for channel " << channel);
        SVH_LOG_DEBUG_STREAM("SVHController",
                             "wmn " << m_current_settings[channel].wmn << " "
                                    << "wmx " << m_current_settings[channel].wmx << " "
                                    << "ky " << m_current_settings[channel].ky << " "
                                    << "dt " << m_current_settings[channel].dt << " "
                                    << "imn " << m_current_settings[channel].imn << " "
                                    << "imx " << m_current_settings[channel].imx << " "
                                    << "kp " << m_current_settings[channel].kp << " "
                                    << "ki " << m_current_settings[channel].ki << " "
                                    << "umn " << m_current_settings[channel].umn << " "
                                    << "umx " << m_current_settings[channel].umx << " ");
      }
      else
      {
        SVH_LOG_ERROR_STREAM("SVHController",
                             "Received a get/set current setting packet for ILLEGAL channel "
                               << channel << "- packet ignored!");
      }
      break;
    case SVH_GET_CONTROLLER_STATE:
    case SVH_SET_CONTROLLER_STATE:
      // std::cout << "Recieved: Controller State RAW Data: " << ab; // for really intensive
      // debugging
      ab >> m_controller_state;
      // std::cout << "Received controllerState interpreded data: "<< m_controller_state <<
      // std::endl; // for really intensive debugging
      SVH_LOG_DEBUG_STREAM("SVHController", "Received a get/set controler state packet ");
      SVH_LOG_DEBUG_STREAM("SVHController",
                           "Controllerstate (NO HEX):"
                             << "pwm_fault "
                             << "0x" << static_cast<int>(m_controller_state.pwm_fault) << " "
                             << "pwm_otw "
                             << "0x" << static_cast<int>(m_controller_state.pwm_otw) << " "
                             << "pwm_reset "
                             << "0x" << static_cast<int>(m_controller_state.pwm_reset) << " "
                             << "pwm_active "
                             << "0x" << static_cast<int>(m_controller_state.pwm_active) << " "
                             << "pos_ctr "
                             << "0x" << static_cast<int>(m_controller_state.pos_ctrl) << " "
                             << "cur_ctrl "
                             << "0x" << static_cast<int>(m_controller_state.cur_ctrl));
      break;
    case SVH_GET_ENCODER_VALUES:
    case SVH_SET_ENCODER_VALUES:
      SVH_LOG_DEBUG_STREAM("SVHController", "Received a get/set encoder settings packet ");
      ab >> m_encoder_settings;
      break;
    case SVH_GET_FIRMWARE_INFO:
      // std::cout << "Recieved: Firmware Settings RAW Data: " << ab; // for really intensive
      // debugging
      ab >> m_firmware_info;
      SVH_LOG_INFO_STREAM("SVHController",
                          "Hardware is using the following Firmware: "
                            << m_firmware_info.svh << " Version: " << m_firmware_info.version_major
                            << "." << m_firmware_info.version_minor << " : "
                            << m_firmware_info.text);
      break;
    default:
      SVH_LOG_ERROR_STREAM("SVHController",
                           "Received a Packet with unknown address: " << (packet.address & 0x0F)
                                                                      << " - ignoring packet");
      break;
  }
}
 
bool SVHController::getControllerFeedback(const SVHChannel& channel,
                                          SVHControllerFeedback& controller_feedback)
{
  if (channel >= 0 && static_cast<uint8_t>(channel) < m_controller_feedback.size())
  {
    controller_feedback = m_controller_feedback[channel];
    return true;
  }
  else
  {
    SVH_LOG_WARN_STREAM("SVHController",
                        "GetFeedback was requested for unknown channel: " << channel
                                                                          << "- ignoring request");
    return false;
  }
}
 
void SVHController::getControllerFeedbackAllChannels(
  SVHControllerFeedbackAllChannels& controller_feedback)
{
  controller_feedback.feedbacks = m_controller_feedback;
}
 
bool SVHController::getPositionSettings(const SVHChannel& channel,
                                        SVHPositionSettings& position_settings)
{
  if (channel >= 0 && static_cast<uint8_t>(channel) < m_position_settings.size())
  {
    position_settings = m_position_settings[channel];
    return true;
  }
  else
  {
    SVH_LOG_WARN_STREAM(
      "SVHController",
      "GetPositionSettings was requested for unknown channel: " << channel << "- ignoring request");
    return false;
  }
}
 
bool SVHController::getCurrentSettings(const SVHChannel& channel,
                                       SVHCurrentSettings& current_settings)
{
  if (channel >= 0 && static_cast<uint8_t>(channel) < m_current_settings.size())
  {
    current_settings = m_current_settings[channel];
    return true;
  }
  else
  {
    SVH_LOG_WARN_STREAM(
      "SVHController",
      "GetCurrentSettings was requested for unknown channel: " << channel << "- ignoring request");
    return false;
  }
}
 
SVHFirmwareInfo SVHController::getFirmwareInfo()
{
  return m_firmware_info;
}
 
void SVHController::resetPackageCounts()
{
  m_received_package_count = 0;
  // The serial interface also keeps track about these counts
  m_serial_interface->resetTransmitPackageCount();
  SVH_LOG_DEBUG_STREAM("SVHController", "Received package count resetted");
}
 
unsigned int SVHController::getSentPackageCount()
{
  if (m_serial_interface != NULL)
  {
    return m_serial_interface->transmittedPacketCount();
  }
  else
  {
    SVH_LOG_WARN_STREAM("SVHController",
                        "Request for transmit packet count could not be answered as the device is "
                        "not connected - ignoring request");
    return 0;
  }
}
 
unsigned int SVHController::getReceivedPackageCount()
{
  return m_received_package_count;
}
 
bool SVHController::isEnabled(const SVHChannel& channel)
{
  return ((1 << channel & m_enable_mask) > 0);
}
 
 
} // namespace driver_svh