Sensors.cpp

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