bool RTIMUGD20HM303DLHC::setAccelCTRL4()
{
unsigned char ctrl4;
switch (m_settings->m_GD20HM303DLHCAccelFsr) {
case LSM303DLHC_ACCEL_FSR_2:
m_accelScale = (RTFLOAT)0.001 / (RTFLOAT)16;
break;
case LSM303DLHC_ACCEL_FSR_4:
m_accelScale = (RTFLOAT)0.002 / (RTFLOAT)16;
break;
case LSM303DLHC_ACCEL_FSR_8:
m_accelScale = (RTFLOAT)0.004 / (RTFLOAT)16;
break;
case LSM303DLHC_ACCEL_FSR_16:
m_accelScale = (RTFLOAT)0.012 / (RTFLOAT)16;
break;
default:
HAL_ERROR1("Illegal LSM303DLHC accel FSR code %d\n", m_settings->m_GD20HM303DLHCAccelFsr);
return false;
}
ctrl4 = 0x80 + (m_settings->m_GD20HM303DLHCAccelFsr << 4);
return m_settings->HALWrite(m_accelSlaveAddr, LSM303DLHC_CTRL4_A, ctrl4, "Failed to set LSM303DLHC CTRL4");
}
bool RTIMUMPU9150::setGyroFsr(unsigned char fsr)
{
switch (fsr) {
case MPU9150_GYROFSR_250:
m_gyroFsr = fsr;
m_gyroScale = RTMATH_PI / (131.0 * 180.0);
return true;
case MPU9150_GYROFSR_500:
m_gyroFsr = fsr;
m_gyroScale = RTMATH_PI / (62.5 * 180.0);
return true;
case MPU9150_GYROFSR_1000:
m_gyroFsr = fsr;
m_gyroScale = RTMATH_PI / (32.8 * 180.0);
return true;
case MPU9150_GYROFSR_2000:
m_gyroFsr = fsr;
m_gyroScale = RTMATH_PI / (16.4 * 180.0);
return true;
default:
HAL_ERROR1("Illegal MPU9150 gyro fsr %d\n", fsr);
return false;
}
}
bool RTIMULSM9DS0::setCompassCTRL6()
{
unsigned char ctrl6;
// convert FSR to uT
switch (m_settings->m_LSM9DS0CompassFsr) {
case LSM9DS0_COMPASS_FSR_2:
ctrl6 = 0;
m_compassScale = (RTFLOAT)0.008;
break;
case LSM9DS0_COMPASS_FSR_4:
ctrl6 = 0x20;
m_compassScale = (RTFLOAT)0.016;
break;
case LSM9DS0_COMPASS_FSR_8:
ctrl6 = 0x40;
m_compassScale = (RTFLOAT)0.032;
break;
case LSM9DS0_COMPASS_FSR_12:
ctrl6 = 0x60;
m_compassScale = (RTFLOAT)0.0479;
break;
default:
HAL_ERROR1("Illegal LSM9DS0 compass FSR code %d\n", m_settings->m_LSM9DS0CompassFsr);
return false;
}
return I2CWrite(m_accelCompassSlaveAddr, LSM9DS0_CTRL6, ctrl6, "Failed to set LSM9DS0 compass CTRL6");
}
bool RTIMUGD20HM303DLHC::setGyroCTRL4()
{
unsigned char ctrl4;
switch (m_settings->m_GD20HM303DLHCGyroFsr) {
case L3GD20H_FSR_245:
ctrl4 = 0x00;
m_gyroScale = (RTFLOAT)0.00875 * RTMATH_DEGREE_TO_RAD;
break;
case L3GD20H_FSR_500:
ctrl4 = 0x10;
m_gyroScale = (RTFLOAT)0.0175 * RTMATH_DEGREE_TO_RAD;
break;
case L3GD20H_FSR_2000:
ctrl4 = 0x20;
m_gyroScale = (RTFLOAT)0.07 * RTMATH_DEGREE_TO_RAD;
break;
default:
HAL_ERROR1("Illegal L3GD20H FSR code %d\n", m_settings->m_GD20HM303DLHCGyroFsr);
return false;
}
return m_settings->HALWrite(m_gyroSlaveAddr, L3GD20H_CTRL4, ctrl4, "Failed to set L3GD20H CTRL4");
}
bool RTIMUMPU9150::setAccelFsr(unsigned char fsr)
{
switch (fsr) {
case MPU9150_ACCELFSR_2:
m_accelFsr = fsr;
m_accelScale = 1.0/16384.0;
return true;
case MPU9150_ACCELFSR_4:
m_accelFsr = fsr;
m_accelScale = 1.0/8192.0;
return true;
case MPU9150_ACCELFSR_8:
m_accelFsr = fsr;
m_accelScale = 1.0/4096.0;
return true;
case MPU9150_ACCELFSR_16:
m_accelFsr = fsr;
m_accelScale = 1.0/2048.0;
return true;
default:
HAL_ERROR1("Illegal MPU9150 accel fsr %d\n", fsr);
return false;
}
}
bool RTIMUBMX055::setAccelFSR()
{
unsigned char reg;
switch(m_settings->m_BMX055AccelFsr) {
case BMX055_ACCEL_FSR_2:
reg = 0x03;
m_accelScale = 0.00098 / 16.0;
break;
case BMX055_ACCEL_FSR_4:
reg = 0x05;
m_accelScale = 0.00195 / 16.0;
break;
case BMX055_ACCEL_FSR_8:
reg = 0x08;
m_accelScale = 0.00391 / 16.0;
break;
case BMX055_ACCEL_FSR_16:
reg = 0x0c;
m_accelScale = 0.00781 / 16.0;
break;
default:
HAL_ERROR1("Illegal BMX055 accel FSR code %d\n", m_settings->m_BMX055AccelFsr);
return false;
}
return (m_settings->HALWrite(m_accelSlaveAddr, BMX055_ACCEL_PMU_RANGE, reg, "Failed to set BMX055 accel rate"));
}
bool RTIMUBMX055::setGyroFSR()
{
switch(m_settings->m_BMX055GyroFsr) {
case BMX055_GYRO_FSR_2000:
m_gyroScale = 0.061 * RTMATH_DEGREE_TO_RAD;
break;
case BMX055_GYRO_FSR_1000:
m_gyroScale = 0.0305 * RTMATH_DEGREE_TO_RAD;
break;
case BMX055_GYRO_FSR_500:
m_gyroScale = 0.0153 * RTMATH_DEGREE_TO_RAD;
break;
case BMX055_GYRO_FSR_250:
m_gyroScale = 0.0076 * RTMATH_DEGREE_TO_RAD;
break;
case BMX055_GYRO_FSR_125:
m_gyroScale = 0.0038 * RTMATH_DEGREE_TO_RAD;
break;
default:
HAL_ERROR1("Illegal BMX055 gyro FSR code %d\n", m_settings->m_BMX055GyroFsr);
return false;
}
return (m_settings->HALWrite(m_gyroSlaveAddr, BMX055_GYRO_RANGE, m_settings->m_BMX055GyroFsr, "Failed to set BMX055 gyro rate"));
}
bool RTIMULSM9DS1::setCompassCTRL2()
{
unsigned char ctrl2;
// convert FSR to uT
switch (m_settings->m_LSM9DS1CompassFsr) {
case LSM9DS1_COMPASS_FSR_4:
ctrl2 = 0;
m_compassScale = (RTFLOAT)0.014;
break;
case LSM9DS1_COMPASS_FSR_8:
ctrl2 = 0x20;
m_compassScale = (RTFLOAT)0.029;
break;
case LSM9DS1_COMPASS_FSR_12:
ctrl2 = 0x40;
m_compassScale = (RTFLOAT)0.043;
break;
case LSM9DS1_COMPASS_FSR_16:
ctrl2 = 0x60;
m_compassScale = (RTFLOAT)0.058;
break;
default:
HAL_ERROR1("Illegal LSM9DS1 compass FSR code %d\n", m_settings->m_LSM9DS1CompassFsr);
return false;
}
return m_settings->HALWrite(m_magSlaveAddr, LSM9DS1_MAG_CTRL2, ctrl2, "Failed to set LSM9DS1 compass CTRL6");
}
bool RTIMULSM9DS0::setGyroCTRL4()
{
unsigned char ctrl4;
switch (m_settings->m_LSM9DS0GyroFsr) {
case LSM9DS0_GYRO_FSR_250:
ctrl4 = 0x00;
m_gyroScale = (RTFLOAT)0.00875 * RTMATH_DEGREE_TO_RAD;
break;
case LSM9DS0_GYRO_FSR_500:
ctrl4 = 0x10;
m_gyroScale = (RTFLOAT)0.0175 * RTMATH_DEGREE_TO_RAD;
break;
case LSM9DS0_GYRO_FSR_2000:
ctrl4 = 0x20;
m_gyroScale = (RTFLOAT)0.07 * RTMATH_DEGREE_TO_RAD;
break;
default:
HAL_ERROR1("Illegal LSM9DS0 gyro FSR code %d\n", m_settings->m_LSM9DS0GyroFsr);
return false;
}
return I2CWrite(m_gyroSlaveAddr, LSM9DS0_GYRO_CTRL4, ctrl4, "Failed to set LSM9DS0 gyro CTRL4");
}
bool RTIMULSM9DS0::setGyroCTRL2()
{
if ((m_settings->m_LSM9DS0GyroHpf < LSM9DS0_GYRO_HPF_0) || (m_settings->m_LSM9DS0GyroHpf > LSM9DS0_GYRO_HPF_9)) {
HAL_ERROR1("Illegal LSM9DS0 gyro high pass filter code %d\n", m_settings->m_LSM9DS0GyroHpf);
return false;
}
return I2CWrite(m_gyroSlaveAddr, LSM9DS0_GYRO_CTRL2, m_settings->m_LSM9DS0GyroHpf, "Failed to set LSM9DS0 gyro CTRL2");
}
bool RTIMUGD20HM303DLHC::setGyroCTRL2()
{
if ((m_settings->m_GD20HM303DLHCGyroHpf < L3GD20H_HPF_0) || (m_settings->m_GD20HM303DLHCGyroHpf > L3GD20H_HPF_9)) {
HAL_ERROR1("Illegal L3GD20H high pass filter code %d\n", m_settings->m_GD20HM303DLHCGyroHpf);
return false;
}
return m_settings->HALWrite(m_gyroSlaveAddr, L3GD20H_CTRL2, m_settings->m_GD20HM303DLHCGyroHpf, "Failed to set L3GD20H CTRL2");
}
bool RTIMUMPU9150::setCompassRate(int rate)
{
if ((rate < MPU9150_COMPASSRATE_MIN) || (rate > MPU9150_COMPASSRATE_MAX)) {
HAL_ERROR1("Illegal compass rate %d\n", rate);
return false;
}
m_compassRate = rate;
return true;
}
bool RTIMUGD20HM303DLHC::setCompassCRB()
{
unsigned char crb;
// convert FSR to uT
switch (m_settings->m_GD20HM303DLHCCompassFsr) {
case LSM303DLHC_COMPASS_FSR_1_3:
crb = 0x20;
m_compassScaleXY = (RTFLOAT)100 / (RTFLOAT)1100;
m_compassScaleZ = (RTFLOAT)100 / (RTFLOAT)980;
break;
case LSM303DLHC_COMPASS_FSR_1_9:
crb = 0x40;
m_compassScaleXY = (RTFLOAT)100 / (RTFLOAT)855;
m_compassScaleZ = (RTFLOAT)100 / (RTFLOAT)760;
break;
case LSM303DLHC_COMPASS_FSR_2_5:
crb = 0x60;
m_compassScaleXY = (RTFLOAT)100 / (RTFLOAT)670;
m_compassScaleZ = (RTFLOAT)100 / (RTFLOAT)600;
break;
case LSM303DLHC_COMPASS_FSR_4:
crb = 0x80;
m_compassScaleXY = (RTFLOAT)100 / (RTFLOAT)450;
m_compassScaleZ = (RTFLOAT)100 / (RTFLOAT)400;
break;
case LSM303DLHC_COMPASS_FSR_4_7:
crb = 0xa0;
m_compassScaleXY = (RTFLOAT)100 / (RTFLOAT)400;
m_compassScaleZ = (RTFLOAT)100 / (RTFLOAT)355;
break;
case LSM303DLHC_COMPASS_FSR_5_6:
crb = 0xc0;
m_compassScaleXY = (RTFLOAT)100 / (RTFLOAT)330;
m_compassScaleZ = (RTFLOAT)100 / (RTFLOAT)295;
break;
case LSM303DLHC_COMPASS_FSR_8_1:
crb = 0xe0;
m_compassScaleXY = (RTFLOAT)100 / (RTFLOAT)230;
m_compassScaleZ = (RTFLOAT)100 / (RTFLOAT)205;
break;
default:
HAL_ERROR1("Illegal LSM303DLHC compass FSR code %d\n", m_settings->m_GD20HM303DLHCCompassFsr);
return false;
}
return m_settings->HALWrite(m_compassSlaveAddr, LSM303DLHC_CRB_M, crb, "Failed to set LSM303DLHC CRB_M");
}
bool RTIMUMPU9150::setSampleRate(int rate)
{
if ((rate < MPU9150_SAMPLERATE_MIN) || (rate > MPU9150_SAMPLERATE_MAX)) {
HAL_ERROR1("Illegal sample rate %d\n", rate);
return false;
}
m_sampleRate = rate;
m_sampleInterval = (uint64_t)1000000 / m_sampleRate;
return true;
}
bool RTIMULSM9DS1::setAccelCTRL6()
{
unsigned char ctrl6;
if ((m_settings->m_LSM9DS1AccelSampleRate < 0) || (m_settings->m_LSM9DS1AccelSampleRate > 6)) {
HAL_ERROR1("Illegal LSM9DS1 accel sample rate code %d\n", m_settings->m_LSM9DS1AccelSampleRate);
return false;
}
ctrl6 = (m_settings->m_LSM9DS1AccelSampleRate << 5);
if ((m_settings->m_LSM9DS1AccelLpf < 0) || (m_settings->m_LSM9DS1AccelLpf > 3)) {
HAL_ERROR1("Illegal LSM9DS1 accel low pass fiter code %d\n", m_settings->m_LSM9DS1AccelLpf);
return false;
}
switch (m_settings->m_LSM9DS1AccelFsr) {
case LSM9DS1_ACCEL_FSR_2:
m_accelScale = (RTFLOAT)0.000061;
break;
case LSM9DS1_ACCEL_FSR_4:
m_accelScale = (RTFLOAT)0.000122;
break;
case LSM9DS1_ACCEL_FSR_8:
m_accelScale = (RTFLOAT)0.000244;
break;
case LSM9DS1_ACCEL_FSR_16:
m_accelScale = (RTFLOAT)0.000732;
break;
default:
HAL_ERROR1("Illegal LSM9DS1 accel FSR code %d\n", m_settings->m_LSM9DS1AccelFsr);
return false;
}
ctrl6 |= (m_settings->m_LSM9DS1AccelLpf) | (m_settings->m_LSM9DS1AccelFsr << 3);
return m_settings->HALWrite(m_accelGyroSlaveAddr, LSM9DS1_CTRL6, ctrl6, "Failed to set LSM9DS1 accel CTRL6");
}
bool RTIMULSM9DS0::setGyroSampleRate()
{
unsigned char ctrl1;
switch (m_settings->m_LSM9DS0GyroSampleRate) {
case LSM9DS0_GYRO_SAMPLERATE_95:
ctrl1 = 0x0f;
m_sampleRate = 95;
break;
case LSM9DS0_GYRO_SAMPLERATE_190:
ctrl1 = 0x4f;
m_sampleRate = 190;
break;
case LSM9DS0_GYRO_SAMPLERATE_380:
ctrl1 = 0x8f;
m_sampleRate = 380;
break;
case LSM9DS0_GYRO_SAMPLERATE_760:
ctrl1 = 0xcf;
m_sampleRate = 760;
break;
default:
HAL_ERROR1("Illegal LSM9DS0 gyro sample rate code %d\n", m_settings->m_LSM9DS0GyroSampleRate);
return false;
}
m_sampleInterval = (uint64_t)1000000 / m_sampleRate;
switch (m_settings->m_LSM9DS0GyroBW) {
case LSM9DS0_GYRO_BANDWIDTH_0:
ctrl1 |= 0x00;
break;
case LSM9DS0_GYRO_BANDWIDTH_1:
ctrl1 |= 0x10;
break;
case LSM9DS0_GYRO_BANDWIDTH_2:
ctrl1 |= 0x20;
break;
case LSM9DS0_GYRO_BANDWIDTH_3:
ctrl1 |= 0x30;
break;
}
return (I2CWrite(m_gyroSlaveAddr, LSM9DS0_GYRO_CTRL1, ctrl1, "Failed to set LSM9DS0 gyro CTRL1"));
}
bool RTIMUBMX055::setGyroSampleRate()
{
unsigned char bw;
switch (m_settings->m_BMX055GyroSampleRate) {
case BMX055_GYRO_SAMPLERATE_2000_523:
bw = 0;
m_sampleRate = 2000;
break;
case BMX055_GYRO_SAMPLERATE_2000_230:
bw = 1;
m_sampleRate = 2000;
break;
case BMX055_GYRO_SAMPLERATE_1000_116:
bw = 2;
m_sampleRate = 1000;
break;
case BMX055_GYRO_SAMPLERATE_400_47:
bw = 3;
m_sampleRate = 400;
break;
case BMX055_GYRO_SAMPLERATE_200_23:
bw = 4;
m_sampleRate = 200;
break;
case BMX055_GYRO_SAMPLERATE_100_12:
bw = 5;
m_sampleRate = 100;
break;
case BMX055_GYRO_SAMPLERATE_200_64:
bw = 6;
m_sampleRate = 200;
break;
case BMX055_GYRO_SAMPLERATE_100_32:
bw = 7;
m_sampleRate = 100;
break;
default:
HAL_ERROR1("Illegal BMX055 gyro sample rate code %d\n", m_settings->m_BMX055GyroSampleRate);
return false;
}
m_sampleInterval = (uint64_t)1000000 / m_sampleRate;
return (m_settings->HALWrite(m_gyroSlaveAddr, BMX055_GYRO_BW, bw, "Failed to set BMX055 gyro rate"));
}
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 RTIMULSM9DS0::setAccelCTRL1()
{
unsigned char ctrl1;
if ((m_settings->m_LSM9DS0AccelSampleRate < 0) || (m_settings->m_LSM9DS0AccelSampleRate > 10)) {
HAL_ERROR1("Illegal LSM9DS0 accel sample rate code %d\n", m_settings->m_LSM9DS0AccelSampleRate);
return false;
}
ctrl1 = (m_settings->m_LSM9DS0AccelSampleRate << 4) | 0x07;
return I2CWrite(m_accelCompassSlaveAddr, LSM9DS0_CTRL1, ctrl1, "Failed to set LSM9DS0 accell CTRL1");
}
bool RTIMULSM9DS0::setAccelCTRL2()
{
unsigned char ctrl2;
if ((m_settings->m_LSM9DS0AccelLpf < 0) || (m_settings->m_LSM9DS0AccelLpf > 3)) {
HAL_ERROR1("Illegal LSM9DS0 accel low pass fiter code %d\n", m_settings->m_LSM9DS0AccelLpf);
return false;
}
switch (m_settings->m_LSM9DS0AccelFsr) {
case LSM9DS0_ACCEL_FSR_2:
m_accelScale = (RTFLOAT)0.000061;
break;
case LSM9DS0_ACCEL_FSR_4:
m_accelScale = (RTFLOAT)0.000122;
break;
case LSM9DS0_ACCEL_FSR_6:
m_accelScale = (RTFLOAT)0.000183;
break;
case LSM9DS0_ACCEL_FSR_8:
m_accelScale = (RTFLOAT)0.000244;
break;
case LSM9DS0_ACCEL_FSR_16:
m_accelScale = (RTFLOAT)0.000732;
break;
default:
HAL_ERROR1("Illegal LSM9DS0 accel FSR code %d\n", m_settings->m_LSM9DS0AccelFsr);
return false;
}
ctrl2 = (m_settings->m_LSM9DS0AccelLpf << 6) | (m_settings->m_LSM9DS0AccelFsr << 3);
return I2CWrite(m_accelCompassSlaveAddr, LSM9DS0_CTRL2, ctrl2, "Failed to set LSM9DS0 accel CTRL2");
}
bool RTIMULSM9DS1::setCompassCTRL1()
{
unsigned char ctrl1;
if ((m_settings->m_LSM9DS1CompassSampleRate < 0) || (m_settings->m_LSM9DS1CompassSampleRate > 5)) {
HAL_ERROR1("Illegal LSM9DS1 compass sample rate code %d\n", m_settings->m_LSM9DS1CompassSampleRate);
return false;
}
ctrl1 = (m_settings->m_LSM9DS1CompassSampleRate << 2);
return m_settings->HALWrite(m_magSlaveAddr, LSM9DS1_MAG_CTRL1, ctrl1, "Failed to set LSM9DS1 compass CTRL5");
}
bool RTIMUGD20HM303DLHC::setAccelCTRL1()
{
unsigned char ctrl1;
if ((m_settings->m_GD20HM303DLHCAccelSampleRate < 1) || (m_settings->m_GD20HM303DLHCAccelSampleRate > 7)) {
HAL_ERROR1("Illegal LSM303DLHC accel sample rate code %d\n", m_settings->m_GD20HM303DLHCAccelSampleRate);
return false;
}
ctrl1 = (m_settings->m_GD20HM303DLHCAccelSampleRate << 4) | 0x07;
return m_settings->HALWrite(m_accelSlaveAddr, LSM303DLHC_CTRL1_A, ctrl1, "Failed to set LSM303D CTRL1");
}
bool RTIMUGD20HM303DLHC::setCompassCRA()
{
unsigned char cra;
if ((m_settings->m_GD20HM303DLHCCompassSampleRate < 0) || (m_settings->m_GD20HM303DLHCCompassSampleRate > 7)) {
HAL_ERROR1("Illegal LSM303DLHC compass sample rate code %d\n", m_settings->m_GD20HM303DLHCCompassSampleRate);
return false;
}
cra = (m_settings->m_GD20HM303DLHCCompassSampleRate << 2);
return m_settings->HALWrite(m_compassSlaveAddr, LSM303DLHC_CRA_M, cra, "Failed to set LSM303DLHC CRA_M");
}
bool RTIMULSM9DS1::setGyroCTRL3()
{
unsigned char ctrl3;
if ((m_settings->m_LSM9DS1GyroHpf < LSM9DS1_GYRO_HPF_0) || (m_settings->m_LSM9DS1GyroHpf > LSM9DS1_GYRO_HPF_9)) {
HAL_ERROR1("Illegal LSM9DS1 gyro high pass filter code %d\n", m_settings->m_LSM9DS1GyroHpf);
return false;
}
ctrl3 = m_settings->m_LSM9DS1GyroHpf;
// Turn on hpf
ctrl3 |= 0x40;
return m_settings->HALWrite(m_accelGyroSlaveAddr, LSM9DS1_CTRL3, ctrl3, "Failed to set LSM9DS1 gyro CTRL3");
}
bool RTIMUMPU9150::setLpf(unsigned char lpf)
{
switch (lpf) {
case MPU9150_LPF_256:
case MPU9150_LPF_188:
case MPU9150_LPF_98:
case MPU9150_LPF_42:
case MPU9150_LPF_20:
case MPU9150_LPF_10:
case MPU9150_LPF_5:
m_lpf = lpf;
return true;
default:
HAL_ERROR1("Illegal MPU9150 lpf %d\n", lpf);
return false;
}
}
bool RTIMUBMX055::setAccelSampleRate()
{
unsigned char reg;
switch(m_settings->m_BMX055AccelSampleRate) {
case BMX055_ACCEL_SAMPLERATE_15:
reg = 0x08;
break;
case BMX055_ACCEL_SAMPLERATE_31:
reg = 0x09;
break;
case BMX055_ACCEL_SAMPLERATE_62:
reg = 0x0a;
break;
case BMX055_ACCEL_SAMPLERATE_125:
reg = 0x0b;
break;
case BMX055_ACCEL_SAMPLERATE_250:
reg = 0x0c;
break;
case BMX055_ACCEL_SAMPLERATE_500:
reg = 0x0d;
break;
case BMX055_ACCEL_SAMPLERATE_1000:
reg = 0x0e;
break;
case BMX055_ACCEL_SAMPLERATE_2000:
reg = 0x0f;
break;
default:
HAL_ERROR1("Illegal BMX055 accel FSR code %d\n", m_settings->m_BMX055AccelSampleRate);
return false;
}
return (m_settings->HALWrite(m_accelSlaveAddr, BMX055_ACCEL_PMU_BW, reg, "Failed to set BMX055 accel rate"));
}
bool RTIMUHal::HALRead(unsigned char slaveAddr, unsigned char length,
unsigned char *data, const char *errorMsg)
{
if (m_busIsI2C) {
if (I2Cdev::readBytes(slaveAddr, length, data, 10) == length)
return true;
if (strlen(errorMsg) > 0)
HAL_ERROR1("I2C read failed - %s\n", errorMsg);
return false;
} else {
SPI.beginTransaction(m_SPISettings);
digitalWrite(m_SPISelect, LOW);
for (int i = 0; i < length; i++)
data[i] = SPI.transfer(0);
digitalWrite(m_SPISelect, HIGH);
SPI.endTransaction();
return true;
}
}
bool RTIMUMPU9255::setAccelLpf(unsigned char lpf)
{
switch (lpf) {
case MPU9255_ACCEL_LPF_1130:
case MPU9255_ACCEL_LPF_460:
case MPU9255_ACCEL_LPF_184:
case MPU9255_ACCEL_LPF_92:
case MPU9255_ACCEL_LPF_41:
case MPU9255_ACCEL_LPF_20:
case MPU9255_ACCEL_LPF_10:
case MPU9255_ACCEL_LPF_5:
m_accelLpf = lpf;
return true;
default:
HAL_ERROR1("Illegal MPU9255 accel lpf %d\n", lpf);
return false;
}
}
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 RTIMULSM9DS0::setCompassCTRL5()
{
unsigned char ctrl5;
if ((m_settings->m_LSM9DS0CompassSampleRate < 0) || (m_settings->m_LSM9DS0CompassSampleRate > 5)) {
HAL_ERROR1("Illegal LSM9DS0 compass sample rate code %d\n", m_settings->m_LSM9DS0CompassSampleRate);
return false;
}
ctrl5 = (m_settings->m_LSM9DS0CompassSampleRate << 2);
#ifdef LSM9DS0_CACHE_MODE
// enable fifo
ctrl5 |= 0x40;
#endif
return I2CWrite(m_accelCompassSlaveAddr, LSM9DS0_CTRL5, ctrl5, "Failed to set LSM9DS0 compass CTRL5");
}
