bool RTIMUHal::HALRead(unsigned char slaveAddr, unsigned char length,
unsigned char *data, const char *errorMsg)
{
int tries, result, total;
unsigned char rxBuff[MAX_READ_LEN + 1];
struct spi_ioc_transfer rdIOC;
if (m_busIsI2C) {
if (!I2CSelectSlave(slaveAddr, errorMsg))
return false;
total = 0;
tries = 0;
while ((total < length) && (tries < 5)) {
result = read(m_I2C, data + total, length - total);
if (result < 0) {
if (strlen(errorMsg) > 0)
HAL_ERROR2("I2C read error from %d - %s\n", slaveAddr, errorMsg);
return false;
}
total += result;
if (total == length)
break;
delayMs(10);
tries++;
}
if (total < length) {
if (strlen(errorMsg) > 0)
HAL_ERROR2("I2C read from %d failed - %s\n", slaveAddr, errorMsg);
return false;
}
} else {
memset(&rdIOC, 0, sizeof(rdIOC));
rdIOC.tx_buf = 0;
rdIOC.rx_buf = (unsigned long) rxBuff;
rdIOC.len = length;
if (ioctl(m_SPI, SPI_IOC_MESSAGE(1), &rdIOC) < 0) {
if (strlen(errorMsg) > 0)
HAL_ERROR1("SPI read error from - %s\n", errorMsg);
return false;
}
memcpy(data, rxBuff, length);
}
return true;
}
bool RTIMUHal::HALWrite(unsigned char slaveAddr, unsigned char regAddr,
unsigned char length, unsigned char const *data, const char *errorMsg)
{
int result;
unsigned char txBuff[MAX_WRITE_LEN + 1];
char *ifType;
if (m_busIsI2C) {
if (!I2CSelectSlave(slaveAddr, errorMsg))
return false;
ifType = (char *)"I2C";
} else {
ifType = (char *)"SPI";
}
if (length == 0) {
result = ifWrite(®Addr, 1);
if (result < 0) {
if (strlen(errorMsg) > 0)
HAL_ERROR2("%s write of regAddr failed - %s\n", ifType, errorMsg);
return false;
} else if (result != 1) {
if (strlen(errorMsg) > 0)
HAL_ERROR2("%s write of regAddr failed (nothing written) - %s\n", ifType, errorMsg);
return false;
}
} else {
txBuff[0] = regAddr;
memcpy(txBuff + 1, data, length);
result = ifWrite(txBuff, length + 1);
if (result < 0) {
if (strlen(errorMsg) > 0)
HAL_ERROR3("%s data write of %d bytes failed - %s\n", ifType, length, errorMsg);
return false;
} else if (result < (int)length) {
if (strlen(errorMsg) > 0)
HAL_ERROR4("%s data write of %d bytes failed, only %d written - %s\n", ifType, length, result, errorMsg);
return false;
}
}
return true;
}
bool RTIMUHal::I2CSelectSlave(unsigned char slaveAddr, const char *errorMsg)
{
if (m_currentSlave == slaveAddr)
return true;
if (!HALOpen()) {
HAL_ERROR1("Failed to open I2C port - %s\n", errorMsg);
return false;
}
if (ioctl(m_I2C, I2C_SLAVE, slaveAddr) < 0) {
HAL_ERROR2("I2C slave select %d failed - %s\n", slaveAddr, errorMsg);
return false;
}
m_currentSlave = slaveAddr;
return true;
}
bool RTIMUMPU9255::IMUInit()
{
unsigned char result;
m_firstTime = true;
#ifdef MPU9255_CACHE_MODE
m_cacheIn = m_cacheOut = m_cacheCount = 0;
#endif
// set validity flags
m_imuData.fusionPoseValid = false;
m_imuData.fusionQPoseValid = false;
m_imuData.gyroValid = true;
m_imuData.accelValid = true;
m_imuData.compassValid = true;
m_imuData.motion = true;
m_imuData.IMUtemperatureValid = false;
m_imuData.IMUtemperature = 0.0;
m_imuData.humidityValid = false;
m_imuData.humidity = -1.0;
m_imuData.humidityTemperatureValid = false;
m_imuData.humidityTemperature = 0.0;
m_imuData.pressureValid = false;
m_imuData.pressure = 0.0;
m_imuData.pressureTemperatureValid = false;
m_imuData.pressureTemperature = 0.0;
// configure IMU
m_slaveAddr = m_settings->m_I2CSlaveAddress;
setSampleRate(m_settings->m_MPU9255GyroAccelSampleRate);
setCompassRate(m_settings->m_MPU9255CompassSampleRate);
setGyroLpf(m_settings->m_MPU9255GyroLpf);
setAccelLpf(m_settings->m_MPU9255AccelLpf);
setGyroFsr(m_settings->m_MPU9255GyroFsr);
setAccelFsr(m_settings->m_MPU9255AccelFsr);
setCalibrationData();
// enable the bus
if (!m_settings->HALOpen())
return false;
// reset the MPU9255
if (!m_settings->HALWrite(m_slaveAddr, MPU9255_PWR_MGMT_1, 0x80, "Failed to initiate MPU9255 reset"))
return false;
m_settings->delayMs(100);
if (!m_settings->HALWrite(m_slaveAddr, MPU9255_PWR_MGMT_1, 0x00, "Failed to stop MPU9255 reset"))
return false;
if (!m_settings->HALRead(m_slaveAddr, MPU9255_WHO_AM_I, 1, &result, "Failed to read MPU9255 id"))
return false;
if (result != MPU9255_ID) {
HAL_ERROR2("Incorrect %s id %d\n", IMUName(), result);
return false;
}
// now configure the various components
if (!setGyroConfig())
return false;
if (!setAccelConfig())
return false;
if (!setSampleRate())
return false;
if(!compassSetup()) {
return false;
}
if (!setCompassRate())
return false;
// enable the sensors
if (!m_settings->HALWrite(m_slaveAddr, MPU9255_PWR_MGMT_1, 1, "Failed to set pwr_mgmt_1"))
return false;
if (!m_settings->HALWrite(m_slaveAddr, MPU9255_PWR_MGMT_2, 0, "Failed to set pwr_mgmt_2"))
return false;
// select the data to go into the FIFO and enable
if (!resetFifo())
return false;
gyroBiasInit();
HAL_INFO1("%s init complete\n", IMUName());
return true;
}
bool RTIMUHal::HALOpen()
{
char buf[32];
unsigned char SPIMode = SPI_MODE_0;
unsigned char SPIBits = 8;
uint32_t SPISpeed = m_SPISpeed;
if (m_busIsI2C) {
if (m_I2C >= 0)
return true;
if (m_I2CBus == 255) {
HAL_ERROR("No I2C bus has been set\n");
return false;
}
sprintf(buf, "/dev/i2c-%d", m_I2CBus);
m_I2C = open(buf, O_RDWR);
if (m_I2C < 0) {
HAL_ERROR1("Failed to open I2C bus %d\n", m_I2CBus);
m_I2C = -1;
return false;
}
} else {
if (m_SPIBus == 255) {
HAL_ERROR("No SPI bus has been set\n");
return false;
}
sprintf(buf, "/dev/spidev%d.%d", m_SPIBus, m_SPISelect);
m_SPI = open(buf, O_RDWR);
if (m_SPI < 0) {
HAL_ERROR2("Failed to open SPI bus %d, select %d\n", m_SPIBus, m_SPISelect);
m_SPI = -1;
return false;
}
if (ioctl(m_SPI, SPI_IOC_WR_MODE, &SPIMode) < 0) {
HAL_ERROR1("Failed to set WR SPI_MODE0 on bus %d", m_SPIBus);
close(m_SPIBus);
return false;
}
if (ioctl(m_SPI, SPI_IOC_RD_MODE, &SPIMode) < 0) {
HAL_ERROR1("Failed to set RD SPI_MODE0 on bus %d", m_SPIBus);
close(m_SPIBus);
return false;
}
if (ioctl(m_SPI, SPI_IOC_WR_BITS_PER_WORD, &SPIBits) < 0) {
HAL_ERROR1("Failed to set WR 8 bit mode on bus %d", m_SPIBus);
close(m_SPIBus);
return false;
}
if (ioctl(m_SPI, SPI_IOC_RD_BITS_PER_WORD, &SPIBits) < 0) {
HAL_ERROR1("Failed to set RD 8 bit mode on bus %d", m_SPIBus);
close(m_SPIBus);
return false;
}
if (ioctl(m_SPI, SPI_IOC_WR_MAX_SPEED_HZ, &SPISpeed) < 0) {
HAL_ERROR2("Failed to set WR %dHz on bus %d", SPISpeed, m_SPIBus);
close(m_SPIBus);
return false;
}
if (ioctl(m_SPI, SPI_IOC_RD_MAX_SPEED_HZ, &SPISpeed) < 0) {
HAL_ERROR2("Failed to set RD %dHz on bus %d", SPISpeed, m_SPIBus);
close(m_SPIBus);
return false;
}
}
return true;
}