Luke Petre / Mbed 2 deprecated RazorAHRS

Port of http://dev.qu.tu-berlin.de/projects/sf-razor-9dof-ahrs to an mbed, tested with a 9DOF Sensor Stick, SEN-10724

Dependencies:   mbed

Sensors.cpp@1:e27c4c0b71d8, 2011-12-28 (annotated)

Committer:
lpetre
Date:
Wed Dec 28 17:13:14 2011 +0000
Revision:
1:e27c4c0b71d8
Parent:
0:9a72d42c0da3 
Removed custom settings

Who changed what in which revision?

UserRevisionLine numberNew 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 }