Daniel Kouba / IMU

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?

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