Luke Petre
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@2:5aa75c3d8cc3, 2011-12-28 (annotated)
- Committer:
- lpetre
- Date:
- Wed Dec 28 18:09:14 2011 +0000
- Revision:
- 2:5aa75c3d8cc3
- Parent:
- 1:e27c4c0b71d8
Who changed what in which revision?
| User | Revision | Line number | New contents of line |
|---|---|---|---|
| lpetre | 0:9a72d42c0da3 | 1 | /* This file is part of the Razor AHRS Firmware */ |
| lpetre | 0:9a72d42c0da3 | 2 | #include "Razor_AHRS.h" |
| lpetre | 0:9a72d42c0da3 | 3 | |
| lpetre | 0:9a72d42c0da3 | 4 | // I2C code to read the sensors |
| lpetre | 0:9a72d42c0da3 | 5 | |
| lpetre | 0:9a72d42c0da3 | 6 | // Sensor I2C addresses |
| lpetre | 0:9a72d42c0da3 | 7 | #define ACCEL_READ 0xA7 |
| lpetre | 0:9a72d42c0da3 | 8 | #define ACCEL_WRITE 0xA6 |
| lpetre | 1:e27c4c0b71d8 | 9 | #define MAGN_WRITE 0x3C |
| lpetre | 1:e27c4c0b71d8 | 10 | #define MAGN_READ 0x3D |
| lpetre | 0:9a72d42c0da3 | 11 | #define GYRO_WRITE 0xD0 |
| lpetre | 0:9a72d42c0da3 | 12 | #define GYRO_READ 0xD1 |
| lpetre | 0:9a72d42c0da3 | 13 | |
| lpetre | 1:e27c4c0b71d8 | 14 | void IMU::I2C_Init() { |
| lpetre | 1:e27c4c0b71d8 | 15 | Wire.frequency(100000); |
| lpetre | 0:9a72d42c0da3 | 16 | } |
| lpetre | 0:9a72d42c0da3 | 17 | |
| lpetre | 1:e27c4c0b71d8 | 18 | void IMU::Accel_Init() { |
| lpetre | 1:e27c4c0b71d8 | 19 | char tx[2]; |
| lpetre | 1:e27c4c0b71d8 | 20 | |
| lpetre | 1:e27c4c0b71d8 | 21 | tx[0] = 0x2D; // Power register |
| lpetre | 1:e27c4c0b71d8 | 22 | tx[1] = 0x08; // Power register |
| lpetre | 1:e27c4c0b71d8 | 23 | Wire.write(ACCEL_WRITE, tx, 2); |
| lpetre | 1:e27c4c0b71d8 | 24 | wait_ms(5); |
| lpetre | 1:e27c4c0b71d8 | 25 | |
| lpetre | 1:e27c4c0b71d8 | 26 | tx[0] = 0x31; // Data format register |
| lpetre | 1:e27c4c0b71d8 | 27 | tx[1] = 0x08; // Set to full resolution |
| lpetre | 1:e27c4c0b71d8 | 28 | Wire.write(ACCEL_WRITE, tx, 2); |
| lpetre | 1:e27c4c0b71d8 | 29 | wait_ms(5); |
| lpetre | 1:e27c4c0b71d8 | 30 | |
| lpetre | 1:e27c4c0b71d8 | 31 | // Because our main loop runs at 50Hz we adjust the output data rate to 50Hz (25Hz bandwidth) |
| lpetre | 1:e27c4c0b71d8 | 32 | tx[0] = 0x2C; // Rate |
| lpetre | 1:e27c4c0b71d8 | 33 | tx[1] = 0x09; // Set to 50Hz, normal operation |
| lpetre | 1:e27c4c0b71d8 | 34 | Wire.write(ACCEL_WRITE, tx, 2); |
| lpetre | 1:e27c4c0b71d8 | 35 | wait_ms(5); |
| lpetre | 0:9a72d42c0da3 | 36 | } |
| lpetre | 0:9a72d42c0da3 | 37 | |
| lpetre | 0:9a72d42c0da3 | 38 | // Reads x, y and z accelerometer registers |
| lpetre | 1:e27c4c0b71d8 | 39 | void IMU::Read_Accel() { |
| lpetre | 1:e27c4c0b71d8 | 40 | char buff[6]; |
| lpetre | 1:e27c4c0b71d8 | 41 | char tx = 0x32; // Send address to read from |
| lpetre | 1:e27c4c0b71d8 | 42 | |
| lpetre | 1:e27c4c0b71d8 | 43 | Wire.write(ACCEL_WRITE, &tx, 1); |
| lpetre | 1:e27c4c0b71d8 | 44 | |
| lpetre | 1:e27c4c0b71d8 | 45 | if (Wire.read(ACCEL_READ, buff, 6) == 0) { // All bytes received? |
| lpetre | 1:e27c4c0b71d8 | 46 | // No multiply by -1 for coordinate system transformation here, because of double negation: |
| lpetre | 1:e27c4c0b71d8 | 47 | // We want the gravity vector, which is negated acceleration vector. |
| lpetre | 1:e27c4c0b71d8 | 48 | accel[0] = (int)buff[3] << 8 | (int)buff[2]; // X axis (internal sensor y axis) |
| lpetre | 1:e27c4c0b71d8 | 49 | accel[1] = (int)buff[1] << 8 | (int)buff[0]; // Y axis (internal sensor x axis) |
| lpetre | 1:e27c4c0b71d8 | 50 | accel[2] = (int)buff[5] << 8 | (int)buff[4]; // Z axis (internal sensor z axis) |
| lpetre | 1:e27c4c0b71d8 | 51 | } else { |
| lpetre | 1:e27c4c0b71d8 | 52 | num_accel_errors++; |
| lpetre | 1:e27c4c0b71d8 | 53 | if (output_errors) pc.printf("!ERR: reading accelerometer" NEW_LINE); |
| lpetre | 1:e27c4c0b71d8 | 54 | } |
| lpetre | 0:9a72d42c0da3 | 55 | } |
| lpetre | 0:9a72d42c0da3 | 56 | |
| lpetre | 1:e27c4c0b71d8 | 57 | void IMU::Magn_Init() { |
| lpetre | 1:e27c4c0b71d8 | 58 | char tx[2]; |
| lpetre | 1:e27c4c0b71d8 | 59 | tx[0] = 0x02; // Mode |
| lpetre | 1:e27c4c0b71d8 | 60 | tx[1] = 0x00; // Set continuous mode (default 10Hz) |
| lpetre | 1:e27c4c0b71d8 | 61 | Wire.write(MAGN_WRITE, tx, 2); |
| lpetre | 1:e27c4c0b71d8 | 62 | wait_ms(5); |
| lpetre | 0:9a72d42c0da3 | 63 | |
| lpetre | 1:e27c4c0b71d8 | 64 | tx[0] = 0x00; // CONFIG_A |
| lpetre | 1:e27c4c0b71d8 | 65 | tx[1] = 0x18; // Set 50Hz |
| lpetre | 1:e27c4c0b71d8 | 66 | Wire.write(MAGN_WRITE, tx, 2); |
| lpetre | 1:e27c4c0b71d8 | 67 | wait_ms(5); |
| lpetre | 0:9a72d42c0da3 | 68 | } |
| lpetre | 0:9a72d42c0da3 | 69 | |
| lpetre | 1:e27c4c0b71d8 | 70 | void IMU::Read_Magn() { |
| lpetre | 1:e27c4c0b71d8 | 71 | char buff[6]; |
| lpetre | 1:e27c4c0b71d8 | 72 | char tx = 0x03; // Send address to read from |
| lpetre | 0:9a72d42c0da3 | 73 | |
| lpetre | 1:e27c4c0b71d8 | 74 | Wire.write(MAGN_WRITE, &tx, 1); |
| lpetre | 1:e27c4c0b71d8 | 75 | |
| lpetre | 1:e27c4c0b71d8 | 76 | if (Wire.read(MAGN_READ, buff, 6) == 0) { // All bytes received? |
| lpetre | 0:9a72d42c0da3 | 77 | // 9DOF Razor IMU SEN-10125 using HMC5843 magnetometer |
| lpetre | 0:9a72d42c0da3 | 78 | #if HW__VERSION_CODE == 10125 |
| lpetre | 1:e27c4c0b71d8 | 79 | // MSB byte first, then LSB; X, Y, Z |
| lpetre | 1:e27c4c0b71d8 | 80 | magnetom[0] = -1 * (((int) buff[2]) << 8) | buff[3]; // X axis (internal sensor -y axis) |
| lpetre | 1:e27c4c0b71d8 | 81 | magnetom[1] = -1 * (((int) buff[0]) << 8) | buff[1]; // Y axis (internal sensor -x axis) |
| lpetre | 1:e27c4c0b71d8 | 82 | magnetom[2] = -1 * (((int) buff[4]) << 8) | buff[5]; // Z axis (internal sensor -z axis) |
| lpetre | 0:9a72d42c0da3 | 83 | // 9DOF Razor IMU SEN-10736 using HMC5883L magnetometer |
| lpetre | 0:9a72d42c0da3 | 84 | #elif HW__VERSION_CODE == 10736 |
| lpetre | 1:e27c4c0b71d8 | 85 | // MSB byte first, then LSB; Y and Z reversed: X, Z, Y |
| lpetre | 1:e27c4c0b71d8 | 86 | magnetom[0] = -1 * (((int) buff[4]) << 8) | buff[5]; // X axis (internal sensor -y axis) |
| lpetre | 1:e27c4c0b71d8 | 87 | magnetom[1] = -1 * (((int) buff[0]) << 8) | buff[1]; // Y axis (internal sensor -x axis) |
| lpetre | 1:e27c4c0b71d8 | 88 | magnetom[2] = -1 * (((int) buff[2]) << 8) | buff[3]; // Z axis (internal sensor -z axis) |
| lpetre | 0:9a72d42c0da3 | 89 | // 9DOF Sensor Stick SEN-10183 and SEN-10321 using HMC5843 magnetometer |
| lpetre | 0:9a72d42c0da3 | 90 | #elif (HW__VERSION_CODE == 10183) || (HW__VERSION_CODE == 10321) |
| lpetre | 1:e27c4c0b71d8 | 91 | // MSB byte first, then LSB; X, Y, Z |
| lpetre | 1:e27c4c0b71d8 | 92 | magnetom[0] = (((int) buff[0]) << 8) | buff[1]; // X axis (internal sensor x axis) |
| lpetre | 1:e27c4c0b71d8 | 93 | magnetom[1] = -1 * (((int) buff[2]) << 8) | buff[3]; // Y axis (internal sensor -y axis) |
| lpetre | 1:e27c4c0b71d8 | 94 | magnetom[2] = -1 * (((int) buff[4]) << 8) | buff[5]; // Z axis (internal sensor -z axis) |
| lpetre | 0:9a72d42c0da3 | 95 | // 9DOF Sensor Stick SEN-10724 using HMC5883L magnetometer |
| lpetre | 0:9a72d42c0da3 | 96 | #elif HW__VERSION_CODE == 10724 |
| lpetre | 1:e27c4c0b71d8 | 97 | // MSB byte first, then LSB; Y and Z reversed: X, Z, Y |
| lpetre | 1:e27c4c0b71d8 | 98 | magnetom[0] = 1 * ((int)buff[0] << 8 | (int)buff[1]); // X axis (internal sensor x axis) |
| lpetre | 1:e27c4c0b71d8 | 99 | magnetom[1] = -1 * ((int)buff[4] << 8 | (int)buff[5]); // Y axis (internal sensor -y axis) |
| lpetre | 1:e27c4c0b71d8 | 100 | magnetom[2] = -1 * ((int)buff[2] << 8 | (int)buff[3]); // Z axis (internal sensor -z axis) |
| lpetre | 0:9a72d42c0da3 | 101 | #endif |
| lpetre | 1:e27c4c0b71d8 | 102 | } else { |
| lpetre | 1:e27c4c0b71d8 | 103 | num_magn_errors++; |
| lpetre | 1:e27c4c0b71d8 | 104 | if (output_errors) pc.printf("!ERR: reading magnetometer" NEW_LINE); |
| lpetre | 1:e27c4c0b71d8 | 105 | } |
| lpetre | 0:9a72d42c0da3 | 106 | } |
| lpetre | 0:9a72d42c0da3 | 107 | |
| lpetre | 1:e27c4c0b71d8 | 108 | void IMU::Gyro_Init() { |
| lpetre | 1:e27c4c0b71d8 | 109 | char tx[2]; |
| lpetre | 1:e27c4c0b71d8 | 110 | |
| lpetre | 1:e27c4c0b71d8 | 111 | // Power up reset defaults |
| lpetre | 1:e27c4c0b71d8 | 112 | tx[0] = 0x3E; // Power management |
| lpetre | 1:e27c4c0b71d8 | 113 | tx[1] = 0x80; // ? |
| lpetre | 1:e27c4c0b71d8 | 114 | Wire.write(GYRO_WRITE, tx, 2); |
| lpetre | 1:e27c4c0b71d8 | 115 | wait_ms(5); |
| lpetre | 0:9a72d42c0da3 | 116 | |
| lpetre | 1:e27c4c0b71d8 | 117 | // Select full-scale range of the gyro sensors |
| lpetre | 1:e27c4c0b71d8 | 118 | // Set LP filter bandwidth to 42Hz |
| lpetre | 1:e27c4c0b71d8 | 119 | tx[0] = 0x16; // |
| lpetre | 1:e27c4c0b71d8 | 120 | tx[1] = 0x1B; // DLPF_CFG = 3, FS_SEL = 3 |
| lpetre | 1:e27c4c0b71d8 | 121 | Wire.write(GYRO_WRITE, tx, 2); |
| lpetre | 1:e27c4c0b71d8 | 122 | wait_ms(5); |
| lpetre | 1:e27c4c0b71d8 | 123 | |
| lpetre | 1:e27c4c0b71d8 | 124 | // Set sample rato to 50Hz |
| lpetre | 1:e27c4c0b71d8 | 125 | tx[0] = 0x15; // |
| lpetre | 1:e27c4c0b71d8 | 126 | tx[1] = 0x0A; // SMPLRT_DIV = 10 (50Hz) |
| lpetre | 1:e27c4c0b71d8 | 127 | Wire.write(GYRO_WRITE, tx, 2); |
| lpetre | 1:e27c4c0b71d8 | 128 | wait_ms(5); |
| lpetre | 1:e27c4c0b71d8 | 129 | |
| lpetre | 1:e27c4c0b71d8 | 130 | // Set clock to PLL with z gyro reference |
| lpetre | 1:e27c4c0b71d8 | 131 | tx[0] = 0x3E; |
| lpetre | 1:e27c4c0b71d8 | 132 | tx[1] = 0x00; |
| lpetre | 1:e27c4c0b71d8 | 133 | Wire.write(GYRO_WRITE, tx, 2); |
| lpetre | 1:e27c4c0b71d8 | 134 | wait_ms(5); |
| lpetre | 0:9a72d42c0da3 | 135 | } |
| lpetre | 0:9a72d42c0da3 | 136 | |
| lpetre | 0:9a72d42c0da3 | 137 | // Reads x, y and z gyroscope registers |
| lpetre | 1:e27c4c0b71d8 | 138 | void IMU::Read_Gyro() { |
| lpetre | 1:e27c4c0b71d8 | 139 | char buff[6]; |
| lpetre | 1:e27c4c0b71d8 | 140 | char tx = 0x1D; // Sends address to read from |
| lpetre | 1:e27c4c0b71d8 | 141 | |
| lpetre | 1:e27c4c0b71d8 | 142 | Wire.write(GYRO_WRITE, &tx, 1); |
| lpetre | 1:e27c4c0b71d8 | 143 | |
| lpetre | 1:e27c4c0b71d8 | 144 | if (Wire.read(GYRO_READ, buff, 6) == 0) { // All bytes received? |
| lpetre | 1:e27c4c0b71d8 | 145 | gyro[0] = -1 * ((int)buff[2] << 8 | (int)buff[3]); // X axis (internal sensor -y axis) |
| lpetre | 1:e27c4c0b71d8 | 146 | gyro[1] = -1 * ((int)buff[0] << 8 | (int)buff[1]); // Y axis (internal sensor -x axis) |
| lpetre | 1:e27c4c0b71d8 | 147 | gyro[2] = -1 * ((int)buff[4] << 8 | (int)buff[5]); // Z axis (internal sensor -z axis) |
| lpetre | 1:e27c4c0b71d8 | 148 | } else { |
| lpetre | 1:e27c4c0b71d8 | 149 | num_gyro_errors++; |
| lpetre | 1:e27c4c0b71d8 | 150 | if (output_errors) pc.printf("!ERR: reading gyroscope" NEW_LINE); |
| lpetre | 1:e27c4c0b71d8 | 151 | } |
| lpetre | 0:9a72d42c0da3 | 152 | } |