sensor::imu::LaeImu Class Reference

#include <laeImu.h>

Inheritance diagram for sensor::imu::LaeImu:

Public Member Functions
	LaeImu (std::string strIdent="Virtual IMU")
	Default constructor. More...
virtual	~LaeImu ()
	Destructor.
virtual int	open (const std::string &strDevName, int nBaudRate)
virtual int	close ()
	Close connection to motor controller. More...
virtual bool	isOpen ()
	Check if IMU serial interface is open. More...
virtual void	blacklist ()
	Black list IMU from robot sensors.
virtual void	whitelist ()
	White list IMU sensor.
virtual bool	isBlackListed ()
	Test if IMU is black listed. More...
virtual void	clearSensedData ()
	Clear IMU sensed data.
virtual int	numDoFs ()
	Get the total IMU degress of freedom. More...
virtual bool	hasAccelerometer ()
	Test if IMU has an accelerometer. More...
virtual bool	hasGyroscope ()
	Test if IMU has an gyroscope. More...
virtual bool	hasMagnetometer ()
	Test if IMU has an magnetometer. More...
virtual int	configure (const laelaps::LaeDesc &desc)
	Configure IMU from product description. More...
virtual int	configure (const laelaps::LaeTunes &tunes)
	Configure IMU from tunable parameters. More...
virtual int	reload (const laelaps::LaeTunes &tunes)
	Reload with new tuning parameters. More...
virtual int	readIdentity (std::string &strIdent)=0
	Read sensor identity values. More...
virtual int	readRawImu ()=0
	Read sensor raw IMU values. More...
virtual int	convertRawToSI ()=0
	Convert last read IMU values to International System of Units. More...
virtual void	compute ()
	Compute all IMU values form converted, raw values. More...
virtual void	computeQuaternion ()
	Compute the quaternion from the IMU data.
virtual void	computeDynamics ()
	Compute the velocity, position, and any other dynamics from the IMU data. More...
virtual void	exec ()
	Exectute one step to read, convert, and compute IMU values.
std::string	getIdentity ()
	Get IMU identity. More...
std::string	getDevName ()
	Get IMU device name. More...
virtual void	getRawInertiaData (int accel[], int gyro[])
	Get the last read raw inertia data. More...
virtual void	getInertiaData (double accel[], double gyro[])
	Get the last read and converted inertia data. More...
virtual void	getMagnetometerData (double mag[])
	Get the last read magnetometer values. More...
virtual void	getAttitude (double rpy[])
	Get the last read IMU (vehicle) attitude. More...
virtual void	getAttitude (double &roll, double &pitch, double &yaw)
	Get the last read IMU (vehicle) attitude. More...
virtual void	getQuaternion (Quaternion &q)
	Get the last computed quaternion. More...
virtual void	getImuData (double accel[], double gyro[], double mag[], double rpy[], Quaternion &q)
	Get the last sensed, converted, and computed IMU data. More...

Protected Member Functions
void	lockIo ()
	Lock the shared I/O resource. More...
void	unlockIo ()
	Unlock the shared I/O resource. More...
void	lockOp ()
	Lock the extended operation. More...
void	unlockOp ()
	Unlock the extended operation. More...

Protected Attributes
std::string	m_strIdent
	IMU identity.
std::string	m_strDevName
	serial device name
int	m_nBaudRate
	device baudrate
int	m_fd
	opened device file descriptor
bool	m_bBlackListed
	IMU is [not] black listed.
int	m_accelRaw [NumOfAxes]
	accelerometer raw values
int	m_gyroRaw [NumOfAxes]
	gyroscope raw values
int	m_magRaw [NumOfAxes]
	magnetometer raw values
int	m_rpyRaw [NumOfAxes]
	roll,pitch,yaw raw values
double	m_accel [NumOfAxes]
	accelerometer (m/s^2)
double	m_gyro [NumOfAxes]
	gyrscope (radians/s)
double	m_mag [NumOfAxes]
	magnetometer (tesla)
double	m_rpy [NumOfAxes]
	roll,pitch,yaw (radians)
Quaternion	m_quaternion
	imu orientation (and robot)
pthread_mutex_t	m_mutexIo
	low-level I/O mutex
pthread_mutex_t	m_mutexOp
	high-level operation mutex

Detailed Description

Inertia Measurement Unit virtual base class with serial interface.

Definition at line 185 of file laeImu.h.

Constructor & Destructor Documentation

LaeImu::LaeImu

(

std::string

strIdent = "Virtual IMU"

)

Default constructor.

Parameters

strIdent

IMU identity string.

Definition at line 143 of file laeImu.cxx.

References clearSensedData(), m_bBlackListed, m_fd, m_mutexIo, and m_mutexOp.

                              : m_strIdent(strIdent)
{
  m_fd           = -1;
  m_bBlackListed = false;

  clearSensedData();

  pthread_mutex_init(&m_mutexIo, NULL);
  pthread_mutex_init(&m_mutexOp, NULL);
}

Member Function Documentation

int LaeImu::close ( )

virtual

Close connection to motor controller.

Returns

On success, OK(0) is returned.
On error, RC_ERROR(-1) is returned.

Definition at line 201 of file laeImu.cxx.

References lockIo(), m_fd, m_nBaudRate, m_strDevName, and unlockIo().

Referenced by Laelaps.WatchDog.WatchDog::attach(), blacklist(), laelaps::LaeRobot::disconnect(), Laelaps.WatchDog.WatchDog::open(), and ~LaeImu().

{
  lockIo();

  if( m_fd >= 0 )
  {
    SerDevClose(m_fd);
    LOGDIAG2("Closed %s interface to IMU.", m_strDevName.c_str());
  }

  m_strDevName.clear();
  m_nBaudRate = 0;
  m_fd        = -1;

  unlockIo();

  return LAE_OK;
}

void LaeImu::compute ( )

virtual

Compute all IMU values form converted, raw values.

Todo:

Definition at line 270 of file laeImu.cxx.

References computeDynamics(), and computeQuaternion().

Referenced by exec().

{
  computeQuaternion();
  computeDynamics();
}

void LaeImu::computeDynamics ( )

virtual

Compute the velocity, position, and any other dynamics from the IMU data.

Todo:

Definition at line 281 of file laeImu.cxx.

Referenced by compute().

{
}

int LaeImu::configure ( const laelaps::LaeDesc &  desc )

virtual

Configure IMU from product description.

Call after connection is opened.

Parameters

desc	Product description.

Returns

On success, OK(0) is returned.
On error, RC_ERROR(-1) is returned.

Definition at line 237 of file laeImu.cxx.

Referenced by laelaps::LaeRobot::configForOperation().

{
  return LAE_OK;
}

int LaeImu::configure ( const laelaps::LaeTunes &  tunes )

virtual

Configure IMU from tunable parameters.

Call after connection is opened.

Parameters

tunes

Laelaps tuning parameters.

Returns

On success, OK(0) is returned.
On error, RC_ERROR(-1) is returned.

Definition at line 242 of file laeImu.cxx.

{
  return LAE_OK;
}

virtual int sensor::imu::LaeImu::convertRawToSI ( )

pure virtual

Convert last read IMU values to International System of Units.

Returns

On success, OK(0) is returned.
On error, RC_ERROR(-1) is returned.

Implemented in sensor::imu::LaeImuCleanFlight.

Referenced by exec().

void LaeImu::getAttitude ( double  rpy[] )

virtual

Get the last read IMU (vehicle) attitude.

Note

N/A data are set to zero.

Parameters

[out]

rpy

Vehicle roll, pitch, and yaw (radians). The array size must be ≥ NumOfAxes.

Definition at line 329 of file laeImu.cxx.

References m_rpy, and sensor::imu::NumOfAxes.

Referenced by getImuData().

{
  for(int i = 0; i < NumOfAxes; ++i)
  {
    rpy[i] = m_rpy[i];
  }
}

void LaeImu::getAttitude ( double &  roll, double &  pitch, double &  yaw  )

virtual

Get the last read IMU (vehicle) attitude.

Note

N/A data are set to zero.

Parameters

[out]	roll	Vehicle roll (radians).
[out]	pitch	Vehicle pitch (radians).
[out]	yaw	Vehicle yaw (radians).

Definition at line 337 of file laeImu.cxx.

References m_rpy, sensor::imu::PITCH, sensor::imu::ROLL, and sensor::imu::YAW.

{
  roll  = m_rpy[ROLL];
  pitch = m_rpy[PITCH];
  yaw   = m_rpy[YAW];
}

std::string sensor::imu::LaeImu::getDevName ( )

inline

Get IMU device name.

Returns

String.

Definition at line 396 of file laeImu.h.

      {
        return m_strDevName;
      }

std::string sensor::imu::LaeImu::getIdentity ( )

inline

Get IMU identity.

Returns

String.

Definition at line 387 of file laeImu.h.

      {
        return m_strIdent;
      }

void LaeImu::getImuData ( double  accel[], double  gyro[], double  mag[], double  rpy[], Quaternion &  q  )

virtual

Get the last sensed, converted, and computed IMU data.

Note

N/A data are set to zero.

Parameters

[out]	accel	Accelerometer data (m/s^2). The array size must be ≥ NumOfAxes.
[out]	gyro	Gyroscope data (radians/s). The array size must be ≥ NumOfAxes.
[out]	mag	Magnetometer data (tesla). The array size must be ≥ NumOfAxes.
[out]	rpy	Vehicle roll, pitch, and yaw (radians). The array size must be ≥ NumOfAxes.
[out]	q	Vehicle quaternion.

Definition at line 349 of file laeImu.cxx.

References getAttitude(), getInertiaData(), getMagnetometerData(), getQuaternion(), lockOp(), and unlockOp().

Referenced by laelaps::LaeRobot::getImu().

{
  lockOp();

  getInertiaData(accel, gyro);
  getMagnetometerData(mag);
  getAttitude(rpy);
  getQuaternion(q);

  unlockOp();
}

void LaeImu::getInertiaData ( double  accel[], double  gyro[]  )

virtual

Get the last read and converted inertia data.

Note

N/A data are set to zero.

Parameters

[out]	accel	Accelerometer data (m/s^2). The array size must be ≥ NumOfAxes.
[out]	gyro	Gyroscope data (radians/s). The array size must be ≥ NumOfAxes.

Definition at line 312 of file laeImu.cxx.

References m_accel, m_gyro, and sensor::imu::NumOfAxes.

Referenced by getImuData().

{
  for(int i = 0; i < NumOfAxes; ++i)
  {
    accel[i] = m_accel[i];
    gyro[i]  = m_gyro[i];
  }
}

void LaeImu::getMagnetometerData ( double  mag[] )

virtual

Get the last read magnetometer values.

Note

N/A data are set to zero.

Parameters

[out]

mag

Magnetometer data (tesla). The array size must be ≥ NumOfAxes.

Definition at line 321 of file laeImu.cxx.

References m_mag, and sensor::imu::NumOfAxes.

Referenced by getImuData().

{
  for(int i = 0; i < NumOfAxes; ++i)
  {
    mag[i] = m_mag[i];
  }
}

void LaeImu::getQuaternion ( Quaternion &  q )

virtual

Get the last computed quaternion.

Parameters

[out]

q

Vehicle quaternion.

Definition at line 344 of file laeImu.cxx.

References m_quaternion.

Referenced by getImuData().

{
  q = m_quaternion;
}

void LaeImu::getRawInertiaData ( int  accel[], int  gyro[]  )

virtual

Get the last read raw inertia data.

Note

N/A data are set to zero.

Parameters

[out]	accel	Raw accelerometer data. The array size must be ≥ NumOfAxes.
[out]	gyro	Raw gyroscope data. The array size must be ≥ NumOfAxes.

Definition at line 303 of file laeImu.cxx.

References m_accelRaw, m_gyroRaw, and sensor::imu::NumOfAxes.

{
  for(int i = 0; i < NumOfAxes; ++i)
  {
    accel[i] = m_accelRaw[i];
    gyro[i]  = m_gyroRaw[i];
  }
}

virtual bool sensor::imu::LaeImu::hasAccelerometer ( )

inlinevirtual

Test if IMU has an accelerometer.

Returns

Returns true or false.

Reimplemented in sensor::imu::LaeImuCleanFlight.

Definition at line 267 of file laeImu.h.

      {
        return false;
      }

virtual bool sensor::imu::LaeImu::hasGyroscope ( )

inlinevirtual

Test if IMU has an gyroscope.

Returns

Returns true or false.

Reimplemented in sensor::imu::LaeImuCleanFlight.

Definition at line 277 of file laeImu.h.

      {
        return false;
      }

virtual bool sensor::imu::LaeImu::hasMagnetometer ( )

inlinevirtual

Test if IMU has an magnetometer.

Returns

Returns true or false.

Definition at line 287 of file laeImu.h.

      {
        return false;
      }

virtual bool sensor::imu::LaeImu::isBlackListed ( )

inlinevirtual

Test if IMU is black listed.

Returns

Returns true or false.

Definition at line 242 of file laeImu.h.

Referenced by exec(), sensor::imu::LaeImuCleanFlight::readIdentity(), sensor::imu::LaeImuCleanFlight::readRawImu(), sensor::imu::LaeImuCleanFlight::readRawInertia(), and sensor::imu::LaeImuCleanFlight::readRawRollPitchYaw().

      {
        return m_bBlackListed;
      }

virtual bool sensor::imu::LaeImu::isOpen ( )

inlinevirtual

Check if IMU serial interface is open.

Returns

Returns true or false.

Definition at line 222 of file laeImu.h.

Referenced by blacklist(), Laelaps.WatchDog.WatchDog::cmdConfigDPin(), Laelaps.WatchDog.WatchDog::cmdEnableAuxPort5V(), Laelaps.WatchDog.WatchDog::cmdEnableAuxPortBatt(), Laelaps.WatchDog.WatchDog::cmdEnableMotorCtlrs(), Laelaps.WatchDog.WatchDog::cmdGetFwVersion(), Laelaps.WatchDog.WatchDog::cmdPetTheDog(), Laelaps.WatchDog.WatchDog::cmdReadAPin(), Laelaps.WatchDog.WatchDog::cmdReadDPin(), Laelaps.WatchDog.WatchDog::cmdReadEnables(), Laelaps.WatchDog.WatchDog::cmdReadVoltages(), Laelaps.WatchDog.WatchDog::cmdResetRgbLed(), Laelaps.WatchDog.WatchDog::cmdSetAlarms(), Laelaps.WatchDog.WatchDog::cmdSetBatterySoC(), Laelaps.WatchDog.WatchDog::cmdSetRgbLed(), Laelaps.WatchDog.WatchDog::cmdWriteDPin(), sensor::imu::LaeImuCleanFlight::mspReadAttitude(), sensor::imu::LaeImuCleanFlight::mspReadIdent(), and sensor::imu::LaeImuCleanFlight::mspReadRawImu().

      {
        return m_fd >= 0? true: false;
      }

void sensor::imu::LaeImu::lockIo ( )

inlineprotected

Lock the shared I/O resource.

The lock()/unlock() primitives provide for safe multi-threading.

Context:

Any.

Definition at line 541 of file laeImu.h.

Referenced by close(), sensor::imu::LaeImuCleanFlight::mspReadAttitude(), sensor::imu::LaeImuCleanFlight::mspReadIdent(), sensor::imu::LaeImuCleanFlight::mspReadRawImu(), and open().

      {
        pthread_mutex_lock(&m_mutexIo);
      }

void sensor::imu::LaeImu::lockOp ( )

inlineprotected

Lock the extended operation.

The lock()/unlock() primitives provide for safe multi-threading.

Context:

Any.

Definition at line 565 of file laeImu.h.

Referenced by exec(), and getImuData().

      {
        pthread_mutex_lock(&m_mutexOp);
      }

virtual int sensor::imu::LaeImu::numDoFs ( )

inlinevirtual

Get the total IMU degress of freedom.

Returns

DoF

Reimplemented in sensor::imu::LaeImuCleanFlight.

Definition at line 257 of file laeImu.h.

      {
        return 0;
      }

int LaeImu::open ( const std::string &  strDevName, int  nBaudRate  )

virtual

Open connection to IMU.

Parameters

strDevName	Serial device name.
nBaudRate	Serial device baud rate.

Returns

On success, OK(0) is returned.
On error, RC_ERROR(-1) is returned.

Definition at line 162 of file laeImu.cxx.

References clearSensedData(), laelaps::getRealDeviceName(), lockIo(), m_bBlackListed, m_fd, m_nBaudRate, m_strDevName, and unlockIo().

Referenced by laelaps::LaeRobot::connSensors().

{
  string  strDevNameReal;
  int     rc;

  lockIo();

  // Get the real device name, not any symbolic links.
  strDevNameReal  = getRealDeviceName(strDevName);

  m_fd = SerDevOpen(strDevNameReal.c_str(), nBaudRate, 8, 'N', 1, false, false);

  if( m_fd < 0 )
  {
    LOGERROR("Failed to open %s@%d.", strDevNameReal.c_str(), nBaudRate);
    rc = -LAE_ECODE_NO_DEV;
  }

  else
  {
    m_strDevName    = strDevNameReal;
    m_nBaudRate     = nBaudRate;
    m_bBlackListed  = false;

    RtDb.m_enable.m_bImu = true;

    clearSensedData();

    LOGDIAG2("Opened interface to IMU on %s@%d.",
      strDevNameReal.c_str(), nBaudRate);

    rc = LAE_OK;
  }

  unlockIo();

  return rc;
}

virtual int sensor::imu::LaeImu::readIdentity ( std::string &  strIdent )

pure virtual

Read sensor identity values.

Parameters

[out]

strIdent

Hardware specific, formated identity string.

Returns

On success, OK(0) is returned.
On error, RC_ERROR(-1) is returned.

Implemented in sensor::imu::LaeImuCleanFlight.

virtual int sensor::imu::LaeImu::readRawImu ( )

pure virtual

Read sensor raw IMU values.

Depending on the sensor and the board, raw values read:

• accelerometer
• gyroscope
• magnetometer
• GPS
• barometer
• temperature
• accurate timing
• attitude
• quaternions

Returns

On success, OK(0) is returned.
On error, RC_ERROR(-1) is returned.

Implemented in sensor::imu::LaeImuCleanFlight.

Referenced by exec().

int LaeImu::reload ( const laelaps::LaeTunes &  tunes )

virtual

Reload with new tuning parameters.

Parameters

tunes

Laelaps tuning parameters.

Returns

On success, OK(0) is returned.
On error, RC_ERROR(-1) is returned.

Definition at line 247 of file laeImu.cxx.

Referenced by laelaps::LaeThreadImu::reload().

{
  return LAE_OK;
}

void sensor::imu::LaeImu::unlockIo ( )

inlineprotected

Unlock the shared I/O resource.

Context:

Any.

Definition at line 552 of file laeImu.h.

Referenced by close(), sensor::imu::LaeImuCleanFlight::mspReadAttitude(), sensor::imu::LaeImuCleanFlight::mspReadIdent(), sensor::imu::LaeImuCleanFlight::mspReadRawImu(), and open().

      {
        pthread_mutex_unlock(&m_mutexIo);
      }

void sensor::imu::LaeImu::unlockOp ( )

inlineprotected

Unlock the extended operation.

Context:

Any.

Definition at line 576 of file laeImu.h.

Referenced by exec(), and getImuData().

      {
        pthread_mutex_unlock(&m_mutexOp);
      }

---

The documentation for this class was generated from the following files:

• include/Laelaps/laeImu.h
• sw/liblaelaps/laeImu.cxx