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
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 }
Generated on Sun Jul 17 2022 22:09:52 by 1.7.2