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 "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];
};

class RokubiminiSerialImpl
{
public:
  RokubiminiSerialImpl() = delete;

  RokubiminiSerialImpl(const std::string& name, const std::string& port, const std::uint32_t& baudRate);

  ~RokubiminiSerialImpl() = default;

  bool init();

  bool startup();

  void updateRead(){};

  void updateWrite(){};

  void shutdown();

  void setState(const uint16_t state){};

  bool waitForState(const uint16_t state)
  {
    return false;
  }

  std::string getName() const
  {
    return name_;
  }

  // Methods for all the SDOs

  bool getSerialNumber(unsigned int& serialNumber)
  {
    return false;
  }

  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 uint32_t& baudRate);
  bool saveConfigParameter();

  bool loadConfig();

  bool printUserConfig();

  bool firmwareUpdate(const std::string& filePath);

  bool isRunning()
  {
    return isRunning_;
  }

  void setPollingTimeStep(double timeStep)
  {
    pollingThreadTimeStep_ = timeStep;
  }

  bool hasFrameSync()
  {
    return frameSync_;
  }

private:
  bool startPollingThread();

  bool writeSerial(const std::string& str);

  bool sendCalibrationMatrixEntry(const uint8_t subId, const double entry)
  {
    return false;
  }

  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 hasError() const;

  bool isInConfigMode() const;


  ConnectionState getConnectionState() const;

  ErrorState getErrorState() const;

  std::string getErrorString() const;


  bool initSensorCommunication(bool keepConnecting);

  std::uint32_t getBaudRateDefinition(const uint32_t& baudRate);

  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();

  /************ rokubimini_interface methods **************/

  std::string name_;

  Reading serialImplReading_;

  std::string port_;

  std::uint32_t baudRate_;

  mutable std::recursive_mutex readingMutex_;

  mutable std::recursive_mutex serialMutex_;

  /************ rokubimini_interface variables **************/
  /*
   * Device USB communication inteface
   */
  // int deviceId_;
  // boost::atomic<int> productId_;

  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
   */

  double timeStampSecs_;

  bool runInThreadedMode_;

  double pollingThreadTimeStep_;

  bool useDeviceTimeStamps_;

  boost::thread connectionThread_;

  boost::thread pollingThread_;

  boost::atomic<ConnectionState> connectionState_;

  boost::atomic<ErrorState> errorState_;

  boost::atomic<ModeState> modeState_;
  boost::atomic<bool> isRunning_;
  // boost::atomic<bool> isReceivingOffset_;

  unsigned long pollingSyncErrorCounter_;

  unsigned long frameReceivedCounter_;

  unsigned long frameSuccessCounter_;

  unsigned long frameCrcErrorCounter_;

  unsigned int frameSyncErrorCounter_;

  unsigned int maxFrameSyncErrorCounts_;
  const static uint64_t NSEC_PER_SEC = 1000000000;
};

using RokubiminiSerialImplPtr = std::shared_ptr<RokubiminiSerialImpl>;

}  // namespace serial
}  // namespace rokubimini