Sensors.cpp

00001 /* This file is part of the Razor AHRS Firmware */
00002 #include "Razor_AHRS.h"
00003 
00004 // I2C code to read the sensors
00005 
00006 // Sensor I2C addresses
00007 #define ACCEL_READ  0xA7
00008 #define ACCEL_WRITE 0xA6
00009 #define MAGN_WRITE  0x3C
00010 #define MAGN_READ   0x3D
00011 #define GYRO_WRITE  0xD0
00012 #define GYRO_READ   0xD1
00013 
00014 void IMU::I2C_Init() {
00015     Wire.frequency(100000);
00016 }
00017 
00018 void IMU::Accel_Init() {
00019     char tx[2];
00020 
00021     tx[0] = 0x2D; // Power register
00022     tx[1] = 0x08; // Power register
00023     Wire.write(ACCEL_WRITE, tx, 2);
00024     wait_ms(5);
00025 
00026     tx[0] = 0x31; // Data format register
00027     tx[1] = 0x08; // Set to full resolution
00028     Wire.write(ACCEL_WRITE, tx, 2);
00029     wait_ms(5);
00030 
00031     // Because our main loop runs at 50Hz we adjust the output data rate to 50Hz (25Hz bandwidth)
00032     tx[0] = 0x2C;  // Rate
00033     tx[1] = 0x09;  // Set to 50Hz, normal operation
00034     Wire.write(ACCEL_WRITE, tx, 2);
00035     wait_ms(5);
00036 }
00037 
00038 // Reads x, y and z accelerometer registers
00039 void IMU::Read_Accel() {
00040     char buff[6];
00041     char tx = 0x32;  // Send address to read from
00042 
00043     Wire.write(ACCEL_WRITE, &tx, 1);
00044 
00045     if (Wire.read(ACCEL_READ, buff, 6) == 0) { // All bytes received?
00046         // No multiply by -1 for coordinate system transformation here, because of double negation:
00047         // We want the gravity vector, which is negated acceleration vector.
00048         accel[0] = (int)buff[3] << 8 | (int)buff[2]; // X axis (internal sensor y axis)
00049         accel[1] = (int)buff[1] << 8 | (int)buff[0]; // Y axis (internal sensor x axis)
00050         accel[2] = (int)buff[5] << 8 | (int)buff[4]; // Z axis (internal sensor z axis)
00051     } else {
00052         num_accel_errors++;
00053         if (output_errors) pc.printf("!ERR: reading accelerometer" NEW_LINE);
00054     }
00055 }
00056 
00057 void IMU::Magn_Init() {
00058     char tx[2];
00059     tx[0] = 0x02; // Mode
00060     tx[1] = 0x00; // Set continuous mode (default 10Hz)
00061     Wire.write(MAGN_WRITE, tx, 2);
00062     wait_ms(5);
00063 
00064     tx[0] = 0x00; // CONFIG_A
00065     tx[1] = 0x18; // Set 50Hz
00066     Wire.write(MAGN_WRITE, tx, 2);
00067     wait_ms(5);
00068 }
00069 
00070 void IMU::Read_Magn() {
00071     char buff[6];
00072     char tx = 0x03;  // Send address to read from
00073 
00074     Wire.write(MAGN_WRITE, &tx, 1);
00075 
00076     if (Wire.read(MAGN_READ, buff, 6) == 0) { // All bytes received?
00077 // 9DOF Razor IMU SEN-10125 using HMC5843 magnetometer
00078 #if HW__VERSION_CODE == 10125
00079         // MSB byte first, then LSB; X, Y, Z
00080         magnetom[0] = -1 * (((int) buff[2]) << 8) | buff[3];  // X axis (internal sensor -y axis)
00081         magnetom[1] = -1 * (((int) buff[0]) << 8) | buff[1];  // Y axis (internal sensor -x axis)
00082         magnetom[2] = -1 * (((int) buff[4]) << 8) | buff[5];  // Z axis (internal sensor -z axis)
00083 // 9DOF Razor IMU SEN-10736 using HMC5883L magnetometer
00084 #elif HW__VERSION_CODE == 10736
00085         // MSB byte first, then LSB; Y and Z reversed: X, Z, Y
00086         magnetom[0] = -1 * (((int) buff[4]) << 8) | buff[5];  // X axis (internal sensor -y axis)
00087         magnetom[1] = -1 * (((int) buff[0]) << 8) | buff[1];  // Y axis (internal sensor -x axis)
00088         magnetom[2] = -1 * (((int) buff[2]) << 8) | buff[3];  // Z axis (internal sensor -z axis)
00089 // 9DOF Sensor Stick SEN-10183 and SEN-10321 using HMC5843 magnetometer
00090 #elif (HW__VERSION_CODE == 10183) || (HW__VERSION_CODE == 10321)
00091         // MSB byte first, then LSB; X, Y, Z
00092         magnetom[0] = (((int) buff[0]) << 8) | buff[1];       // X axis (internal sensor x axis)
00093         magnetom[1] = -1 * (((int) buff[2]) << 8) | buff[3];  // Y axis (internal sensor -y axis)
00094         magnetom[2] = -1 * (((int) buff[4]) << 8) | buff[5];  // Z axis (internal sensor -z axis)
00095 // 9DOF Sensor Stick SEN-10724 using HMC5883L magnetometer
00096 #elif HW__VERSION_CODE == 10724
00097         // MSB byte first, then LSB; Y and Z reversed: X, Z, Y
00098         magnetom[0] =  1 * ((int)buff[0] << 8 | (int)buff[1]); // X axis (internal sensor x axis)
00099         magnetom[1] = -1 * ((int)buff[4] << 8 | (int)buff[5]); // Y axis (internal sensor -y axis)
00100         magnetom[2] = -1 * ((int)buff[2] << 8 | (int)buff[3]); // Z axis (internal sensor -z axis)
00101 #endif
00102     } else {
00103         num_magn_errors++;
00104         if (output_errors) pc.printf("!ERR: reading magnetometer" NEW_LINE);
00105     }
00106 }
00107 
00108 void IMU::Gyro_Init() {
00109     char tx[2];
00110 
00111     // Power up reset defaults
00112     tx[0] = 0x3E; // Power management
00113     tx[1] = 0x80; // ?
00114     Wire.write(GYRO_WRITE, tx, 2);
00115     wait_ms(5);
00116 
00117     // Select full-scale range of the gyro sensors
00118     // Set LP filter bandwidth to 42Hz
00119     tx[0] = 0x16; //
00120     tx[1] = 0x1B; // DLPF_CFG = 3, FS_SEL = 3
00121     Wire.write(GYRO_WRITE, tx, 2);
00122     wait_ms(5);
00123 
00124     // Set sample rato to 50Hz
00125     tx[0] = 0x15; //
00126     tx[1] = 0x0A; //  SMPLRT_DIV = 10 (50Hz)
00127     Wire.write(GYRO_WRITE, tx, 2);
00128     wait_ms(5);
00129 
00130     // Set clock to PLL with z gyro reference
00131     tx[0] = 0x3E;
00132     tx[1] = 0x00;
00133     Wire.write(GYRO_WRITE, tx, 2);
00134     wait_ms(5);
00135 }
00136 
00137 // Reads x, y and z gyroscope registers
00138 void IMU::Read_Gyro() {
00139     char buff[6];
00140     char tx = 0x1D; // Sends address to read from
00141 
00142     Wire.write(GYRO_WRITE, &tx, 1);
00143 
00144     if (Wire.read(GYRO_READ, buff, 6) == 0) { // All bytes received?
00145         gyro[0] = -1 * ((int)buff[2] << 8 | (int)buff[3]);   // X axis (internal sensor -y axis)
00146         gyro[1] = -1 * ((int)buff[0] << 8 | (int)buff[1]);   // Y axis (internal sensor -x axis)
00147         gyro[2] = -1 * ((int)buff[4] << 8 | (int)buff[5]);   // Z axis (internal sensor -z axis)
00148     } else {
00149         num_gyro_errors++;
00150         if (output_errors) pc.printf("!ERR: reading gyroscope" NEW_LINE);
00151     }
00152 }