AHRS library, modified version of Peter Bartz work.
Diff: Sensors.cpp
- Revision:
- 0:014ee3239c80
diff -r 000000000000 -r 014ee3239c80 Sensors.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Sensors.cpp Thu Nov 08 18:57:18 2012 +0000 @@ -0,0 +1,175 @@ +/* This file is part of the Razor AHRS Firmware */ +#include "AHRS.h" + +// I2C code to read the sensors + +// Sensor I2C addresses +#define ACCEL_READ 0xA7 +#define ACCEL_WRITE 0xA6 +#define MAGN_WRITE 0x3C +#define MAGN_READ 0x3D +#define GYRO_WRITE 0xD0 +#define GYRO_READ 0xD1 + +void IMU::I2C_Init() +{ + Wire.frequency(100000); +} + +void IMU::Accel_Init() +{ + char tx[2]; + + tx[0] = 0x2D; // Power register + tx[1] = 0x08; // Power register + Wire.write(ACCEL_WRITE, tx, 2); + wait_ms(5); + + tx[0] = 0x31; // Data format register + tx[1] = 0x08; // Set to full resolution + Wire.write(ACCEL_WRITE, tx, 2); + wait_ms(5); + + // Because our main loop runs at 50Hz we adjust the output data rate to 50Hz (25Hz bandwidth) + tx[0] = 0x2C; // Rate + tx[1] = 0x09; // Set to 50Hz, normal operation + Wire.write(ACCEL_WRITE, tx, 2); + wait_ms(5); +} + +// Reads x, y and z accelerometer registers +void IMU::Read_Accel() +{ + char buff[6]; + char tx = 0x32; // Send address to read from + + Wire.write(ACCEL_WRITE, &tx, 1); + + if (Wire.read(ACCEL_READ, buff, 6) == 0) { // All bytes received? + // No multiply by -1 for coordinate system transformation here, because of double negation: + // We want the gravity vector, which is negated acceleration vector. + accel[0] = (int)buff[3] << 8 | (int)buff[2]; // X axis (internal sensor y axis) + accel[1] = (int)buff[1] << 8 | (int)buff[0]; // Y axis (internal sensor x axis) + accel[2] = (int)buff[5] << 8 | (int)buff[4]; // Z axis (internal sensor z axis) + } else { + num_accel_errors++; + if (output_errors) pc.printf("!ERR: reading accelerometer" NEW_LINE); + } +} + +void IMU::Magn_Init() +{ + char tx[2]; + tx[0] = 0x02; // Mode + tx[1] = 0x00; // Set continuous mode (default 10Hz) + Wire.write(MAGN_WRITE, tx, 2); + wait_ms(5); + + tx[0] = 0x00; // CONFIG_A + tx[1] = 0x18; // Set 75Hz + Wire.write(MAGN_WRITE, tx, 2); + wait_ms(5); +} + +void IMU::Read_Magn() +{ + char buff[6]; + char tx = 0x03; // Send address to read from + + Wire.write(MAGN_WRITE, &tx, 1); + + if (Wire.read(MAGN_READ, buff, 6) == 0) { // All bytes received? +// 9DOF Razor IMU SEN-10125 using HMC5843 magnetometer +#if HW__VERSION_CODE == 10125 + // MSB byte first, then LSB; X, Y, Z + magnetom[0] = -1 * (((int) buff[2]) << 8) | buff[3]; // X axis (internal sensor -y axis) + magnetom[1] = -1 * (((int) buff[0]) << 8) | buff[1]; // Y axis (internal sensor -x axis) + magnetom[2] = -1 * (((int) buff[4]) << 8) | buff[5]; // Z axis (internal sensor -z axis) +// 9DOF Razor IMU SEN-10736 using HMC5883L magnetometer +#elif HW__VERSION_CODE == 10736 + // MSB byte first, then LSB; Y and Z reversed: X, Z, Y + magnetom[0] = -1 * (((int) buff[4]) << 8) | buff[5]; // X axis (internal sensor -y axis) + magnetom[1] = -1 * (((int) buff[0]) << 8) | buff[1]; // Y axis (internal sensor -x axis) + magnetom[2] = -1 * (((int) buff[2]) << 8) | buff[3]; // Z axis (internal sensor -z axis) +// 9DOF Sensor Stick SEN-10183 and SEN-10321 using HMC5843 magnetometer +#elif (HW__VERSION_CODE == 10183) || (HW__VERSION_CODE == 10321) + // MSB byte first, then LSB; X, Y, Z + magnetom[0] = (((int) buff[0]) << 8) | buff[1]; // X axis (internal sensor x axis) + magnetom[1] = -1 * (((int) buff[2]) << 8) | buff[3]; // Y axis (internal sensor -y axis) + magnetom[2] = -1 * (((int) buff[4]) << 8) | buff[5]; // Z axis (internal sensor -z axis) +// 9DOF Sensor Stick SEN-10724 using HMC5883L magnetometer +#elif HW__VERSION_CODE == 10724 + // MSB byte first, then LSB; Y and Z reversed: X, Z, Y + magnetom[0] = 1 * ((int)buff[0] << 8 | (int)buff[1]); // X axis (internal sensor x axis) + magnetom[1] = -1 * ((int)buff[4] << 8 | (int)buff[5]); // Y axis (internal sensor -y axis) + magnetom[2] = -1 * ((int)buff[2] << 8 | (int)buff[3]); // Z axis (internal sensor -z axis) +#endif + } else { + num_magn_errors++; + if (output_errors) pc.printf("!ERR: reading magnetometer" NEW_LINE); + } +} + +void IMU::Gyro_Init() +{ + char tx[2]; + + // Power up reset defaults + tx[0] = 0x3E; // Power management + tx[1] = 0x80; // ? + Wire.write(GYRO_WRITE, tx, 2); + wait_ms(5); + + // Select full-scale range of the gyro sensors + // Set LP filter bandwidth to 42Hz + tx[0] = 0x16; // + tx[1] = 0x1B; // DLPF_CFG = 3, FS_SEL = 3 + Wire.write(GYRO_WRITE, tx, 2); + wait_ms(5); + + // Set sample rato to 50Hz + tx[0] = 0x15; // + tx[1] = 0x0A; // SMPLRT_DIV = 10 (50Hz) + Wire.write(GYRO_WRITE, tx, 2); + wait_ms(5); + + // Set clock to PLL with z gyro reference + tx[0] = 0x3E; + tx[1] = 0x00; + Wire.write(GYRO_WRITE, tx, 2); + wait_ms(5); + + // Set offset values + foffset[0] = 99.5; // x (standard axis) + foffset[1] = -45.0; // y + foffset[2] = -29.7; // z + // set slope values + slope[0] = -1.05; + slope[1] = 0.95; + slope[2] = 0.47; +} + +// Reads x, y and z gyroscope registers - and temperature +void IMU::Read_Gyro() +{ + char buff[8]; + int16_t readings[3]; + char tx = 0x1B; // Sends address to read from + + Wire.write(GYRO_WRITE, &tx, 1); + + if (Wire.read(GYRO_READ, buff, 8) == 0) { // All bytes received? + int16_t temp = int16_t(((unsigned char)buff[0] << 8) | (unsigned char)buff[1]); + temperature = 35.0 + ((temp + 13200)/280.0); // temperature + for(int i = 0; i < 3; i++) { + readings[i] = (buff[(i*2)+2] << 8) | (buff[(i*2)+3]); + readings[i] -= slope[i] * temperature + foffset[i]; + } + gyro[0] = -1 * readings[1]; // X axis (internal sensor -y axis) + gyro[1] = -1 * readings[0]; // Y axis (internal sensor -x axis) + gyro[2] = -1 * readings[2]; // Z axis (internal sensor -z axis) + } else { + num_gyro_errors++; + if (output_errors) pc.printf("!ERR: reading gyroscope" NEW_LINE); + } +}