Tyler Weaver / AHRS

AHRS library, modified version of Peter Bartz work.

Dependencies:   MODSERIAL

Dependents:   AHRS_demo

Sensors.cpp@1:da3b20b5d38a, 2012-11-08 (annotated)

Committer:
tylerjw
Date:
Thu Nov 08 20:13:35 2012 +0000
Revision:
1:da3b20b5d38a
Parent:
0:014ee3239c80 
Modified AHRS library.

Who changed what in which revision?

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