Port of http://dev.qu.tu-berlin.de/projects/sf-razor-9dof-ahrs to an mbed, tested with a 9DOF Sensor Stick, SEN-10724
Sensors.cpp
- Committer:
- lpetre
- Date:
- 2011-12-28
- Revision:
- 2:5aa75c3d8cc3
- Parent:
- 1:e27c4c0b71d8
File content as of revision 2:5aa75c3d8cc3:
/* This file is part of the Razor AHRS Firmware */ #include "Razor_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 50Hz 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); } // Reads x, y and z gyroscope registers void IMU::Read_Gyro() { char buff[6]; char tx = 0x1D; // Sends address to read from Wire.write(GYRO_WRITE, &tx, 1); if (Wire.read(GYRO_READ, buff, 6) == 0) { // All bytes received? gyro[0] = -1 * ((int)buff[2] << 8 | (int)buff[3]); // X axis (internal sensor -y axis) gyro[1] = -1 * ((int)buff[0] << 8 | (int)buff[1]); // Y axis (internal sensor -x axis) gyro[2] = -1 * ((int)buff[4] << 8 | (int)buff[5]); // Z axis (internal sensor -z axis) } else { num_gyro_errors++; if (output_errors) pc.printf("!ERR: reading gyroscope" NEW_LINE); } }