ICM20948.cpp

Go to the documentation of this file.

#include <stdlib.h>
#include <unistd.h>
#include <fcntl.h>
#include <linux/i2c.h>
#include <linux/i2c-dev.h>
#include <sys/ioctl.h>
#include <stropts.h>
#include <stdio.h>
#include "ICM20948.h"
#include <iostream>
#include <libraries/math_neon/math_neon.h>
using namespace std;
#define MAX_BUS 64
 
 
#define GYR_X_LSB   0x34
#define GYR_X_MSB   0x33
#define GYR_Y_LSB   0x36
#define GYR_Y_MSB   0x35
#define GYR_Z_LSB   0x38
#define GYR_Z_MSB   0x37
 
#define ACC_X_LSB   0x2E
#define ACC_X_MSB   0x2D
#define ACC_Y_LSB   0x30
#define ACC_Y_MSB   0x2F
#define ACC_Z_LSB   0x32
#define ACC_Z_MSB   0x31
 
ICM20948::ICM20948(int bus, int address) {
  I2CBus = bus;
  I2CAddress = address;
  readFullSensorState();
}
 
static inline float vectorLength(const float a, const float b, const float c) {
  return sqrtf_neon(((a*a)+(b*b)+(c*c))/3.0f);
}
 
int ICM20948::readFullSensorState(){
 
  //cerr << "Starting ICM20948 I2C sensor state read" << endl;
  char namebuf[MAX_BUS];
  snprintf(namebuf, sizeof(namebuf), "/dev/i2c-%d", I2CBus);
  int file;
  _error = false;
 
  if ((file = open(namebuf, O_RDWR)) < 0){
    cerr << "Failed to open ICM20948 Sensor on " << namebuf << " I2C Bus" << endl;
    _error = true;
    return(1);
  }
  if (ioctl(file, I2C_SLAVE, I2CAddress) < 0){
    cerr << "I2C_SLAVE address " << I2CAddress << " failed..." << endl;
    _error = true;
    return(2);
  }
 
  // According to the BMA180 datasheet on page 59, you need to send the first address
  // in write mode and then a stop/start condition is issued. Data bytes are
  // transferred with automatic address increment.
  char buf[1] = { 0x00 };
  if(write(file, buf, 1) !=1){
    cerr << "Failed to Reset Address in readFullSensorState() " << endl;
    _error = true;
  }
 
  int numberBytes = ICM20948_I2C_BUFFER;
  int bytesRead = read(file, this->dataBuffer, numberBytes);
  //cerr << "numberBytes " << numberBytes << endl;
  if (bytesRead == -1){
    cerr << "Failure to read Byte Stream in readFullSensorState()" << endl;
    _error = true;
  }
  close(file);
 
  if (this->dataBuffer[0]!=0xEA){
    /* cerr << "MAJOR FAILURE: DATA WITH IMU HAS LOST SYNC!" << endl; */
    _error = true;
  }
 
  gyroX = convertGyro(GYR_X_MSB, GYR_X_LSB);
  gyroY = convertGyro(GYR_Y_MSB, GYR_Y_LSB);
  gyroZ = convertGyro(GYR_Z_MSB, GYR_Z_LSB);
 
  gyroAbs = vectorLength(gyroX, gyroY, gyroZ);
 
  accX = convertAcc(ACC_X_MSB, ACC_X_LSB);
  accY = convertAcc(ACC_Y_MSB, ACC_Y_LSB);
  accZ = convertAcc(ACC_Z_MSB, ACC_Z_LSB);
 
  accAbs = vectorLength(accX, accY, accZ);
 
  return 0;
}
 
float ICM20948::convertGyro(int msb_reg_addr, int lsb_reg_addr){
  short temp = dataBuffer[msb_reg_addr];
  temp = (temp<<8) | dataBuffer[lsb_reg_addr];
  return (float) temp / 32767.0f;
}
 
float ICM20948::convertAcc(int msb_reg_addr, int lsb_reg_addr){
  short temp = dataBuffer[msb_reg_addr];
  temp = (temp<<8) | dataBuffer[lsb_reg_addr];
  const float scale_fac = 32767.0f;
  return (float) temp/scale_fac;
}
 
void ICM20948::displayMode(int iterations){
  for(int i=0; i<iterations; i++){
    this->readFullSensorState();
    /* printf("Rotation X,Y,Z,Abs (\t\t%d,\t %d,\t %d,\t %.2f\t) \tAcceleration (\t%.2f,\t %.2f,\t %.2f)\n", gyroX, gyroY, gyroZ, Abs, accX, accY, accZ); */
  }
}
 
void ICM20948::WakeUp(){
  char temp = 0b00000001;
  if(this->writeI2CDeviceByte(6, temp)!=0){
    cerr << "Failure to Wake chip" << endl;
  }
 
  temp = 0b01010000;
  if(this->writeI2CDeviceByte(5, temp)!=0){
    cerr << "Failure to set Gyro" << endl;
  }
}
 
void ICM20948::GetScale(){
  char Bank0 = 0b00000000;
  char Bank2 = 0b00100000;
  this->writeI2CDeviceByte(127, Bank2);
  this->readFullSensorState();
  /* char temp = dataBuffer[1]; */
  /* cerr << "Bank2 Reg 1: " << (int)temp << endl; */
  this->writeI2CDeviceByte(127, Bank0);
}
 
void ICM20948::SetScale2000(){
  char Bank0 = 0b00000000;
  char Bank2 = 0b00100000;
  this->writeI2CDeviceByte(127, Bank2);
  this->writeI2CDeviceByte(1, 0b00000111);
  this->readFullSensorState();
  /* char temp0 = dataBuffer[0]; */
  /* char temp = dataBuffer[1]; */
  /* cerr << "Bank2 Reg 0: " << (int)temp0 << endl; */
  /* cerr << "Bank2 Reg 1: " << (int)temp << endl; */
  this->writeI2CDeviceByte(127, Bank0);
  this->readFullSensorState();
  /* temp0 = dataBuffer[0]; */
  /* cerr << "Bank0 Reg 0: " << (int)temp0 << endl; */
  /* cerr << "Scale set to 2000" << endl; */
}
 
void ICM20948::SetScale250(){
  char Bank0 = 0b00000000;
  char Bank2 = 0b00100000;
  this->writeI2CDeviceByte(127, Bank2);
  this->writeI2CDeviceByte(1, 0b00000001);
  this->readFullSensorState();
 /* char temp0 = dataBuffer[0]; */
  /* char temp = dataBuffer[1]; */
  /* cerr << "Bank2 Reg 0: " << (int)temp0 << endl; */
  /* cerr << "Bank2 Reg 1: " << (int)temp << endl; */
  this->writeI2CDeviceByte(127, Bank0);
  this->readFullSensorState();
  /* temp0 = dataBuffer[0]; */
  /* cerr << "Bank0 Reg 0: " << (int)temp0 << endl; */
  /* cerr << "Scale set to 250" << endl; */
}
 
void ICM20948::ResetChip(){
  char Bank0 = 0b00000000;
  char Bank2 = 0b00100000;
  this->writeI2CDeviceByte(127, Bank2);
  this->writeI2CDeviceByte(6, 0b10000000);
  this->writeI2CDeviceByte(127, Bank0);
  this->readFullSensorState();
  /* char temp0 = dataBuffer[0]; */
  /* cerr << "Bank0 Reg 0: " << (int)temp0 << endl; */
}
 
int ICM20948::writeI2CDeviceByte(char address, char value){
 
  /* cerr << "Starting ICM20948 I2C sensor state write" << endl; */
  char namebuf[MAX_BUS];
  snprintf(namebuf, sizeof(namebuf), "/dev/i2c-%d", I2CBus);
  int file;
  if ((file = open(namebuf, O_RDWR)) < 0){
    cerr << "Failed to open ICM20948 Sensor on " << namebuf << " I2C Bus" << endl;
    return(1);
  }
  if (ioctl(file, I2C_SLAVE, I2CAddress) < 0){
    cerr << "I2C_SLAVE address " << I2CAddress << " failed..." << endl;
    return(2);
  }
 
  char buffer[2];
  buffer[0] = address;
  buffer[1] = value;
  if ( write(file, buffer, 2) != 2) {
    cerr << "Failure to write values to I2C Device address." << endl;
    return(3);
  }
  close(file);
  /* cerr << "Finished ICM20948 I2C sensor state write" << endl; */
  return 0;
}