Interface library for the Atmel Inertial One IMU. Contains drivers for the ITG 3200 3 axis gyro, BMA-150 3 axis accelerometer, and AK8975 3 axis compass
IMU.cpp@13:eca05448904d, 2012-02-16 (annotated)
- Committer:
- Ductapemaster
- Date:
- Thu Feb 16 08:56:11 2012 +0000
- Revision:
- 13:eca05448904d
- Parent:
- 12:cab3f7305522
Cleaned up and consolidated gyro methods, implemented accelerometer methods, implemented struct for storing 3d data values now getter methods return that struct.
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
Ductapemaster | 0:5a636973285e | 1 | /* Atmel Inertial One IMU Library |
Ductapemaster | 0:5a636973285e | 2 | * File: IMU.H |
Ductapemaster | 0:5a636973285e | 3 | * |
Ductapemaster | 0:5a636973285e | 4 | * Copyright (c) 2012 Dan Kouba |
Ductapemaster | 0:5a636973285e | 5 | * |
Ductapemaster | 0:5a636973285e | 6 | * Permission is hereby granted, free of charge, to any person obtaining a copy |
Ductapemaster | 0:5a636973285e | 7 | * of this software and associated documentation files (the "Software"), to deal |
Ductapemaster | 0:5a636973285e | 8 | * in the Software without restriction, including without limitation the rights |
Ductapemaster | 0:5a636973285e | 9 | * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell |
Ductapemaster | 0:5a636973285e | 10 | * copies of the Software, and to permit persons to whom the Software is |
Ductapemaster | 0:5a636973285e | 11 | * furnished to do so, subject to the following conditions: |
Ductapemaster | 0:5a636973285e | 12 | * |
Ductapemaster | 0:5a636973285e | 13 | * The above copyright notice and this permission notice shall be included in |
Ductapemaster | 0:5a636973285e | 14 | * all copies or substantial portions of the Software. |
Ductapemaster | 0:5a636973285e | 15 | * |
Ductapemaster | 0:5a636973285e | 16 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR |
Ductapemaster | 0:5a636973285e | 17 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, |
Ductapemaster | 0:5a636973285e | 18 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE |
Ductapemaster | 0:5a636973285e | 19 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER |
Ductapemaster | 0:5a636973285e | 20 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, |
Ductapemaster | 0:5a636973285e | 21 | * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN |
Ductapemaster | 0:5a636973285e | 22 | * THE SOFTWARE. |
Ductapemaster | 0:5a636973285e | 23 | * |
Ductapemaster | 0:5a636973285e | 24 | */ |
Ductapemaster | 0:5a636973285e | 25 | |
Ductapemaster | 0:5a636973285e | 26 | #include "IMU.h" |
Ductapemaster | 0:5a636973285e | 27 | |
Ductapemaster | 13:eca05448904d | 28 | /************** |
Ductapemaster | 13:eca05448904d | 29 | * Constructor * |
Ductapemaster | 13:eca05448904d | 30 | **************/ |
Ductapemaster | 13:eca05448904d | 31 | |
Ductapemaster | 13:eca05448904d | 32 | IMU::IMU(PinName sda, PinName scl) : _i2c(sda, scl) |
Ductapemaster | 13:eca05448904d | 33 | { |
Ductapemaster | 0:5a636973285e | 34 | |
Ductapemaster | 0:5a636973285e | 35 | _i2c.frequency(400000); //400kHz bus speed |
Ductapemaster | 0:5a636973285e | 36 | |
Ductapemaster | 0:5a636973285e | 37 | //Initial gyro config - must set DLPF[4:3] to 0x03 for proper operation |
Ductapemaster | 0:5a636973285e | 38 | char poke[2]; |
Ductapemaster | 0:5a636973285e | 39 | |
Ductapemaster | 0:5a636973285e | 40 | poke[0] = GYRO_DLPF_REG; |
Ductapemaster | 0:5a636973285e | 41 | poke[1] = FS_SEL_INIT; |
Ductapemaster | 0:5a636973285e | 42 | |
Ductapemaster | 0:5a636973285e | 43 | _i2c.write(GYRO_ADR, poke, 2, false); |
Ductapemaster | 0:5a636973285e | 44 | |
Ductapemaster | 0:5a636973285e | 45 | } |
Ductapemaster | 0:5a636973285e | 46 | |
Ductapemaster | 13:eca05448904d | 47 | /*************** |
Ductapemaster | 13:eca05448904d | 48 | * Gyro Methods * |
Ductapemaster | 13:eca05448904d | 49 | ***************/ |
Ductapemaster | 0:5a636973285e | 50 | |
Ductapemaster | 13:eca05448904d | 51 | IMU::data3d IMU::getGyroData(void) |
Ductapemaster | 13:eca05448904d | 52 | { |
Ductapemaster | 0:5a636973285e | 53 | |
Ductapemaster | 0:5a636973285e | 54 | char poke = GYRO_XOUT_H_REG; |
Ductapemaster | 0:5a636973285e | 55 | char peek[6]; |
Ductapemaster | 0:5a636973285e | 56 | |
Ductapemaster | 0:5a636973285e | 57 | _i2c.write(GYRO_ADR, &poke, 1, false); |
Ductapemaster | 0:5a636973285e | 58 | _i2c.read(GYRO_ADR, peek, 6, false); |
Ductapemaster | 0:5a636973285e | 59 | |
Ductapemaster | 13:eca05448904d | 60 | int16_t result[3] = |
Ductapemaster | 13:eca05448904d | 61 | { |
Ductapemaster | 13:eca05448904d | 62 | |
Ductapemaster | 13:eca05448904d | 63 | ( ( peek[0] << 8 ) | peek[1] ), // X |
Ductapemaster | 13:eca05448904d | 64 | ( ( peek[2] << 8 ) | peek[3] ), // Y |
Ductapemaster | 13:eca05448904d | 65 | ( ( peek[4] << 8 ) | peek[5] ) // Z |
Ductapemaster | 13:eca05448904d | 66 | |
Ductapemaster | 13:eca05448904d | 67 | }; |
Ductapemaster | 13:eca05448904d | 68 | |
Ductapemaster | 13:eca05448904d | 69 | gyro_data.x = int(result[0]); |
Ductapemaster | 13:eca05448904d | 70 | gyro_data.y = int(result[1]); |
Ductapemaster | 13:eca05448904d | 71 | gyro_data.z = int(result[2]); |
Ductapemaster | 13:eca05448904d | 72 | |
Ductapemaster | 13:eca05448904d | 73 | return gyro_data; |
Ductapemaster | 0:5a636973285e | 74 | |
Ductapemaster | 0:5a636973285e | 75 | } |
Ductapemaster | 0:5a636973285e | 76 | |
Ductapemaster | 13:eca05448904d | 77 | void IMU::setGyroLPF(char bw) |
Ductapemaster | 13:eca05448904d | 78 | { |
Ductapemaster | 0:5a636973285e | 79 | |
Ductapemaster | 13:eca05448904d | 80 | char poke[2] = { |
Ductapemaster | 13:eca05448904d | 81 | |
Ductapemaster | 13:eca05448904d | 82 | GYRO_DLPF_REG, |
Ductapemaster | 13:eca05448904d | 83 | ( bw | ( FS_SEL_INIT << 3 ) ) //Bandwidth value with FS_SEL bits set properly |
Ductapemaster | 13:eca05448904d | 84 | |
Ductapemaster | 13:eca05448904d | 85 | }; |
Ductapemaster | 0:5a636973285e | 86 | |
Ductapemaster | 0:5a636973285e | 87 | _i2c.write(GYRO_ADR, poke, 2, false); |
Ductapemaster | 0:5a636973285e | 88 | |
Ductapemaster | 0:5a636973285e | 89 | } |
Ductapemaster | 13:eca05448904d | 90 | |
Ductapemaster | 13:eca05448904d | 91 | /************************ |
Ductapemaster | 13:eca05448904d | 92 | * Accelerometer Methods * |
Ductapemaster | 13:eca05448904d | 93 | ************************/ |
Ductapemaster | 13:eca05448904d | 94 | |
Ductapemaster | 13:eca05448904d | 95 | int IMU::accX(void) |
Ductapemaster | 13:eca05448904d | 96 | { |
Ductapemaster | 13:eca05448904d | 97 | |
Ductapemaster | 13:eca05448904d | 98 | char poke = ACC_XOUT_L_REG; |
Ductapemaster | 13:eca05448904d | 99 | char peek[2]; |
Ductapemaster | 13:eca05448904d | 100 | |
Ductapemaster | 13:eca05448904d | 101 | _i2c.write(ACC_ADR, &poke, 1, false); |
Ductapemaster | 13:eca05448904d | 102 | _i2c.read(ACC_ADR, peek, 2, false); |
Ductapemaster | 13:eca05448904d | 103 | |
Ductapemaster | 13:eca05448904d | 104 | //Turning a 10 bit signed number into a signed 16 bit int |
Ductapemaster | 13:eca05448904d | 105 | return ( ( (0x80 & peek[1]) ? 0xFFFFFE00 : 0x0 ) | ( ( peek[1] & 0x7F ) << 2 ) | ( peek[0] >> 6 ) ); |
Ductapemaster | 13:eca05448904d | 106 | |
Ductapemaster | 13:eca05448904d | 107 | } |
Ductapemaster | 13:eca05448904d | 108 | |
Ductapemaster | 13:eca05448904d | 109 | int IMU::accY(void) |
Ductapemaster | 13:eca05448904d | 110 | { |
Ductapemaster | 13:eca05448904d | 111 | |
Ductapemaster | 13:eca05448904d | 112 | char poke = ACC_YOUT_L_REG; |
Ductapemaster | 13:eca05448904d | 113 | char peek[2]; |
Ductapemaster | 13:eca05448904d | 114 | |
Ductapemaster | 13:eca05448904d | 115 | _i2c.write(ACC_ADR, &poke, 1, false); |
Ductapemaster | 13:eca05448904d | 116 | _i2c.read(ACC_ADR, peek, 2, false); |
Ductapemaster | 13:eca05448904d | 117 | |
Ductapemaster | 13:eca05448904d | 118 | //Turning a 10 bit signed number into a signed 16 bit int |
Ductapemaster | 13:eca05448904d | 119 | return ( ( (0x80 & peek[1]) ? 0xFFFFFE00 : 0x0 ) | ( ( peek[1] & 0x7F ) << 2 ) | ( peek[0] >> 6 ) ); |
Ductapemaster | 13:eca05448904d | 120 | |
Ductapemaster | 13:eca05448904d | 121 | } |
Ductapemaster | 13:eca05448904d | 122 | |
Ductapemaster | 13:eca05448904d | 123 | int IMU::accZ(void) |
Ductapemaster | 13:eca05448904d | 124 | { |
Ductapemaster | 13:eca05448904d | 125 | |
Ductapemaster | 13:eca05448904d | 126 | char poke = ACC_ZOUT_L_REG; |
Ductapemaster | 13:eca05448904d | 127 | char peek[2]; |
Ductapemaster | 13:eca05448904d | 128 | |
Ductapemaster | 13:eca05448904d | 129 | _i2c.write(ACC_ADR, &poke, 1, false); |
Ductapemaster | 13:eca05448904d | 130 | _i2c.read(ACC_ADR, peek, 2, false); |
Ductapemaster | 13:eca05448904d | 131 | |
Ductapemaster | 13:eca05448904d | 132 | //Turning a 10 bit signed number into a signed 16 bit int |
Ductapemaster | 13:eca05448904d | 133 | return ( ( (0x80 & peek[1]) ? 0xFFFFFE00 : 0x0 ) | ( ( peek[1] & 0x7F ) << 2 ) | ( peek[0] >> 6 ) ); |
Ductapemaster | 13:eca05448904d | 134 | |
Ductapemaster | 13:eca05448904d | 135 | } |
Ductapemaster | 13:eca05448904d | 136 | |
Ductapemaster | 13:eca05448904d | 137 | IMU::data3d IMU::getAccData(void) |
Ductapemaster | 13:eca05448904d | 138 | { |
Ductapemaster | 13:eca05448904d | 139 | |
Ductapemaster | 13:eca05448904d | 140 | char poke = ACC_XOUT_L_REG; |
Ductapemaster | 13:eca05448904d | 141 | char peek[6]; |
Ductapemaster | 13:eca05448904d | 142 | |
Ductapemaster | 13:eca05448904d | 143 | _i2c.write(ACC_ADR, &poke, 1, false); |
Ductapemaster | 13:eca05448904d | 144 | _i2c.read(ACC_ADR, peek, 6, false); |
Ductapemaster | 13:eca05448904d | 145 | |
Ductapemaster | 13:eca05448904d | 146 | //Turning a 10 bit signed number into a signed 16 bit int |
Ductapemaster | 13:eca05448904d | 147 | |
Ductapemaster | 13:eca05448904d | 148 | acc_data.x = ( ( (0x80 & peek[1]) ? 0xFFFFFE00 : 0x0 ) | ( ( peek[1] & 0x7F ) << 2 ) | ( peek[0] >> 6 ) ); |
Ductapemaster | 13:eca05448904d | 149 | acc_data.y = ( ( (0x80 & peek[3]) ? 0xFFFFFE00 : 0x0 ) | ( ( peek[3] & 0x7F ) << 2 ) | ( peek[2] >> 6 ) ); |
Ductapemaster | 13:eca05448904d | 150 | acc_data.z = ( ( (0x80 & peek[5]) ? 0xFFFFFE00 : 0x0 ) | ( ( peek[5] & 0x7F ) << 2 ) | ( peek[4] >> 6 ) ); |
Ductapemaster | 13:eca05448904d | 151 | |
Ductapemaster | 13:eca05448904d | 152 | return acc_data; |
Ductapemaster | 13:eca05448904d | 153 | |
Ductapemaster | 13:eca05448904d | 154 | } |
Ductapemaster | 13:eca05448904d | 155 | |
Ductapemaster | 13:eca05448904d | 156 | void IMU::setAccLPF(char bw) |
Ductapemaster | 13:eca05448904d | 157 | { |
Ductapemaster | 13:eca05448904d | 158 | |
Ductapemaster | 13:eca05448904d | 159 | char poke[2] = { |
Ductapemaster | 13:eca05448904d | 160 | |
Ductapemaster | 13:eca05448904d | 161 | ACC_OPER_REG, |
Ductapemaster | 13:eca05448904d | 162 | 0x00 |
Ductapemaster | 13:eca05448904d | 163 | |
Ductapemaster | 13:eca05448904d | 164 | }; |
Ductapemaster | 13:eca05448904d | 165 | |
Ductapemaster | 13:eca05448904d | 166 | char peek; |
Ductapemaster | 13:eca05448904d | 167 | |
Ductapemaster | 13:eca05448904d | 168 | _i2c.write(ACC_ADR, poke, 1, false); |
Ductapemaster | 13:eca05448904d | 169 | _i2c.read(ACC_ADR, &peek, 1, false); //Get current register value |
Ductapemaster | 13:eca05448904d | 170 | |
Ductapemaster | 13:eca05448904d | 171 | poke[1] = ( peek | bw ); //Insert bandwidth bits at [2:0] |
Ductapemaster | 13:eca05448904d | 172 | _i2c.write(ACC_ADR, poke, 2, false); //Write register |
Ductapemaster | 13:eca05448904d | 173 | |
Ductapemaster | 13:eca05448904d | 174 | } |
Ductapemaster | 13:eca05448904d | 175 | |
Ductapemaster | 13:eca05448904d | 176 | void IMU::setAccRange(char range) |
Ductapemaster | 13:eca05448904d | 177 | { |
Ductapemaster | 13:eca05448904d | 178 | |
Ductapemaster | 13:eca05448904d | 179 | char poke[2] = { |
Ductapemaster | 13:eca05448904d | 180 | |
Ductapemaster | 13:eca05448904d | 181 | ACC_OPER_REG, |
Ductapemaster | 13:eca05448904d | 182 | 0x00 |
Ductapemaster | 13:eca05448904d | 183 | |
Ductapemaster | 13:eca05448904d | 184 | }; |
Ductapemaster | 13:eca05448904d | 185 | |
Ductapemaster | 13:eca05448904d | 186 | char peek; |
Ductapemaster | 13:eca05448904d | 187 | |
Ductapemaster | 13:eca05448904d | 188 | _i2c.write(ACC_ADR, poke, 1, false); |
Ductapemaster | 13:eca05448904d | 189 | _i2c.read(ACC_ADR, &peek, 1, false); //Get current register value |
Ductapemaster | 13:eca05448904d | 190 | |
Ductapemaster | 13:eca05448904d | 191 | poke[1] = ( peek | range << 3 ); //Insert bandwidth bits at [4:3] |
Ductapemaster | 13:eca05448904d | 192 | _i2c.write(ACC_ADR, poke, 2, false); //Write register |
Ductapemaster | 13:eca05448904d | 193 | |
Ductapemaster | 13:eca05448904d | 194 | } |
Ductapemaster | 13:eca05448904d | 195 | |
Ductapemaster | 13:eca05448904d | 196 | void IMU::accUpdateImage(void) |
Ductapemaster | 13:eca05448904d | 197 | { |
Ductapemaster | 13:eca05448904d | 198 | |
Ductapemaster | 13:eca05448904d | 199 | } |
Ductapemaster | 13:eca05448904d | 200 | |
Ductapemaster | 13:eca05448904d | 201 | void IMU::accEEWriteEn(bool we) |
Ductapemaster | 13:eca05448904d | 202 | { |
Ductapemaster | 13:eca05448904d | 203 | |
Ductapemaster | 13:eca05448904d | 204 | } |
Ductapemaster | 13:eca05448904d | 205 | |
Ductapemaster | 13:eca05448904d | 206 | /*********************** |
Ductapemaster | 13:eca05448904d | 207 | * Magnetometer Methods * |
Ductapemaster | 13:eca05448904d | 208 | ***********************/ |
Ductapemaster | 13:eca05448904d | 209 | |
Ductapemaster | 0:5a636973285e | 210 |