imuBosch_BNO055.h

Go to the documentation of this file.

/*
 * SPDX-FileCopyrightText: 2006-2021 Istituto Italiano di Tecnologia (IIT)
 * SPDX-License-Identifier: LGPL-2.1-or-later
 */
 
#ifndef BOSCH_IMU_DEVICE
#define BOSCH_IMU_DEVICE
 
#include <atomic>
#include <yarp/sig/Vector.h>
#include <yarp/os/PeriodicThread.h>
#include <yarp/dev/PolyDriver.h>
#include <yarp/os/ResourceFinder.h>
#include <yarp/dev/ISerialDevice.h>
#include <yarp/dev/IGenericSensor.h>
#include <yarp/dev/MultipleAnalogSensorsInterfaces.h>
#include <yarp/math/Quaternion.h>
#include <mutex>
 
#include "BoschIMU_ParamsParser.h"
 
/* Serial protocol description
 *
 *  Write operation on a register:
 * | Byte 1 |  Byte 2 |  Byte 3  | Byte 4 |  Byte 5 |  ...  |  Byte (n+4) |
 * | Start  |  Write  |  Reg Addr| Length |  Data 1 |  ...  |   Data n    |
 * |  0xAA  |   0x00  |  <...>   | <...>  |  <...>  |  ...  |    <...>    |
 *
 * Response to write command:
 * | 0xEE   | <code>  |
 * 0x01: Write_success
 * all other values are errors ... TODO: improve doc
 *
 *  Read operation on a register:
 * | Byte 1 |  Byte 2 |  Byte 3  | Byte 4 |
 * | Start  |  Read   |  Reg Addr| Length |
 * |  0xAA  |  0x01   |  <...>   | <...>  |
 *
 * Response to a successful read command:
 * | Byte 1 |  Byte 2 |  Byte 3 |  ...  |  Byte (n+2) |
 * |  Resp  |  Length |  Data 1 |  ...  |   Data n    |
 * |  0xBB  |  <...>  |  <...>  |  ...  |    <...>    |
 *
 * Response to a failed read command:
 * | Byte 1 |  Byte 2 |
 * |  Resp  |  Status |
 * |  0xEE  |  <...>  |
 *
 * Read error code:  TODO
 */
 
constexpr int MAX_MSG_LENGTH = 128;
 
// Commands
#define START_BYTE  0xAA
#define WRITE_CMD   0x00
#define READ_CMD    0x01
 
// Responses
#define REPLY_HEAD  0xBB
#define ERROR_HEAD  0xEE
#define WRITE_SUCC  0x01
#define READ_FAIL   0x02
#define WRITE_FAIL  0x03
 
// Error code
#define REGISTER_NOT_READY  0x07
 
// Registers
// Page 0       // Device has 2 pages of registers
#define REG_CHIP_ID         0x00
#define REG_SW_VERSION      0x04    // 2 software revision bytes
#define REG_BOOTLOADER      0x06    // 1 byte  bootloader version
#define REG_PAGE_ID         0x07    // page ID number
 
#define REG_ACC_DATA        0x08    // 3*2 bytes: LSB first (LSB 0x08, MSB 0x09) for X
#define REG_MAGN_DATA       0x0E    // 3*2 bytes: LSB first
#define REG_GYRO_DATA       0x14    // 3*2 bytes: LSB first
#define REG_RPY_DATA        0x1A    // 3*2 bytes: LSB first  (raw order is Yaw, Roll, Pitch)
#define REG_QUATERN_DATA    0x20    // 4*2 bytes: LSB first  (raw order is w, x, y, z)
#define REG_GRAVITY         0x2E    // Gravity Vector data
#define REG_CALIB_STATUS    0x35    // Check if sensors are calibrated, 2 bits each. SYS - GYRO - ACC - MAG. 3 means calibrated, 0 not calbrated
#define REG_SYS_CLK_STATUS  0x38    // only 1 last LSB
#define REG_SYS_STATUS      0x39
#define REG_SYS_ERR         0x3A
#define REG_UNIT_SEL        0x3B
#define REG_OP_MODE         0x3D
#define REG_POWER_MODE      0x3E
#define REG_SYS_TRIGGER     0x3F
 
// Values
#define CONFIG_MODE         0x00
#define AMG_MODE            0x07
#define IMU_MODE            0x08
#define M4G_MODE            0x0A
#define NDOF_MODE           0x0C
 
// Sys trigger values (in OR if more than one is to be activated)
#define TRIG_EXT_CLK_SEL    0x80    // 1 for external clock (if available), 0 for internal clock
#define TRIG_RESET_INT      0x40    // reset interrupts
#define TRIG_RESET_SYSTEM   0x20    // reset system
#define TRIG_SELF_TEST      0x01    // Start self test
 
#define BNO055_ID           0xA0
 
#define RESP_HEADER_SIZE                 2
// Time to wait while switching to and from config_mode & any operation_mode
#define SWITCHING_TIME                   0.020  //   20ms
#define TIME_REPORT_INTERVAL             30
//number of attempts of sending config command
#define ATTEMPTS_NUM_OF_SEND_CONFIG_CMD  3
 
 
 
class BoschIMU:
        public yarp::dev::DeviceDriver,
        public yarp::os::PeriodicThread,
        public yarp::dev::IGenericSensor,
        public yarp::dev::IThreeAxisGyroscopes,
        public yarp::dev::IThreeAxisLinearAccelerometers,
        public yarp::dev::IThreeAxisMagnetometers,
        public yarp::dev::IOrientationSensors,
        public BoschIMU_ParamsParser
{
protected:
    bool                        verbose;              
    short                       status;               
    yarp::sig::Vector           data;                 
    yarp::sig::Vector           data_tmp;             
    yarp::math::Quaternion      quaternion;           
    yarp::math::Quaternion      quaternion_tmp;       
    yarp::sig::Vector           RPY_angle;            
    double                      m_timeStamp;          
    double                      timeLastReport;       
    mutable std::mutex             mutex;        
    bool                        m_i2c_flag=false;     
 
    bool                        checkError;           
 
    int                         fd;                   
    size_t                      responseOffset;
    yarp::os::ResourceFinder    rf;                   
 
    using ReadFuncPtr = bool (BoschIMU::*)(unsigned char, int, unsigned char*, std::string);  
    ReadFuncPtr readFunc;                             
 
    unsigned char command[MAX_MSG_LENGTH];            
    unsigned char response[MAX_MSG_LENGTH];           
 
 
    bool checkWriteResponse(unsigned char *response);
    bool checkReadResponse(unsigned char *response);
 
    void printBuffer(unsigned char *buffer, int length);
    int  readBytes(unsigned char *buffer, int bytes);
    void dropGarbage();
 
    long int           totMessagesRead;
    yarp::sig::Vector  errorCounter;
    yarp::sig::Vector  errorReading;
 
    void readSysError();
    // Serial
    bool sendReadCommandSer(unsigned char register_add, int len, unsigned char* buf, std::string comment = "");
    bool sendWriteCommandSer(unsigned char register_add, int len, unsigned char* cmd, std::string comment = "");
    bool sendAndVerifyCommandSer(unsigned char register_add, int len, unsigned char* cmd, std::string comment);
 
    // i2c
    bool sendReadCommandI2c(unsigned char register_add, int len, unsigned char* buf, std::string comment = "");
 
    int errs;
    std::atomic<bool> dataIsValid;
 
public:
    BoschIMU();
 
    ~BoschIMU();
 
    bool open(yarp::os::Searchable& config) override;
 
    bool close() override;
 
    bool read(yarp::sig::Vector &out) override;
 
    bool getChannels(int *nc) override;
 
    bool calibrate(int ch, double v) override;
 
    /* IThreeAxisGyroscopes methods */
    size_t getNrOfThreeAxisGyroscopes() const override;
 
    yarp::dev::MAS_status getThreeAxisGyroscopeStatus(size_t sens_index) const override;
 
    bool getThreeAxisGyroscopeName(size_t sens_index, std::string &name) const override;
 
    bool getThreeAxisGyroscopeFrameName(size_t sens_index, std::string &frameName) const override;
 
    bool getThreeAxisGyroscopeMeasure(size_t sens_index, yarp::sig::Vector& out, double& timestamp) const override;
 
    /* IThreeAxisLinearAccelerometers methods */
    size_t getNrOfThreeAxisLinearAccelerometers() const override;
 
    yarp::dev::MAS_status getThreeAxisLinearAccelerometerStatus(size_t sens_index) const override;
 
    bool getThreeAxisLinearAccelerometerName(size_t sens_index, std::string &name) const override;
 
    bool getThreeAxisLinearAccelerometerFrameName(size_t sens_index, std::string &frameName) const override;
 
    bool getThreeAxisLinearAccelerometerMeasure(size_t sens_index, yarp::sig::Vector& out, double& timestamp) const override;
 
    /* IThreeAxisMagnetometers methods */
    size_t getNrOfThreeAxisMagnetometers() const override;
 
    yarp::dev::MAS_status getThreeAxisMagnetometerStatus(size_t sens_index) const override;
 
    bool getThreeAxisMagnetometerName(size_t sens_index, std::string &name) const override;
 
    bool getThreeAxisMagnetometerFrameName(size_t sens_index, std::string &frameName) const override;
 
    bool getThreeAxisMagnetometerMeasure(size_t sens_index, yarp::sig::Vector& out, double& timestamp) const override;
 
    /* IOrientationSensors methods */
    size_t getNrOfOrientationSensors() const override;
 
    yarp::dev::MAS_status getOrientationSensorStatus(size_t sens_index) const override;
 
    bool getOrientationSensorName(size_t sens_index, std::string &name) const override;
 
    bool getOrientationSensorFrameName(size_t sens_index, std::string &frameName) const override;
 
    bool getOrientationSensorMeasureAsRollPitchYaw(size_t sens_index, yarp::sig::Vector& rpy, double& timestamp) const override;
 
    bool threadInit() override;
 
 
    void threadRelease() override;
 
    void run() override;
 
private:
    yarp::dev::MAS_status genericGetStatus(size_t sens_index) const;
    bool genericGetSensorName(size_t sens_index, std::string &name) const;
    bool genericGetFrameName(size_t sens_index, std::string &frameName) const;
 
};
 
 
#endif  // BOSCH_IMU_DEVICE