int
BMA180::set_range(unsigned max_g)
{
uint8_t rangebits;
if (max_g == 0) {
max_g = 16;
}
if (max_g > 16) {
return -ERANGE;
}
if (max_g <= 2) {
_current_range = 2;
rangebits = OFFSET_LSB1_RANGE_2G;
} else if (max_g <= 3) {
_current_range = 3;
rangebits = OFFSET_LSB1_RANGE_3G;
} else if (max_g <= 4) {
_current_range = 4;
rangebits = OFFSET_LSB1_RANGE_4G;
} else if (max_g <= 8) {
_current_range = 8;
rangebits = OFFSET_LSB1_RANGE_8G;
} else if (max_g <= 16) {
_current_range = 16;
rangebits = OFFSET_LSB1_RANGE_16G;
} else {
return -EINVAL;
}
/* set new range scaling factor */
_accel_range_m_s2 = _current_range * 9.80665f;
_accel_range_scale = _accel_range_m_s2 / 8192.0f;
/* enable writing to chip config */
modify_reg(ADDR_CTRL_REG0, 0, REG0_WRITE_ENABLE);
/* adjust sensor configuration */
modify_reg(ADDR_OFFSET_LSB1, OFFSET_LSB1_RANGE_MASK, rangebits);
/* block writing to chip config */
modify_reg(ADDR_CTRL_REG0, REG0_WRITE_ENABLE, 0);
/* check if wanted value is now in register */
return !((read_reg(ADDR_OFFSET_LSB1) & OFFSET_LSB1_RANGE_MASK) ==
(OFFSET_LSB1_RANGE_MASK & rangebits));
}
void FXAS21002C::set_onchip_lowpass_filter(int frequency_hz)
{
int high = 256 / (800 / _current_rate);
int med = high / 2 ;
int low = med / 2;
if (_current_rate <= 25) {
low = -1;
}
if (_current_rate == 13) {
med = -1;
low = -1;
}
uint8_t filter = CTRL_REG0_BW_HIGH;
if (frequency_hz == 0) {
filter = CTRL_REG0_BW_HIGH;
} else if (frequency_hz <= low) {
filter = CTRL_REG0_BW_LOW;
} else if (frequency_hz <= med) {
filter = CTRL_REG0_BW_MED;
} else if (frequency_hz <= high) {
filter = CTRL_REG0_BW_HIGH;
}
set_standby(_current_rate, true);
modify_reg(FXAS21002C_CTRL_REG1, CTRL_REG0_BW_MASK, filter);
set_standby(_current_rate, false);
}
int
BMI055_gyro::gyro_set_sample_rate(float frequency)
{
uint8_t setbits = 0;
uint8_t clearbits = BMI055_GYRO_BW_MASK;
if (frequency <= 100) {
setbits |= BMI055_GYRO_RATE_100;
_gyro_sample_rate = 100;
} else if (frequency <= 250) {
setbits |= BMI055_GYRO_RATE_400;
_gyro_sample_rate = 400;
} else if (frequency <= 1000) {
setbits |= BMI055_GYRO_RATE_1000;
_gyro_sample_rate = 1000;
} else if (frequency > 1000) {
setbits |= BMI055_GYRO_RATE_2000;
_gyro_sample_rate = 2000;
} else {
return -EINVAL;
}
modify_reg(BMI055_GYR_BW, clearbits, setbits);
return OK;
}
void init_87338(void)
{
int i;
for (i = 0; base_addrs[i]; i++)
if (inb(base_addrs[i]) == 0x88 &&
inb(base_addrs[i]) == 0x00)
break;
if (!base_addrs[i])
return;
config_port = base_addrs[i];
printf("Initialising 87338 at 0x%x\n", config_port);
for (i = 0; i < (sizeof(regs) / sizeof(regs[0])); i++) {
modify_reg(regs[i].reg, regs[i].mask, regs[i].val);
if (regs[i].reg == 0x51 && regs[i].val & 4) {
outb(0x51, config_port);
while ((inb(config_port + 1) & 8) == 0);
}
}
}
int
LSM303D::mag_set_samplerate(unsigned frequency)
{
uint8_t setbits = 0;
uint8_t clearbits = REG5_RATE_BITS_M;
if (frequency == 0)
frequency = 100;
if (frequency <= 25) {
setbits |= REG5_RATE_25HZ_M;
_mag_samplerate = 25;
} else if (frequency <= 50) {
setbits |= REG5_RATE_50HZ_M;
_mag_samplerate = 50;
} else if (frequency <= 100) {
setbits |= REG5_RATE_100HZ_M;
_mag_samplerate = 100;
} else {
return -EINVAL;
}
modify_reg(ADDR_CTRL_REG5, clearbits, setbits);
return OK;
}
int
LSM303D::accel_set_onchip_lowpass_filter_bandwidth(unsigned bandwidth)
{
uint8_t setbits = 0;
uint8_t clearbits = REG2_ANTIALIAS_FILTER_BW_BITS_A;
if (bandwidth == 0)
bandwidth = 773;
if (bandwidth <= 50) {
setbits |= REG2_AA_FILTER_BW_50HZ_A;
_accel_onchip_filter_bandwith = 50;
} else if (bandwidth <= 194) {
setbits |= REG2_AA_FILTER_BW_194HZ_A;
_accel_onchip_filter_bandwith = 194;
} else if (bandwidth <= 362) {
setbits |= REG2_AA_FILTER_BW_362HZ_A;
_accel_onchip_filter_bandwith = 362;
} else if (bandwidth <= 773) {
setbits |= REG2_AA_FILTER_BW_773HZ_A;
_accel_onchip_filter_bandwith = 773;
} else {
return -EINVAL;
}
modify_reg(ADDR_CTRL_REG2, clearbits, setbits);
return OK;
}
int
LSM303D::set_samplerate(unsigned frequency)
{
uint8_t setbits = 0;
uint8_t clearbits = REG1_RATE_BITS_A;
if (frequency == 0)
frequency = 1600;
if (frequency <= 100) {
setbits |= REG1_RATE_100HZ_A;
} else if (frequency <= 200) {
setbits |= REG1_RATE_200HZ_A;
} else if (frequency <= 400) {
setbits |= REG1_RATE_400HZ_A;
} else if (frequency <= 800) {
setbits |= REG1_RATE_800HZ_A;
} else if (frequency <= 1600) {
setbits |= REG1_RATE_1600HZ_A;
} else {
return -EINVAL;
}
modify_reg(ADDR_CTRL_REG1, clearbits, setbits);
return OK;
}
int
FXOS8700CQ::accel_set_range(unsigned max_g)
{
uint8_t setbits = 0;
float lsb_per_g;
float max_accel_g;
if (max_g == 0 || max_g > 8) {
max_g = 8;
}
if (max_g > 4) { // 8g
setbits = XYZ_DATA_CFG_FS_8G;
lsb_per_g = 1024;
max_accel_g = 8;
} else if (max_g > 2) { // 4g
setbits = XYZ_DATA_CFG_FS_4G;
lsb_per_g = 2048;
max_accel_g = 4;
} else { // 2g
setbits = XYZ_DATA_CFG_FS_2G;
lsb_per_g = 4096;
max_accel_g = 2;
}
_accel_range_scale = (FXOS8700C_ONE_G / lsb_per_g);
_accel_range_m_s2 = max_accel_g * FXOS8700C_ONE_G;
modify_reg(FXOS8700CQ_XYZ_DATA_CFG, XYZ_DATA_CFG_FS_MASK, setbits);
return OK;
}
int
BMA180::set_lowpass(unsigned frequency)
{
uint8_t bwbits;
if (frequency > 1200) {
return -ERANGE;
} else if (frequency > 600) {
bwbits = BW_TCS_BW_1200HZ;
} else if (frequency > 300) {
bwbits = BW_TCS_BW_600HZ;
} else if (frequency > 150) {
bwbits = BW_TCS_BW_300HZ;
} else if (frequency > 75) {
bwbits = BW_TCS_BW_150HZ;
} else if (frequency > 40) {
bwbits = BW_TCS_BW_75HZ;
} else if (frequency > 20) {
bwbits = BW_TCS_BW_40HZ;
} else if (frequency > 10) {
bwbits = BW_TCS_BW_20HZ;
} else {
bwbits = BW_TCS_BW_10HZ;
}
/* enable writing to chip config */
modify_reg(ADDR_CTRL_REG0, 0, REG0_WRITE_ENABLE);
/* adjust sensor configuration */
modify_reg(ADDR_BW_TCS, BW_TCS_BW_MASK, bwbits);
/* block writing to chip config */
modify_reg(ADDR_CTRL_REG0, REG0_WRITE_ENABLE, 0);
/* check if wanted value is now in register */
return !((read_reg(ADDR_BW_TCS) & BW_TCS_BW_MASK) ==
(BW_TCS_BW_MASK & bwbits));
}
int
FXOS8700CQ::accel_set_samplerate(unsigned frequency)
{
uint8_t setbits = 0;
/* The selected ODR is reduced by a factor of two when the device is operated in hybrid mode.*/
uint8_t active = read_reg(FXOS8700CQ_CTRL_REG1) & CTRL_REG1_ACTIVE;
if (frequency == 0 || frequency == ACCEL_SAMPLERATE_DEFAULT) {
frequency = FXOS8700C_ACCEL_DEFAULT_RATE;
}
if (frequency <= 25) {
setbits = CTRL_REG1_DR(4); // Use 50 as it is 50 / 2
_accel_samplerate = 25;
} else if (frequency <= 50) {
setbits = CTRL_REG1_DR(3); // Use 100 as it is 100 / 2
_accel_samplerate = 50;
} else if (frequency <= 100) {
setbits = CTRL_REG1_DR(2); // Use 200 as it is 200 / 2
_accel_samplerate = 100;
} else if (frequency <= 200) {
setbits = CTRL_REG1_DR(1); // Use 400 as it is 400 / 2;
_accel_samplerate = 200;
} else if (frequency <= 400) {
setbits = CTRL_REG1_DR(0); // Use 800 as it is 800 / 2;
_accel_samplerate = 400;
} else {
return -EINVAL;
}
modify_reg(FXOS8700CQ_CTRL_REG1, CTRL_REG1_ACTIVE, 0);
modify_reg(FXOS8700CQ_CTRL_REG1, CTRL_REG1_DR_MASK, setbits);
modify_reg(FXOS8700CQ_CTRL_REG1, 0, active);
return OK;
}
int
BMA180::init()
{
int ret = ERROR;
/* do SPI init (and probe) first */
if (SPI::init() != OK)
goto out;
/* allocate basic report buffers */
_reports = new RingBuffer(2, sizeof(accel_report));
if (_reports == nullptr)
goto out;
/* advertise sensor topic */
struct accel_report zero_report;
memset(&zero_report, 0, sizeof(zero_report));
_accel_topic = orb_advertise(ORB_ID(sensor_accel), &zero_report);
/* perform soft reset (p48) */
write_reg(ADDR_RESET, SOFT_RESET);
/* wait 10 ms (datasheet incorrectly lists 10 us on page 49) */
usleep(10000);
/* enable writing to chip config */
modify_reg(ADDR_CTRL_REG0, 0, REG0_WRITE_ENABLE);
/* disable I2C interface */
modify_reg(ADDR_HIGH_DUR, HIGH_DUR_DIS_I2C, 0);
/* switch to low-noise mode */
modify_reg(ADDR_TCO_Z, TCO_Z_MODE_MASK, 0);
/* disable 12-bit mode */
modify_reg(ADDR_OFFSET_T, OFFSET_T_READOUT_12BIT, 0);
/* disable shadow-disable mode */
modify_reg(ADDR_GAIN_Y, GAIN_Y_SHADOW_DIS, 0);
/* disable writing to chip config */
modify_reg(ADDR_CTRL_REG0, REG0_WRITE_ENABLE, 0);
if (set_range(4)) warnx("Failed setting range");
if (set_lowpass(75)) warnx("Failed setting lowpass");
if (read_reg(ADDR_CHIP_ID) == CHIP_ID) {
ret = OK;
} else {
ret = ERROR;
}
out:
return ret;
}
int
BMI160::gyro_set_sample_rate(float frequency)
{
uint8_t setbits = 0;
uint8_t clearbits = (BMI_GYRO_RATE_200 | BMI_GYRO_RATE_25);
if ((int)frequency == 0) {
frequency = 3200;
}
if (frequency <= 25) {
setbits |= BMI_GYRO_RATE_25;
_gyro_sample_rate = 25;
} else if (frequency <= 50) {
setbits |= BMI_GYRO_RATE_50;
_gyro_sample_rate = 50;
} else if (frequency <= 100) {
setbits |= BMI_GYRO_RATE_100;
_gyro_sample_rate = 100;
} else if (frequency <= 200) {
setbits |= BMI_GYRO_RATE_200;
_gyro_sample_rate = 200;
} else if (frequency <= 400) {
setbits |= BMI_GYRO_RATE_400;
_gyro_sample_rate = 400;
} else if (frequency <= 800) {
setbits |= BMI_GYRO_RATE_800;
_gyro_sample_rate = 800;
} else if (frequency <= 1600) {
setbits |= BMI_GYRO_RATE_1600;
_gyro_sample_rate = 1600;
} else if (frequency > 1600) {
setbits |= BMI_GYRO_RATE_3200;
_gyro_sample_rate = 3200;
} else {
return -EINVAL;
}
modify_reg(BMIREG_GYR_CONF, clearbits, setbits);
return OK;
}
int
BMI055_accel::accel_set_sample_rate(float frequency)
{
uint8_t setbits = 0;
uint8_t clearbits = BMI055_ACCEL_BW_1000;
if (frequency < (3125 / 100)) {
setbits |= BMI055_ACCEL_BW_7_81;
_accel_sample_rate = 1563 / 100;
} else if (frequency < (625 / 10)) {
setbits |= BMI055_ACCEL_BW_15_63;
_accel_sample_rate = 625 / 10;
} else if (frequency < (125)) {
setbits |= BMI055_ACCEL_BW_31_25;
_accel_sample_rate = 625 / 10;
} else if (frequency < 250) {
setbits |= BMI055_ACCEL_BW_62_5;
_accel_sample_rate = 125;
} else if (frequency < 500) {
setbits |= BMI055_ACCEL_BW_125;
_accel_sample_rate = 250;
} else if (frequency < 1000) {
setbits |= BMI055_ACCEL_BW_250;
_accel_sample_rate = 500;
} else if (frequency < 2000) {
setbits |= BMI055_ACCEL_BW_500;
_accel_sample_rate = 1000;
} else if (frequency >= 2000) {
setbits |= BMI055_ACCEL_BW_1000;
_accel_sample_rate = 2000;
} else {
return -EINVAL;
}
/* Write accel ODR */
modify_reg(BMI055_ACC_BW, clearbits, setbits);
return OK;
}
int
BMI160::set_gyro_range(unsigned max_dps)
{
uint8_t setbits = 0;
uint8_t clearbits = BMI_GYRO_RANGE_125_DPS | BMI_GYRO_RANGE_250_DPS;
float lsb_per_dps;
float max_gyro_dps;
if (max_dps == 0) {
max_dps = 2000;
}
if (max_dps <= 125) {
max_gyro_dps = 125;
lsb_per_dps = 262.4;
setbits |= BMI_GYRO_RANGE_125_DPS;
} else if (max_dps <= 250) {
max_gyro_dps = 250;
lsb_per_dps = 131.2;
setbits |= BMI_GYRO_RANGE_250_DPS;
} else if (max_dps <= 500) {
max_gyro_dps = 500;
lsb_per_dps = 65.6;
setbits |= BMI_GYRO_RANGE_500_DPS;
} else if (max_dps <= 1000) {
max_gyro_dps = 1000;
lsb_per_dps = 32.8;
setbits |= BMI_GYRO_RANGE_1000_DPS;
} else if (max_dps <= 2000) {
max_gyro_dps = 2000;
lsb_per_dps = 16.4;
setbits |= BMI_GYRO_RANGE_2000_DPS;
} else {
return -EINVAL;
}
_gyro_range_rad_s = (max_gyro_dps / 180.0f * M_PI_F);
_gyro_range_scale = (M_PI_F / (180.0f * lsb_per_dps));
modify_reg(BMIREG_GYR_RANGE, clearbits, setbits);
return OK;
}
int
FXAS21002C::set_samplerate(unsigned frequency)
{
uint8_t bits = 0;
unsigned last_rate = _current_rate;
if (frequency == 0 || frequency == GYRO_SAMPLERATE_DEFAULT) {
frequency = FXAS21002C_DEFAULT_RATE;
}
if (frequency <= 13) {
_current_rate = 13;
bits = CTRL_REG1_DR_12_5;
} else if (frequency <= 25) {
_current_rate = 25;
bits = CTRL_REG1_DR_25HZ;
} else if (frequency <= 50) {
_current_rate = 50;
bits = CTRL_REG1_DR_50HZ;
} else if (frequency <= 100) {
_current_rate = 100;
bits = CTRL_REG1_DR_100HZ;
} else if (frequency <= 200) {
_current_rate = 200;
bits = CTRL_REG1_DR_200HZ;
} else if (frequency <= 400) {
_current_rate = 400;
bits = CTRL_REG1_DR_400HZ;
} else if (frequency <= 800) {
_current_rate = 800;
bits = CTRL_REG1_DR_800HZ;
} else {
return -EINVAL;
}
set_standby(last_rate, true);
modify_reg(FXAS21002C_CTRL_REG1, CTRL_REG1_DR_MASK, bits);
set_standby(_current_rate, false);
return OK;
}
int
LSM303D::accel_set_range(unsigned max_g)
{
uint8_t setbits = 0;
uint8_t clearbits = REG2_FULL_SCALE_BITS_A;
float new_scale_g_digit = 0.0f;
if (max_g == 0)
max_g = 16;
if (max_g <= 2) {
_accel_range_m_s2 = 2.0f*LSM303D_ONE_G;
setbits |= REG2_FULL_SCALE_2G_A;
new_scale_g_digit = 0.061e-3f;
} else if (max_g <= 4) {
_accel_range_m_s2 = 4.0f*LSM303D_ONE_G;
setbits |= REG2_FULL_SCALE_4G_A;
new_scale_g_digit = 0.122e-3f;
} else if (max_g <= 6) {
_accel_range_m_s2 = 6.0f*LSM303D_ONE_G;
setbits |= REG2_FULL_SCALE_6G_A;
new_scale_g_digit = 0.183e-3f;
} else if (max_g <= 8) {
_accel_range_m_s2 = 8.0f*LSM303D_ONE_G;
setbits |= REG2_FULL_SCALE_8G_A;
new_scale_g_digit = 0.244e-3f;
} else if (max_g <= 16) {
_accel_range_m_s2 = 16.0f*LSM303D_ONE_G;
setbits |= REG2_FULL_SCALE_16G_A;
new_scale_g_digit = 0.732e-3f;
} else {
return -EINVAL;
}
_accel_range_scale = new_scale_g_digit * LSM303D_ONE_G;
modify_reg(ADDR_CTRL_REG2, clearbits, setbits);
return OK;
}
int
BMI160::set_accel_range(unsigned max_g)
{
uint8_t setbits = 0;
uint8_t clearbits = BMI_ACCEL_RANGE_2_G | BMI_ACCEL_RANGE_16_G;
float lsb_per_g;
float max_accel_g;
if (max_g == 0) {
max_g = 16;
}
if (max_g <= 2) {
max_accel_g = 2;
setbits |= BMI_ACCEL_RANGE_2_G;
lsb_per_g = 16384;
} else if (max_g <= 4) {
max_accel_g = 4;
setbits |= BMI_ACCEL_RANGE_4_G;
lsb_per_g = 8192;
} else if (max_g <= 8) {
max_accel_g = 8;
setbits |= BMI_ACCEL_RANGE_8_G;
lsb_per_g = 4096;
} else if (max_g <= 16) {
max_accel_g = 16;
setbits |= BMI_ACCEL_RANGE_16_G;
lsb_per_g = 2048;
} else {
return -EINVAL;
}
_accel_range_scale = (BMI160_ONE_G / lsb_per_g);
_accel_range_m_s2 = max_accel_g * BMI160_ONE_G;
modify_reg(BMIREG_ACC_RANGE, clearbits, setbits);
return OK;
}
int
FXAS21002C::set_range(unsigned max_dps)
{
uint8_t bits = CTRL_REG0_FS_250_DPS;
float new_range_scale_dps_digit;
float new_range;
if (max_dps == 0) {
max_dps = 2000;
}
if (max_dps <= 250) {
new_range = 250;
new_range_scale_dps_digit = 7.8125e-3f;
bits = CTRL_REG0_FS_250_DPS;
} else if (max_dps <= 500) {
new_range = 500;
new_range_scale_dps_digit = 15.625e-3f;
bits = CTRL_REG0_FS_500_DPS;
} else if (max_dps <= 1000) {
new_range = 1000;
new_range_scale_dps_digit = 31.25e-3f;
bits = CTRL_REG0_FS_1000_DPS;
} else if (max_dps <= 2000) {
new_range = 2000;
new_range_scale_dps_digit = 62.5e-3f;
bits = CTRL_REG0_FS_2000_DPS;
} else {
return -EINVAL;
}
set_standby(_current_rate, true);
_gyro_range_rad_s = new_range / 180.0f * M_PI_F;
_gyro_range_scale = new_range_scale_dps_digit / 180.0f * M_PI_F;
modify_reg(FXAS21002C_CTRL_REG0, CTRL_REG0_FS_MASK, bits);
set_standby(_current_rate, false);
return OK;
}
void FXAS21002C::set_standby(int rate, bool standby_true)
{
uint8_t c = 0;
uint8_t s = 0;
if (standby_true) {
c = CTRL_REG1_ACTIVE | CTRL_REG1_READY;
} else {
s = CTRL_REG1_ACTIVE | CTRL_REG1_READY;
}
modify_reg(FXAS21002C_CTRL_REG1, c, s);
// From the data sheet
int wait_ms = (1000 / rate) + 60 + 1;
usleep(wait_ms * 1000);
}
int
LSM303D::set_range(unsigned max_g)
{
uint8_t setbits = 0;
uint8_t clearbits = REG2_FULL_SCALE_BITS_A;
float new_range = 0.0f;
if (max_g == 0)
max_g = 16;
if (max_g <= 2) {
new_range = 2.0f;
setbits |= REG2_FULL_SCALE_2G_A;
} else if (max_g <= 4) {
new_range = 4.0f;
setbits |= REG2_FULL_SCALE_4G_A;
} else if (max_g <= 6) {
new_range = 6.0f;
setbits |= REG2_FULL_SCALE_6G_A;
} else if (max_g <= 8) {
new_range = 8.0f;
setbits |= REG2_FULL_SCALE_8G_A;
} else if (max_g <= 16) {
new_range = 16.0f;
setbits |= REG2_FULL_SCALE_16G_A;
} else {
return -EINVAL;
}
_accel_range_m_s2 = new_range * 9.80665f;
_accel_range_scale = _accel_range_m_s2 / 32768.0f;
modify_reg(ADDR_CTRL_REG2, clearbits, setbits);
return OK;
}
int
LSM303D::mag_set_range(unsigned max_ga)
{
uint8_t setbits = 0;
uint8_t clearbits = REG6_FULL_SCALE_BITS_M;
float new_scale_ga_digit = 0.0f;
if (max_ga == 0)
max_ga = 12;
if (max_ga <= 2) {
_mag_range_ga = 2;
setbits |= REG6_FULL_SCALE_2GA_M;
new_scale_ga_digit = 0.080e-3f;
} else if (max_ga <= 4) {
_mag_range_ga = 4;
setbits |= REG6_FULL_SCALE_4GA_M;
new_scale_ga_digit = 0.160e-3f;
} else if (max_ga <= 8) {
_mag_range_ga = 8;
setbits |= REG6_FULL_SCALE_8GA_M;
new_scale_ga_digit = 0.320e-3f;
} else if (max_ga <= 12) {
_mag_range_ga = 12;
setbits |= REG6_FULL_SCALE_12GA_M;
new_scale_ga_digit = 0.479e-3f;
} else {
return -EINVAL;
}
_mag_range_scale = new_scale_ga_digit;
modify_reg(ADDR_CTRL_REG6, clearbits, setbits);
return OK;
}
int
LSM303D::mag_set_range(unsigned max_ga)
{
uint8_t setbits = 0;
uint8_t clearbits = REG6_FULL_SCALE_BITS_M;
float new_range = 0.0f;
if (max_ga == 0)
max_ga = 12;
if (max_ga <= 2) {
new_range = 2.0f;
setbits |= REG6_FULL_SCALE_2GA_M;
} else if (max_ga <= 4) {
new_range = 4.0f;
setbits |= REG6_FULL_SCALE_4GA_M;
} else if (max_ga <= 8) {
new_range = 8.0f;
setbits |= REG6_FULL_SCALE_8GA_M;
} else if (max_ga <= 12) {
new_range = 12.0f;
setbits |= REG6_FULL_SCALE_12GA_M;
} else {
return -EINVAL;
}
_mag_range_ga = new_range;
_mag_range_scale = _mag_range_ga / 32768.0f;
modify_reg(ADDR_CTRL_REG6, clearbits, setbits);
return OK;
}
int
IIS328DQ::set_range(unsigned max_g)
{
uint8_t setbits = 0;
uint8_t clearbits = REG4_FULL_SCALE_BITS;
float new_scale_g_digit = 0.0f;
if (max_g == 0)
max_g = 8;
if (max_g <= 2) {
_accel_range_m_s2 = 2;
setbits |= REG4_FULL_SCALE_2G;
new_scale_g_digit = 0.061e-3f;
} else if (max_g <= 4) {
_accel_range_m_s2 = 4;
setbits |= REG4_FULL_SCALE_4G;
new_scale_g_digit = 0.122e-3f;
} else if (max_g <= 8) {
_accel_range_m_s2 = 8;
setbits |= REG4_FULL_SCALE_8G;
new_scale_g_digit = 0.244e-3f;
}else {
return -EINVAL;
}
_accel_range_scale = new_scale_g_digit * IIS328DQ_ONE_G;
modify_reg(ADDR_CTRL_REG4, clearbits, setbits);
return OK;
}
int
BMA180::init()
{
int ret = PX4_ERROR;
/* do SPI init (and probe) first */
if (SPI::init() != OK) {
goto out;
}
/* allocate basic report buffers */
_reports = new ringbuffer::RingBuffer(2, sizeof(accel_report));
if (_reports == nullptr) {
goto out;
}
/* perform soft reset (p48) */
write_reg(ADDR_RESET, SOFT_RESET);
/* wait 10 ms (datasheet incorrectly lists 10 us on page 49) */
usleep(10000);
/* enable writing to chip config */
modify_reg(ADDR_CTRL_REG0, 0, REG0_WRITE_ENABLE);
/* disable I2C interface */
modify_reg(ADDR_HIGH_DUR, HIGH_DUR_DIS_I2C, 0);
/* switch to low-noise mode */
modify_reg(ADDR_TCO_Z, TCO_Z_MODE_MASK, 0);
/* disable 12-bit mode */
modify_reg(ADDR_OFFSET_T, OFFSET_T_READOUT_12BIT, 0);
/* disable shadow-disable mode */
modify_reg(ADDR_GAIN_Y, GAIN_Y_SHADOW_DIS, 0);
/* disable writing to chip config */
modify_reg(ADDR_CTRL_REG0, REG0_WRITE_ENABLE, 0);
if (set_range(4)) { warnx("Failed setting range"); }
if (set_lowpass(75)) { warnx("Failed setting lowpass"); }
if (read_reg(ADDR_CHIP_ID) == CHIP_ID) {
ret = OK;
} else {
ret = PX4_ERROR;
}
_class_instance = register_class_devname(ACCEL_DEVICE_PATH);
/* advertise sensor topic, measure manually to initialize valid report */
measure();
if (_class_instance == CLASS_DEVICE_PRIMARY) {
struct accel_report arp;
_reports->get(&arp);
/* measurement will have generated a report, publish */
_accel_topic = orb_advertise(ORB_ID(sensor_accel), &arp);
}
out:
return ret;
}
virtual void handle_mov(UByte opcode, size_t word_size) {
switch (opcode) {
case 0x88:
case 0x8A:
case 0x89:
case 0x8E:
{
//
// Modify R/M and REG fields in the MOD R/M byte
//
UByte fault;
UByte modrm;
_scanner.get(&modrm);
switch (decode_modrm_mod(modrm)) {
case 0:
if (word_size == 4 && decode_modrm_rm(modrm) == 4) {
fault = modify_reg(modrm);
}
else if (decode_modrm_rm(modrm) == 5) {
fault = modify_reg(modrm);
}
else {
fault = modify_rm_reg(modrm);
}
break;
case 1:
if (word_size == 4 && decode_modrm_rm(modrm) == 4) {
fault = modify_reg(modrm);
}
else {
fault = modify_rm_reg(modrm);
}
break;
case 2:
if (word_size == 4 && decode_modrm_rm(modrm) == 4) {
fault = modify_reg(modrm);
}
else {
fault = modify_rm_reg(modrm);
}
break;
case 3:
fault = modify_rm_reg(modrm);
break;
default:
System.Print(System.WARN, "Unknown modrm");
return;
}
// Inject a fault
_scanner.seek(-1, scanner::SEEK_CUR);
_scanner.put(fault);
// OK. Let's work on the next instruction.
skip_modrm(fault, word_size);
break;
}
// [SKIP] Segment registers
case 0x8B:
case 0x8C:
{
UByte next;
_scanner.get(&next);
skip_modrm(next, word_size);
break;
}
// [SKIP] Immediate
case 0xC6:
{
UByte next;
_scanner.get(&next);
skip_modrm(next, word_size);
skip_one();
break;
}
// [SKIP] Immediate
case 0xC7:
{
UByte next;
_scanner.get(&next);
skip_modrm(next, word_size);
skip_word(word_size);
break;
}
// [SKIP] 'A' register (i.e. al, ax, eax)
case 0xA0:
case 0xA1:
case 0xA2:
case 0xA3:
{
skip_word(word_size);
break;
}
default:
{
// Move imm8 to r8
if (0xB0 <= opcode && opcode <= 0xB7) {
UByte fault = opcode;
while (fault == opcode) {
fault = 0xB0 | (static_cast<UByte>(rand()) & 0xF);
}
// Inject a fault
_scanner.seek(-1, scanner::SEEK_CUR);
_scanner.put(fault);
// OK. Work on the next instruction.
skip_one();
System.Print("FI: MOV(%.2X): Change opcode %.2X\n",
opcode, fault);
}
// Move imm16/32 to r16/32
else if (0xB8 <= opcode && opcode <= 0xBF) {
UByte fault = opcode;
while (fault == opcode) {
fault = 0xB8 | (static_cast<UByte>(rand()) & 0xF);
}
// Inject a fault
_scanner.seek(-1, scanner::SEEK_CUR);
_scanner.put(fault);
// OK. Work on the next instruction.
skip_word(word_size);
System.Print("FI: MOV(%.2X): Change opcode %.2X\n",
opcode, fault);
}
}
} // switch
}
int
BMI160::accel_set_sample_rate(float frequency)
{
uint8_t setbits = 0;
uint8_t clearbits = (BMI_ACCEL_RATE_25_8 | BMI_ACCEL_RATE_1600);
if ((int)frequency == 0) {
frequency = 1600;
}
if (frequency <= 25 / 32) {
setbits |= BMI_ACCEL_RATE_25_32;
_accel_sample_rate = 25 / 32;
} else if (frequency <= 25 / 16) {
setbits |= BMI_ACCEL_RATE_25_16;
_accel_sample_rate = 25 / 16;
} else if (frequency <= 25 / 8) {
setbits |= BMI_ACCEL_RATE_25_8;
_accel_sample_rate = 25 / 8;
} else if (frequency <= 25 / 4) {
setbits |= BMI_ACCEL_RATE_25_4;
_accel_sample_rate = 25 / 4;
} else if (frequency <= 25 / 2) {
setbits |= BMI_ACCEL_RATE_25_2;
_accel_sample_rate = 25 / 2;
} else if (frequency <= 25) {
setbits |= BMI_ACCEL_RATE_25;
_accel_sample_rate = 25;
} else if (frequency <= 50) {
setbits |= BMI_ACCEL_RATE_50;
_accel_sample_rate = 50;
} else if (frequency <= 100) {
setbits |= BMI_ACCEL_RATE_100;
_accel_sample_rate = 100;
} else if (frequency <= 200) {
setbits |= BMI_ACCEL_RATE_200;
_accel_sample_rate = 200;
} else if (frequency <= 400) {
setbits |= BMI_ACCEL_RATE_400;
_accel_sample_rate = 400;
} else if (frequency <= 800) {
setbits |= BMI_ACCEL_RATE_800;
_accel_sample_rate = 800;
} else if (frequency > 800) {
setbits |= BMI_ACCEL_RATE_1600;
_accel_sample_rate = 1600;
} else {
return -EINVAL;
}
modify_reg(BMIREG_ACC_CONF, clearbits, setbits);
return OK;
}