The Rokubimini Serial Implementation class. More...
#include <RokubiminiSerialImpl.hpp>
Public Member Functions | |
bool | firmwareUpdate (const std::string &filePath) |
Updates the firmware of the device. More... | |
bool | getForceTorqueSamplingRate (int &samplingRate) |
Gets the force torque sampling rate of the device. More... | |
std::string | getName () const |
Accessor for device name. More... | |
void | getReading (rokubimini::Reading &reading) |
Gets the internal reading variable. More... | |
bool | getSerialNumber (unsigned int &serialNumber) |
Accessor for device serial number. More... | |
bool | hasFrameSync () |
Returns if there is frame synchronization with the device. More... | |
bool | init () |
This method initializes internal variables and the device for communication. It connects to the serial-port. More... | |
bool | isRunning () |
Returns if the serial driver is running. More... | |
bool | loadConfig () |
Loads the configuration of the device. More... | |
bool | printUserConfig () |
Prints all the user configurable parameters. More... | |
RokubiminiSerialImpl ()=delete | |
Default constructor is deleted. More... | |
RokubiminiSerialImpl (const std::string &name, const std::string &port, const std::uint32_t &baudRate) | |
Custom constructor for the RokubiminiSerialImpl class. More... | |
bool | saveConfigParameter () |
Saves the current configuration to the device. More... | |
bool | setAccelerationFilter (const unsigned int filter) |
Sets an acceleration filter to the device. More... | |
bool | setAccelerationRange (const unsigned int range) |
Sets an acceleration range to the device. More... | |
bool | setAngularRateFilter (const unsigned int filter) |
Sets an angular rate filter to the device. More... | |
bool | setAngularRateRange (const unsigned int range) |
Sets an angular rate range to the device. More... | |
bool | setCommunicationSetup (const configuration::SensorConfiguration &sensorConfiguration, const uint8_t &dataFormat, const uint32_t &baudRate) |
Sets communication setup for the device. This includes setting the temperature compensation, the matrix calibration, the data format and the baud rate. More... | |
bool | setConfigMode () |
Sets the device in config mode. More... | |
bool | setForceTorqueFilter (const configuration::ForceTorqueFilter &filter) |
Sets a force torque filter to the device. More... | |
bool | setForceTorqueOffset (const Eigen::Matrix< double, 6, 1 > &forceTorqueOffset) |
Sets a force torque offset to the device. More... | |
bool | setHardwareReset () |
Triggers a hardware reset of the sensor. More... | |
bool | setInitMode () |
Triggers a software reset of the sensor bringing it to a known state. More... | |
void | setPollingTimeStep (double timeStep) |
Sets the time step of the polling thread in seconds. timeStep The time step in seconds. More... | |
bool | setRunMode () |
Sets the device in run mode. More... | |
bool | setSensorCalibration (const calibration::SensorCalibration &sensorCalibration) |
Sets a sensor calibration to the device. More... | |
bool | setSensorConfiguration (const configuration::SensorConfiguration &sensorConfiguration) |
Sets a sensor configuration to the device. More... | |
void | setState (const uint16_t state) |
Sets the state of the device in the bus (unused). More... | |
void | shutdown () |
Shuts down the device. It automatically shuts-down threads and disconnects from serial-port. More... | |
bool | startup () |
This method starts up communication with the device. More... | |
void | updateRead () |
This method is called by the BusManager. Each device attached to this bus reads its data from the buffer (not used). More... | |
void | updateWrite () |
This method is called by the BusManager. Each device attached to the bus writes its data to the buffer (not used). More... | |
bool | waitForState (const uint16_t state) |
Wait for a state to be reached (unused). More... | |
~RokubiminiSerialImpl ()=default | |
Private Types | |
using | timespec = struct timespec |
Private Member Functions | |
uint16_t | calcCrc16X25 (uint8_t *data, int len) |
bool | connect () |
Connects to the Serial-over-USB port. More... | |
bool | connect (const std::string &port) |
Connects to the Serial-over-USB port. More... | |
void | connectionWorker () |
Worker threads for managing sensor connections. More... | |
uint16_t | crcCcittUpdate (uint16_t crc, uint8_t data) |
Implementation function of the CRC16 X25 checksum for the input data. More... | |
double | diffSec (timespec a, timespec b) |
Calculates the difference (b-a) of two timespecs in seconds. More... | |
std::uint32_t | getBaudRateDefinition (const uint32_t &baudRate) |
Returns the system definition for the given baudRate. More... | |
ConnectionState | getConnectionState () const |
Retreive sensor state/status. More... | |
ErrorState | getErrorState () const |
Gets the current error status. More... | |
std::string | getErrorString () const |
Retrieves detailed error indication. More... | |
bool | hasError () const |
Checks the connection has errors. More... | |
bool | initSensorCommunication (bool keepConnecting) |
Initializatin-time helper functions. More... | |
bool | initSerialPort (const std::string &port) |
Sets up and initializes the serial port for communication. More... | |
bool | isConnected () const |
Quick sensor/state testers. More... | |
bool | isConnecting () const |
Checks if device is already in the process of connecting. More... | |
bool | isInConfigMode () const |
Checks if the ModeState is Config Mode. More... | |
void | pollingWorker () |
Worker threads for polling the sensors. More... | |
bool | readDevice (RxFrame &frame) |
Reads a raw measurement frame from the serial-port. More... | |
bool | sendCalibrationMatrixEntry (const uint8_t subId, const double entry) |
Sends a calibration matrix entry to device. More... | |
bool | startPollingThread () |
This method starts up the polling thread. More... | |
timespec | timespecAdd (timespec a, timespec b) |
Calculates the sum of two timespecs. More... | |
double | timespecToSec (timespec t) |
Converts a timespec to seconds. More... | |
bool | writeSerial (const std::string &str) |
Writes an Ascii string to the serial device. More... | |
Private Attributes | |
std::uint32_t | baudRate_ |
The baud rate of the serial communication. More... | |
boost::atomic< ConnectionState > | connectionState_ |
Internal connection state. More... | |
boost::thread | connectionThread_ |
Connection thread handle. More... | |
boost::atomic< ErrorState > | errorState_ |
Internal error state. More... | |
RxFrame | frame_ |
The internal variable for the receiving frame. This variable represents the raw data received from the sensor. More... | |
unsigned long | frameCrcErrorCounter_ |
Counter for frames with CRC errors. More... | |
uint8_t | frameHeader = 0xAA |
The frame header value. More... | |
unsigned long | frameReceivedCounter_ |
Received frame counter. More... | |
unsigned long | frameSuccessCounter_ |
Correct frames counter. More... | |
boost::atomic< bool > | frameSync_ |
Flag that indicates if the frame is synced. More... | |
unsigned int | frameSyncErrorCounter_ |
Frame sync errors. More... | |
boost::atomic< bool > | isRunning_ |
Internal flags/indicators. More... | |
unsigned int | maxFrameSyncErrorCounts_ |
Maximum acceptable frame sync errors. More... | |
boost::atomic< ModeState > | modeState_ |
Mode state of the sensor. More... | |
std::string | name_ |
Name of the sensor. More... | |
unsigned long | pollingSyncErrorCounter_ |
Internal statistics and error counters. More... | |
boost::thread | pollingThread_ |
Polling thread handle. More... | |
double | pollingThreadTimeStep_ |
If setup, the sensor polling thread will poll at this rate. More... | |
std::string | port_ |
The serial port to connect to. More... | |
std::recursive_mutex | readingMutex_ |
Mutex prohibiting simultaneous access the internal Reading variable. More... | |
bool | runInThreadedMode_ |
Flag to indicate whether the driver should setup worker threads at startup. More... | |
Reading | serialImplReading_ |
The internal reading variable. It's used for providing to client code the sensor readings, through the getReading () function. More... | |
std::recursive_mutex | serialMutex_ |
Mutex prohibiting simultaneous access to Serial device. More... | |
double | timeStampSecs_ |
Sample time-stamp. More... | |
boost::atomic< int > | usbFileDescriptor_ |
The USB file descriptor. More... | |
std::ifstream | usbStreamIn_ |
Input stream for the USB file descriptor. More... | |
std::ofstream | usbStreamOut_ |
Output stream for the USB file descriptor. More... | |
bool | useDeviceTimeStamps_ |
Flag to indicate whether time-stamps should be generated using sensor or system time. More... | |
Static Private Attributes | |
static const uint64_t | NSEC_PER_SEC = 1000000000 |
The Rokubimini Serial Implementation class.
It's the implementation in the BRIDGE pattern used. It provides the implementation to be called by the interface (RokubiminiSerial) in order to communicate with the Serial Device.
Definition at line 113 of file RokubiminiSerialImpl.hpp.
|
private |
Definition at line 565 of file RokubiminiSerialImpl.hpp.
|
delete |
Default constructor is deleted.
rokubimini::serial::RokubiminiSerialImpl::RokubiminiSerialImpl | ( | const std::string & | name, |
const std::string & | port, | ||
const std::uint32_t & | baudRate | ||
) |
Custom constructor for the RokubiminiSerialImpl class.
name | The name of the device. |
port | The port to connect to. |
baudRate | The baud rate of the serial communication. |
Definition at line 33 of file RokubiminiSerialImpl.cpp.
|
default |
|
inlineprivate |
Definition at line 518 of file RokubiminiSerialImpl.cpp.
|
private |
Connects to the Serial-over-USB port.
Definition at line 138 of file RokubiminiSerialImpl.cpp.
|
private |
Connects to the Serial-over-USB port.
port | The port to connect to. |
Definition at line 178 of file RokubiminiSerialImpl.cpp.
|
private |
Worker threads for managing sensor connections.
Definition at line 536 of file RokubiminiSerialImpl.cpp.
|
private |
Implementation function of the CRC16 X25 checksum for the input data.
data | The input data. |
crc | The current checksum. |
Definition at line 526 of file RokubiminiSerialImpl.cpp.
Calculates the difference (b-a) of two timespecs in seconds.
a | The first timespec. |
b | The second timespec. |
Definition at line 577 of file RokubiminiSerialImpl.hpp.
bool rokubimini::serial::RokubiminiSerialImpl::firmwareUpdate | ( | const std::string & | filePath | ) |
Updates the firmware of the device.
filePath | The path to find the firmware file |
Definition at line 1025 of file RokubiminiSerialImpl.cpp.
|
private |
Returns the system definition for the given baudRate.
baudRate | The baudRate to find the definition for. |
Definition at line 371 of file RokubiminiSerialImpl.cpp.
|
private |
Retreive sensor state/status.
Gets the current connection status.
Definition at line 310 of file RokubiminiSerialImpl.cpp.
|
private |
Gets the current error status.
Definition at line 315 of file RokubiminiSerialImpl.cpp.
|
private |
Retrieves detailed error indication.
Definition at line 320 of file RokubiminiSerialImpl.cpp.
|
inline |
Gets the force torque sampling rate of the device.
samplingRate | The force torque sampling rate to be fetched. |
Definition at line 269 of file RokubiminiSerialImpl.hpp.
|
inline |
Accessor for device name.
Definition at line 234 of file RokubiminiSerialImpl.hpp.
void rokubimini::serial::RokubiminiSerialImpl::getReading | ( | rokubimini::Reading & | reading | ) |
Gets the internal reading variable.
reading | The variable to store the reading. |
Definition at line 638 of file RokubiminiSerialImpl.cpp.
|
inline |
Accessor for device serial number.
serialNumber | The serial number to get. |
Definition at line 252 of file RokubiminiSerialImpl.hpp.
|
private |
Checks the connection has errors.
Definition at line 300 of file RokubiminiSerialImpl.cpp.
|
inline |
Returns if there is frame synchronization with the device.
Definition at line 520 of file RokubiminiSerialImpl.hpp.
bool rokubimini::serial::RokubiminiSerialImpl::init | ( | ) |
This method initializes internal variables and the device for communication. It connects to the serial-port.
Definition at line 62 of file RokubiminiSerialImpl.cpp.
|
private |
Initializatin-time helper functions.
Initializes communication with the sensor.
keepConnecting | Set if there will be multiple attempts or only one. |
Definition at line 346 of file RokubiminiSerialImpl.cpp.
|
private |
Sets up and initializes the serial port for communication.
port | The port to initialize. |
Definition at line 440 of file RokubiminiSerialImpl.cpp.
|
private |
Quick sensor/state testers.
Checks if the device is connected.
Definition at line 290 of file RokubiminiSerialImpl.cpp.
|
private |
Checks if device is already in the process of connecting.
Definition at line 295 of file RokubiminiSerialImpl.cpp.
|
private |
Checks if the ModeState is Config Mode.
Definition at line 305 of file RokubiminiSerialImpl.cpp.
|
inline |
Returns if the serial driver is running.
Definition at line 498 of file RokubiminiSerialImpl.hpp.
bool rokubimini::serial::RokubiminiSerialImpl::loadConfig | ( | ) |
Loads the configuration of the device.
Definition at line 921 of file RokubiminiSerialImpl.cpp.
|
private |
Worker threads for polling the sensors.
Definition at line 541 of file RokubiminiSerialImpl.cpp.
bool rokubimini::serial::RokubiminiSerialImpl::printUserConfig | ( | ) |
Prints all the user configurable parameters.
Definition at line 939 of file RokubiminiSerialImpl.cpp.
|
private |
Reads a raw measurement frame from the serial-port.
frame | The raw measurement frame to read. |
Definition at line 184 of file RokubiminiSerialImpl.cpp.
bool rokubimini::serial::RokubiminiSerialImpl::saveConfigParameter | ( | ) |
Saves the current configuration to the device.
Definition at line 903 of file RokubiminiSerialImpl.cpp.
|
inlineprivate |
Sends a calibration matrix entry to device.
subId | The sub-index of the SDO to write to. |
entry | The entry on the matrix. |
Definition at line 560 of file RokubiminiSerialImpl.hpp.
|
inline |
Sets an acceleration filter to the device.
filter | The filter to be set. |
Definition at line 299 of file RokubiminiSerialImpl.hpp.
|
inline |
Sets an acceleration range to the device.
range | The range to be set. |
Definition at line 331 of file RokubiminiSerialImpl.hpp.
|
inline |
Sets an angular rate filter to the device.
filter | The filter to be set. |
Definition at line 315 of file RokubiminiSerialImpl.hpp.
|
inline |
Sets an angular rate range to the device.
range | The range to be set. |
Definition at line 347 of file RokubiminiSerialImpl.hpp.
bool rokubimini::serial::RokubiminiSerialImpl::setCommunicationSetup | ( | const configuration::SensorConfiguration & | sensorConfiguration, |
const uint8_t & | dataFormat, | ||
const uint32_t & | baudRate | ||
) |
Sets communication setup for the device. This includes setting the temperature compensation, the matrix calibration, the data format and the baud rate.
sensorConfiguration | The sensor configuration to be set. |
dataFormat | The data format (binary = 0, CSV = 1). |
baudRate | The desired baud rate (see user manual). |
Definition at line 864 of file RokubiminiSerialImpl.cpp.
bool rokubimini::serial::RokubiminiSerialImpl::setConfigMode | ( | ) |
Sets the device in config mode.
Definition at line 665 of file RokubiminiSerialImpl.cpp.
bool rokubimini::serial::RokubiminiSerialImpl::setForceTorqueFilter | ( | const configuration::ForceTorqueFilter & | filter | ) |
Sets a force torque filter to the device.
filter | The filter to be set. |
Definition at line 774 of file RokubiminiSerialImpl.cpp.
bool rokubimini::serial::RokubiminiSerialImpl::setForceTorqueOffset | ( | const Eigen::Matrix< double, 6, 1 > & | forceTorqueOffset | ) |
Sets a force torque offset to the device.
forceTorqueOffset | The offset to be set. |
Definition at line 800 of file RokubiminiSerialImpl.cpp.
bool rokubimini::serial::RokubiminiSerialImpl::setHardwareReset | ( | ) |
Triggers a hardware reset of the sensor.
Definition at line 723 of file RokubiminiSerialImpl.cpp.
bool rokubimini::serial::RokubiminiSerialImpl::setInitMode | ( | ) |
Triggers a software reset of the sensor bringing it to a known state.
Definition at line 738 of file RokubiminiSerialImpl.cpp.
|
inline |
Sets the time step of the polling thread in seconds. timeStep The time step in seconds.
Definition at line 509 of file RokubiminiSerialImpl.hpp.
bool rokubimini::serial::RokubiminiSerialImpl::setRunMode | ( | ) |
Sets the device in run mode.
Definition at line 701 of file RokubiminiSerialImpl.cpp.
bool rokubimini::serial::RokubiminiSerialImpl::setSensorCalibration | ( | const calibration::SensorCalibration & | sensorCalibration | ) |
Sets a sensor calibration to the device.
sensorCalibration | The calibration to be set. |
Definition at line 821 of file RokubiminiSerialImpl.cpp.
bool rokubimini::serial::RokubiminiSerialImpl::setSensorConfiguration | ( | const configuration::SensorConfiguration & | sensorConfiguration | ) |
Sets a sensor configuration to the device.
sensorConfiguration | The configuration to be set. |
Definition at line 852 of file RokubiminiSerialImpl.cpp.
|
inline |
Sets the state of the device in the bus (unused).
state | Desired state. |
Definition at line 208 of file RokubiminiSerialImpl.hpp.
void rokubimini::serial::RokubiminiSerialImpl::shutdown | ( | ) |
Shuts down the device. It automatically shuts-down threads and disconnects from serial-port.
Definition at line 184 of file RokubiminiSerialImpl.hpp.
|
private |
This method starts up the polling thread.
Definition at line 86 of file RokubiminiSerialImpl.cpp.
bool rokubimini::serial::RokubiminiSerialImpl::startup | ( | ) |
This method starts up communication with the device.
Definition at line 98 of file RokubiminiSerialImpl.cpp.
|
inlineprivate |
Calculates the sum of two timespecs.
a | The first timespec. |
b | The second timespec. |
Definition at line 606 of file RokubiminiSerialImpl.hpp.
|
inlineprivate |
Converts a timespec to seconds.
t | The timespec. |
Definition at line 591 of file RokubiminiSerialImpl.hpp.
|
inline |
This method is called by the BusManager. Each device attached to this bus reads its data from the buffer (not used).
Definition at line 173 of file RokubiminiSerialImpl.hpp.
|
inline |
This method is called by the BusManager. Each device attached to the bus writes its data to the buffer (not used).
Definition at line 184 of file RokubiminiSerialImpl.hpp.
|
inline |
Wait for a state to be reached (unused).
state | Desired state. |
Definition at line 220 of file RokubiminiSerialImpl.hpp.
|
private |
Writes an Ascii string to the serial device.
str | The string to write. |
Definition at line 982 of file RokubiminiSerialImpl.cpp.
|
private |
The baud rate of the serial communication.
Definition at line 865 of file RokubiminiSerialImpl.hpp.
|
private |
Internal connection state.
Definition at line 1013 of file RokubiminiSerialImpl.hpp.
|
private |
Connection thread handle.
Definition at line 995 of file RokubiminiSerialImpl.hpp.
|
private |
Internal error state.
Definition at line 1021 of file RokubiminiSerialImpl.hpp.
|
private |
The internal variable for the receiving frame. This variable represents the raw data received from the sensor.
Definition at line 945 of file RokubiminiSerialImpl.hpp.
|
private |
Counter for frames with CRC errors.
Definition at line 1074 of file RokubiminiSerialImpl.hpp.
|
private |
The frame header value.
Definition at line 935 of file RokubiminiSerialImpl.hpp.
|
private |
Received frame counter.
Definition at line 1056 of file RokubiminiSerialImpl.hpp.
|
private |
Correct frames counter.
Definition at line 1065 of file RokubiminiSerialImpl.hpp.
|
private |
Flag that indicates if the frame is synced.
Definition at line 908 of file RokubiminiSerialImpl.hpp.
|
private |
Frame sync errors.
Definition at line 1082 of file RokubiminiSerialImpl.hpp.
|
private |
Internal flags/indicators.
Internal flag to indicate if the threads are running.
Definition at line 1038 of file RokubiminiSerialImpl.hpp.
|
private |
Maximum acceptable frame sync errors.
Definition at line 1090 of file RokubiminiSerialImpl.hpp.
|
private |
Mode state of the sensor.
Definition at line 1030 of file RokubiminiSerialImpl.hpp.
|
private |
Name of the sensor.
Definition at line 836 of file RokubiminiSerialImpl.hpp.
|
staticprivate |
Definition at line 1097 of file RokubiminiSerialImpl.hpp.
|
private |
Internal statistics and error counters.
Synchronization error counter.
Definition at line 1048 of file RokubiminiSerialImpl.hpp.
|
private |
Polling thread handle.
Definition at line 1004 of file RokubiminiSerialImpl.hpp.
|
private |
If setup, the sensor polling thread will poll at this rate.
Definition at line 976 of file RokubiminiSerialImpl.hpp.
|
private |
The serial port to connect to.
Definition at line 856 of file RokubiminiSerialImpl.hpp.
|
mutableprivate |
Mutex prohibiting simultaneous access the internal Reading variable.
Definition at line 874 of file RokubiminiSerialImpl.hpp.
|
private |
Flag to indicate whether the driver should setup worker threads at startup.
Definition at line 966 of file RokubiminiSerialImpl.hpp.
|
private |
The internal reading variable. It's used for providing to client code the sensor readings, through the getReading () function.
Definition at line 847 of file RokubiminiSerialImpl.hpp.
|
mutableprivate |
Mutex prohibiting simultaneous access to Serial device.
Definition at line 883 of file RokubiminiSerialImpl.hpp.
|
private |
Sample time-stamp.
Definition at line 957 of file RokubiminiSerialImpl.hpp.
|
private |
The USB file descriptor.
Definition at line 899 of file RokubiminiSerialImpl.hpp.
|
private |
Input stream for the USB file descriptor.
Definition at line 917 of file RokubiminiSerialImpl.hpp.
|
private |
Output stream for the USB file descriptor.
Definition at line 926 of file RokubiminiSerialImpl.hpp.
|
private |
Flag to indicate whether time-stamps should be generated using sensor or system time.
Definition at line 986 of file RokubiminiSerialImpl.hpp.