16#include <linux/i2c-dev.h>
19# include <i2c/smbus.h>
22#include <linux/kernel.h>
55 readFunc(&
BoschIMU::sendReadCommandSer),
64 errorCounter.resize(11);
80 yCError(
IMUBOSCH_BNO055) <<
"Params 'comport' and 'i2c' not found. At least one of the two must be specified";
135 options.c_cflag &=
~CSIZE;
136 options.c_cflag |=
CS8;
150 options.c_cc[
VMIN] = 0;
151 options.c_cc[
VTIME] = 2;
193 "Inertial sensor didn't understand the command. \n\
194 If this error happens more than once in a row, please check serial communication is working fine and it isn't affected by electrical disturbance.");
203 "Received unknown response message. \n\
204 If this error happens more than once in a row, please check serial communication is working fine and it isn't affected by electrical disturbance.");
221 "Inertial sensor didn't understand the command. \n\
222 If this error happens more than once in a row, please check serial communication is working fine and it isn't affected by electrical disturbance.");
231 "Received unknown response message. \n\
232 If this error happens more than once in a row, please check serial communication is working fine and it isn't affected by electrical disturbance.");
246 bool success =
false;
307 for (
int i = 0; i < len; i++) {
358 while( (
::read(
fd, (
void*) &
byte, 1) > 0 ))
367 for (
int i = 0; i < length; i++) {
563 for(
int i=0; i<10; i++)
604 for(
int i=0; i<16; i++)
716 yCError(
IMUBOSCH_BNO055) <<
"sens_index must be equal to 0, since there is only one sensor in consideration";
727 yCError(
IMUBOSCH_BNO055) <<
"sens_index must be equal to 0, since there is only one sensor in consideration";
739 yCError(
IMUBOSCH_BNO055) <<
"sens_index must be equal to 0, since there is only one sensor in consideration";
762 return genericGetSensorName(
sens_index, name);
767 return genericGetFrameName(
sens_index, frameName);
774 yCError(
IMUBOSCH_BNO055) <<
"sens_index must be equal to 0, since there is only one sensor in consideration";
802 return genericGetSensorName(
sens_index, name);
807 return genericGetFrameName(
sens_index, frameName);
814 yCError(
IMUBOSCH_BNO055) <<
"sens_index must be equal to 0, since there is only one sensor in consideration";
841 return genericGetSensorName(
sens_index, name);
846 return genericGetFrameName(
sens_index, frameName);
853 yCError(
IMUBOSCH_BNO055) <<
"sens_index must be equal to 0, since there is only one sensor in consideration";
880 return genericGetSensorName(
sens_index, name);
885 return genericGetFrameName(
sens_index, frameName);
892 yCError(
IMUBOSCH_BNO055) <<
"sens_index must be equal to 0, since there is only one sensor in consideration";
899 out[0] =
data[9] / 1000000;
900 out[1] =
data[10]/ 1000000;
901 out[2] =
data[11]/ 1000000;
#define ReturnValue_error_input_out_of_bounds
std::string m_sensor_name
bool parseParams(const yarp::os::Searchable &config) override
Parse the DeviceDriver parameters.
imuBosch_BNO055: This device will connect to the proper analogServer and read the data broadcasted ma...
void threadRelease() override
Terminate communication with the device and release the thread.
unsigned char command[MAX_MSG_LENGTH]
packet to be written to the device
yarp::dev::ReturnValue getThreeAxisLinearAccelerometerFrameName(size_t sens_index, std::string &frameName) const override
Get the name of the frame in which three axis linear accelerometer measurements are expressed.
bool sendReadCommandSer(unsigned char register_add, int len, unsigned char *buf, std::string comment="")
std::atomic< bool > dataIsValid
std::mutex mutex
mutex to avoid resource clash
bool checkReadResponse(unsigned char *response)
yarp::dev::ReturnValue getNrOfThreeAxisLinearAccelerometers(size_t &num) const override
Get the number of three axis linear accelerometers in the device.
ReadFuncPtr readFunc
Functor object.
yarp::math::Quaternion quaternion_tmp
orientation in quaternion representation
void printBuffer(unsigned char *buffer, int length)
bool checkError
flag to check read error of sensor data
yarp::sig::Vector data
sensor data buffer
bool getChannels(int *nc) override
Get the number of channels of the sensor.
yarp::dev::ReturnValue getThreeAxisGyroscopeFrameName(size_t sens_index, std::string &frameName) const override
Get the name of the frame in which three axis gyroscope measurements are expressed.
bool sendReadCommandI2c(unsigned char register_add, int len, unsigned char *buf, std::string comment="")
yarp::dev::ReturnValue getNrOfThreeAxisMagnetometers(size_t &num) const override
Get the number of three axis magnetometers in the device.
yarp::dev::ReturnValue getOrientationSensorName(size_t sens_index, std::string &name) const override
Get the name of orientation sensor.
yarp::dev::ReturnValue getThreeAxisLinearAccelerometerName(size_t sens_index, std::string &name) const override
Get the name of three axis linear accelerometer.
bool close() override
Close the device.
double m_timeStamp
device timestamp
unsigned char response[MAX_MSG_LENGTH]
packet to be read from the device
yarp::dev::ReturnValue getThreeAxisMagnetometerName(size_t sens_index, std::string &name) const override
Get the name of three axis magnetometer.
bool threadInit() override
Initialize process with desired device configurations.
yarp::dev::ReturnValue getOrientationSensorMeasureAsRollPitchYaw(size_t sens_index, yarp::sig::Vector &rpy, double ×tamp) const override
Get orientation sensor measurements.
bool sendAndVerifyCommandSer(unsigned char register_add, int len, unsigned char *cmd, std::string comment)
yarp::dev::ReturnValue getThreeAxisLinearAccelerometerMeasure(size_t sens_index, yarp::sig::Vector &out, double ×tamp) const override
Get three axis linear accelerometer measurements.
bool checkWriteResponse(unsigned char *response)
yarp::dev::ReturnValue getThreeAxisMagnetometerMeasure(size_t sens_index, yarp::sig::Vector &out, double ×tamp) const override
Get three axis magnetometer measurements.
bool calibrate(int ch, double v) override
Calibrate the sensor, single channel.
yarp::sig::Vector errorCounter
yarp::dev::MAS_status getOrientationSensorStatus(size_t sens_index) const override
Get the status of orientation sensor.
yarp::dev::ReturnValue getThreeAxisMagnetometerFrameName(size_t sens_index, std::string &frameName) const override
Get the name of the frame in which three axis magnetometer measurements are expressed.
bool read(yarp::sig::Vector &out) override
Read a vector from the sensor.
yarp::dev::ReturnValue getNrOfThreeAxisGyroscopes(size_t &num) const override
Get the number of three axis gyroscopes in the device.
yarp::dev::MAS_status getThreeAxisGyroscopeStatus(size_t sens_index) const override
Get the status of three axis gyroscope.
yarp::math::Quaternion quaternion
orientation in quaternion representation
bool sendWriteCommandSer(unsigned char register_add, int len, unsigned char *cmd, std::string comment="")
yarp::dev::MAS_status getThreeAxisMagnetometerStatus(size_t sens_index) const override
Get the status of three axis magnetometer.
double timeLastReport
timestamp of last reported data
int readBytes(unsigned char *buffer, int bytes)
int fd
file descriptor to open device at system level
bool m_i2c_flag
flag to check if device connected through i2c commununication
bool open(yarp::os::Searchable &config) override
Open the device and set up parameters/communication.
yarp::dev::ReturnValue getNrOfOrientationSensors(size_t &num) const override
Get the number of orientation sensors in the device.
yarp::sig::Vector data_tmp
sensor data temporary buffer
yarp::sig::Vector RPY_angle
orientation in Euler angle representation
yarp::dev::ReturnValue getThreeAxisGyroscopeName(size_t sens_index, std::string &name) const override
Get the name of three axis gyroscope.
yarp::dev::MAS_status getThreeAxisLinearAccelerometerStatus(size_t sens_index) const override
Get the status of three axis linear accelerometer.
yarp::dev::ReturnValue getOrientationSensorFrameName(size_t sens_index, std::string &frameName) const override
Get the name of the frame in which orientation sensor measurements are expressed.
yarp::dev::ReturnValue getThreeAxisGyroscopeMeasure(size_t sens_index, yarp::sig::Vector &out, double ×tamp) const override
Get three axis gyroscope measurements.
void run() override
Update loop where measurements are read from the device.
yarp::sig::Matrix toRotationMatrix4x4() const
Converts a quaternion to a rotation matrix.
A mini-server for performing network communication in the background.
An abstraction for a periodic thread.
bool setPeriod(double period)
Set the (new) period of the thread.
bool start()
Call this to start the thread.
void stop()
Call this to stop the thread, this call blocks until the thread is terminated (and releaseThread() ca...
A base class for nested structures that can be searched.
virtual std::string toString() const =0
Return a standard text representation of the content of the object.
static double nowSystem()
static void delaySystem(double seconds)
void zero()
Set to zero the elements of the vector.
void resize(size_t size)
Resize the vector.
#define ATTEMPTS_NUM_OF_SEND_CONFIG_CMD
#define REGISTER_NOT_READY
#define TIME_REPORT_INTERVAL
#define yCError(component,...)
#define yCTrace(component,...)
#define yCDebug(component,...)
#define YARP_LOG_COMPONENT(name,...)
For streams capable of holding different kinds of content, check what they actually have.
MAS_status
Status of a given analog sensor exposed by a multiple analog sensors interface.
@ MAS_ERROR
The sensor is in generic error state.
@ MAS_OK
The sensor is working correctly.
yarp::sig::Vector dcm2rpy(const yarp::sig::Matrix &R)
Converts a dcm (direction cosine matrix) rotation matrix to roll-pitch-yaw angles (defined in Math....
void delay(double seconds)
Wait for a certain number of seconds.
An interface to the operating system, including Port based communication.