RokubiminiSerialImpl.hpp

Go to the documentation of this file.

/*
 * RokubiminiSerialImpl.hpp
 *  Created on: April, 2020
 *  Author(s):  Mike Karamousadakis, Ilias Patsiaouras
 *
 *  and is based on:
 *
 * ForceTorqueSensor.hpp
 *  Created on:   Dec 19, 2016
 *  Author(s):    Christian Gehring, Vassilios Tsounis, Klajd Lika
 */
 
#pragma once
 
#include <fcntl.h>
#include <sys/types.h>
#include <termios.h>
#include <boost/thread.hpp>
#include <chrono>
#include <string>
#include <iostream>
#include <fstream>
#include <string>
#include <condition_variable>
#include <atomic>
#include <memory>
#include <diagnostic_updater/diagnostic_updater.h>
 
// #include "rokubimini_serial/RokubiminiSerialImplOptions.hpp"
#include <rokubimini_serial/states.hpp>
 
#include <rokubimini/Reading.hpp>
#include <rokubimini/configuration/Configuration.hpp>
#include <rokubimini/calibration/SensorCalibration.hpp>
#include <rokubimini/configuration/SensorConfiguration.hpp>
#include <rokubimini/configuration/ForceTorqueFilter.hpp>
#include <rokubimini_serial/RokubiminiSerialCommunication.hpp>
 
namespace rokubimini
{
namespace serial
{
union DataStatus
{
  struct __attribute__((__packed__))
  {
    uint16_t app_took_too_long : 1;
    uint16_t overrange : 1;
    uint16_t invalid_measurements : 1;  // saturation, short, open circuit
    uint16_t raw_measurements : 1;      // gages instead of forces
    uint16_t : 12;                      // reserved
  };
  uint16_t byte;
};
 
union AppOutput
{
  struct __attribute__((__packed__))
  {
    DataStatus status;
    float forces[6];
    uint32_t timestamp;
    float temperature;
    // float volts;
  };
  uint8_t bytes[1];
};
 
union RxFrame
{
  struct __attribute__((__packed__))
  {
    uint8_t header;
    AppOutput data;
    uint16_t crc16_ccitt;
  };
  uint8_t bytes[1];
};
struct BaudRateStruct
{
  uint32_t value; /* The baud rate of the serial communication, as a normal integer (e.g. 115200 etc) */
  uint32_t mask;  /* The baud rate of the serial communication, as a termios bit mask. */
};
const static std::map<uint32_t, BaudRateStruct> CODE_TO_BAUD_RATE_MAP = {
  { 0, { 9600, B9600 } },
  { 1, { 57600, B57600 } },
  { 2, { 115200, B115200 } },
  // WARNING: The option no. 3, is used in the code as the default option. Be careful if you change this struct.
  { 3, { 230400, B230400 } },
  // WARNING: The option no. 4, is used in the code as the max option. Be careful if you change this struct.
  { 4, { 460800, B460800 } }
 
};
class RokubiminiSerialImpl
{
public:
  RokubiminiSerialImpl() = delete;
 
  RokubiminiSerialImpl(const std::string& name, const std::string& port);
 
  ~RokubiminiSerialImpl() = default;
 
  bool init();
 
  bool startup();
 
  void updateRead(){ /* do nothing */ };
 
  void updateWrite(){ /* do nothing */ };
 
  void closeSerialPort();
 
  void shutdown();
 
  std::string getName() const
  {
    return name_;
  }
 
  std::string getModeStateString() const
  {
    switch (modeState_)
    {
      case ModeState::CONFIG_MODE:
        return "Config Mode";
      case ModeState::RUN_MODE:
        return "Run Mode";
      case ModeState::INIT_MODE:
        return "Init Mode";
      default:
        return "ERROR: Unrecognized mode state";
    }
  }
 
  std::string getConnectionStateString() const
  {
    switch (connectionState_)
    {
      case ConnectionState::DISCONNECTED:
        return "Disconnected";
      case ConnectionState::ISCONNECTING:
        return "Is Connecting";
      case ConnectionState::CONNECTED:
        return "Connected";
      default:
        return "ERROR: Unrecognized connection state";
    }
  }
 
  bool getSerialNumber(unsigned int& serialNumber)
  {
    serialNumber = serialNumber_;
    return true;
  }
 
  bool getForceTorqueSamplingRate(int& samplingRate)
  {
    return false;
  }
 
  bool setForceTorqueFilter(const configuration::ForceTorqueFilter& filter);
 
  bool setAccelerationFilter(const unsigned int filter)
  {
    return false;
  }
 
  bool setAngularRateFilter(const unsigned int filter)
  {
    return false;
  }
 
  bool setAccelerationRange(const unsigned int range)
  {
    return false;
  }
 
  bool setAngularRateRange(const unsigned int range)
  {
    return false;
  }
 
  bool setForceTorqueOffset(const Eigen::Matrix<double, 6, 1>& forceTorqueOffset);
 
  bool setSensorConfiguration(const configuration::SensorConfiguration& sensorConfiguration);
 
  bool setSensorCalibration(const calibration::SensorCalibration& sensorCalibration);
 
  void getReading(rokubimini::Reading& reading);
 
  bool setConfigMode();
 
  bool setRunMode();
 
  bool setHardwareReset();
 
  bool setInitMode();
 
  bool setCommunicationSetup(const configuration::SensorConfiguration& sensorConfiguration, const uint8_t& dataFormat,
                             const uint8_t& baudRate);
  bool saveConfigParameter();
 
  bool loadConfig();
 
  bool printUserConfig();
 
  bool firmwareUpdate(const std::string& filePath);
 
  bool isRunning()
  {
    return isRunning_;
  }
 
  bool hasFrameSync()
  {
    return frameSync_;
  }
 
  bool parseCommunicationMsgs(const double& timeout = 1.0);
 
  std::string getProductName() const
  {
    return productName_;
  }
 
  bool runsAsync()
  {
    return runsAsync_;
  }
 
  void setRunsAsync(bool runsAsync)
  {
    runsAsync_ = runsAsync;
  }
 
  std::vector<std::string> getErrorStrings() const;
 
  bool hasError() const;
 
  void updateConnectionStatus(diagnostic_updater::DiagnosticStatusWrapper& stat);
 
  void getFrameDataStatus(DataStatus& status);
 
  void createDataFlagsDiagnostics(diagnostic_updater::DiagnosticStatusWrapper& stat);
 
  void resetDataFlags();
 
  void updateDataFlags();
 
private:
  bool startPollingThread();
 
  bool writeSerial(const std::string& str);
 
  bool readSerialNoWait(const uint32_t& numChars, std::string& str);
 
  bool readSerialWaitTimeout(const uint32_t& numChars, std::string& str, const double& timeout = 1.0);
 
  bool parseAcknowledgement(const char& command_code, const double& timeout = 2.0);
 
  bool clearReadBuffer();
 
  bool sendCommand(const std::string& command);
 
  bool sendCalibrationMatrixEntry(const uint8_t subId, const double entry)
  {
    return false;
  }
 
  bool parseRegexWaitTimeout(RokubiminiSerialResponseRegex& reg, const double& timeout = 1.0);
 
  using timespec = struct timespec;
 
  inline double diffSec(timespec a, timespec b)
  {
    return (static_cast<double>(b.tv_sec - a.tv_sec) + static_cast<double>(b.tv_nsec - a.tv_nsec) / NSEC_PER_SEC);
  }
 
  inline double timespecToSec(timespec t)
  {
    return (static_cast<double>(t.tv_sec) + static_cast<double>(t.tv_nsec) / NSEC_PER_SEC);
  }
 
  inline timespec timespecAdd(timespec a, timespec b)
  {
    timespec result;
    if (static_cast<uint64_t>(a.tv_nsec + b.tv_nsec) >= NSEC_PER_SEC)
    {
      result.tv_sec = a.tv_sec + b.tv_sec + 1;
      result.tv_nsec = a.tv_nsec + b.tv_nsec - NSEC_PER_SEC;
    }
    else
    {
      result.tv_sec = a.tv_sec + b.tv_sec;
      result.tv_nsec = a.tv_nsec + b.tv_nsec;
    }
    return result;
  }
 
  bool connect();
 
  bool connect(const std::string& port);
 
  bool readDevice(RxFrame& frame);
 
  bool isConnected() const;
 
  bool isConnecting() const;
 
  bool isInConfigMode() const;
 
  ConnectionState getConnectionState() const;
 
  ErrorFlags getErrorFlags() const;
 
  bool openSerialPort(bool keepOpening);
  bool initSensorCommunication(bool keepOpening);
 
  bool initSerialPort(const std::string& port);
 
  uint16_t calcCrc16X25(uint8_t* data, int len);
 
  uint16_t crcCcittUpdate(uint16_t crc, uint8_t data);
 
  void connectionWorker();
 
  void pollingWorker();
 
  void increaseAndCheckTimeoutCounter();
 
  std::string name_;
 
  Reading serialImplReading_;
 
  std::string port_;
 
  BaudRateStruct baudRate_;
 
  std::string productName_;
 
  uint32_t serialNumber_;
 
  mutable std::mutex readingMutex_;
 
  mutable std::recursive_mutex serialMutex_;
 
  boost::atomic<int> usbFileDescriptor_;
 
  boost::atomic<bool> frameSync_;
 
  std::ifstream usbStreamIn_;
 
  std::ofstream usbStreamOut_;
 
  uint8_t frameHeader = 0xAA;
 
  RxFrame frame_;
 
  /*
   *  Device measurements and data buffers
   */
 
  bool runInThreadedMode_;
 
  boost::thread connectionThread_;
 
  boost::thread pollingThread_;
 
  boost::atomic<ConnectionState> connectionState_;
 
  ErrorFlags errorFlags_;
  boost::atomic<ModeState> modeState_;
  boost::atomic<bool> isRunning_;
 
  unsigned long pollingSyncErrorCounter_;
 
  unsigned long frameReceivedCounter_;
 
  unsigned long frameSuccessCounter_;
 
  unsigned long frameCrcErrorCounter_;
 
  unsigned int frameSyncErrorCounter_;
 
  unsigned int maxFrameSyncErrorCounts_;
  unsigned int maxCountOpenSerialPort_;
  const static uint64_t NSEC_PER_SEC = 1000000000;
  const static uint8_t DEFAULT_BAUD_RATE_OPTION = 3;
  const static uint8_t MAX_BAUD_RATE_OPTION = 4;
 
  uint32_t frameOffset_;
 
  RxFrame placeholder_;
 
  constexpr static double FTDI_DRIVER_LATENCY = 0.001;
 
  constexpr static double TIMEOUT_MARGIN = 0.15;
 
  constexpr static double MAXIMUM_ACCEPTABLE_TIMEOUT = 1.77;
 
  double readTimeout_;
 
  unsigned int timeoutCounter_;
 
  std::condition_variable newFrameIsAvailable_;
 
  bool dataReady_;
 
  std::atomic<bool> runsAsync_;
 
  DataStatus frameDataStatus_;
 
  std::atomic<DataStatus> currentDataFlagsDiagnostics_;
  ros::Time frameTimestamp_;
};
 
using RokubiminiSerialImplPtr = std::shared_ptr<RokubiminiSerialImpl>;
 
}  // namespace serial
}  // namespace rokubimini