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
00001 /* Atmel Inertial One IMU Library 00002 * File: IMU.H 00003 * 00004 * Copyright (c) 2012 Dan Kouba 00005 * 00006 * Permission is hereby granted, free of charge, to any person obtaining a copy 00007 * of this software and associated documentation files (the "Software"), to deal 00008 * in the Software without restriction, including without limitation the rights 00009 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 00010 * copies of the Software, and to permit persons to whom the Software is 00011 * furnished to do so, subject to the following conditions: 00012 * 00013 * The above copyright notice and this permission notice shall be included in 00014 * all copies or substantial portions of the Software. 00015 * 00016 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 00017 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 00018 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 00019 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 00020 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 00021 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 00022 * THE SOFTWARE. 00023 * 00024 */ 00025 00026 #include "IMU.h" 00027 00028 /************** 00029 * Constructor * 00030 **************/ 00031 00032 IMU::IMU(PinName sda, PinName scl) : _i2c(sda, scl) 00033 { 00034 00035 _i2c.frequency(400000); //400kHz bus speed 00036 00037 //Initial gyro config - must set DLPF[4:3] to 0x03 for proper operation 00038 char poke[2]; 00039 00040 poke[0] = GYRO_DLPF_REG; 00041 poke[1] = FS_SEL_INIT; 00042 00043 _i2c.write(GYRO_ADR, poke, 2, false); 00044 00045 } 00046 00047 /*************** 00048 * Gyro Methods * 00049 ***************/ 00050 00051 IMU::data3d IMU::getGyroData(void) 00052 { 00053 00054 char poke = GYRO_XOUT_H_REG; 00055 char peek[6]; 00056 00057 _i2c.write(GYRO_ADR, &poke, 1, false); 00058 _i2c.read(GYRO_ADR, peek, 6, false); 00059 00060 int16_t result[3] = 00061 { 00062 00063 ( ( peek[0] << 8 ) | peek[1] ), // X 00064 ( ( peek[2] << 8 ) | peek[3] ), // Y 00065 ( ( peek[4] << 8 ) | peek[5] ) // Z 00066 00067 }; 00068 00069 gyro_data.x = int(result[0]); 00070 gyro_data.y = int(result[1]); 00071 gyro_data.z = int(result[2]); 00072 00073 return gyro_data; 00074 00075 } 00076 00077 void IMU::setGyroLPF(char bw) 00078 { 00079 00080 char poke[2] = { 00081 00082 GYRO_DLPF_REG, 00083 ( bw | ( FS_SEL_INIT << 3 ) ) //Bandwidth value with FS_SEL bits set properly 00084 00085 }; 00086 00087 _i2c.write(GYRO_ADR, poke, 2, false); 00088 00089 } 00090 00091 /************************ 00092 * Accelerometer Methods * 00093 ************************/ 00094 00095 int IMU::accX(void) 00096 { 00097 00098 char poke = ACC_XOUT_L_REG; 00099 char peek[2]; 00100 00101 _i2c.write(ACC_ADR, &poke, 1, false); 00102 _i2c.read(ACC_ADR, peek, 2, false); 00103 00104 //Turning a 10 bit signed number into a signed 16 bit int 00105 return ( ( (0x80 & peek[1]) ? 0xFFFFFE00 : 0x0 ) | ( ( peek[1] & 0x7F ) << 2 ) | ( peek[0] >> 6 ) ); 00106 00107 } 00108 00109 int IMU::accY(void) 00110 { 00111 00112 char poke = ACC_YOUT_L_REG; 00113 char peek[2]; 00114 00115 _i2c.write(ACC_ADR, &poke, 1, false); 00116 _i2c.read(ACC_ADR, peek, 2, false); 00117 00118 //Turning a 10 bit signed number into a signed 16 bit int 00119 return ( ( (0x80 & peek[1]) ? 0xFFFFFE00 : 0x0 ) | ( ( peek[1] & 0x7F ) << 2 ) | ( peek[0] >> 6 ) ); 00120 00121 } 00122 00123 int IMU::accZ(void) 00124 { 00125 00126 char poke = ACC_ZOUT_L_REG; 00127 char peek[2]; 00128 00129 _i2c.write(ACC_ADR, &poke, 1, false); 00130 _i2c.read(ACC_ADR, peek, 2, false); 00131 00132 //Turning a 10 bit signed number into a signed 16 bit int 00133 return ( ( (0x80 & peek[1]) ? 0xFFFFFE00 : 0x0 ) | ( ( peek[1] & 0x7F ) << 2 ) | ( peek[0] >> 6 ) ); 00134 00135 } 00136 00137 IMU::data3d IMU::getAccData(void) 00138 { 00139 00140 char poke = ACC_XOUT_L_REG; 00141 char peek[6]; 00142 00143 _i2c.write(ACC_ADR, &poke, 1, false); 00144 _i2c.read(ACC_ADR, peek, 6, false); 00145 00146 //Turning a 10 bit signed number into a signed 16 bit int 00147 00148 acc_data.x = ( ( (0x80 & peek[1]) ? 0xFFFFFE00 : 0x0 ) | ( ( peek[1] & 0x7F ) << 2 ) | ( peek[0] >> 6 ) ); 00149 acc_data.y = ( ( (0x80 & peek[3]) ? 0xFFFFFE00 : 0x0 ) | ( ( peek[3] & 0x7F ) << 2 ) | ( peek[2] >> 6 ) ); 00150 acc_data.z = ( ( (0x80 & peek[5]) ? 0xFFFFFE00 : 0x0 ) | ( ( peek[5] & 0x7F ) << 2 ) | ( peek[4] >> 6 ) ); 00151 00152 return acc_data; 00153 00154 } 00155 00156 void IMU::setAccLPF(char bw) 00157 { 00158 00159 char poke[2] = { 00160 00161 ACC_OPER_REG, 00162 0x00 00163 00164 }; 00165 00166 char peek; 00167 00168 _i2c.write(ACC_ADR, poke, 1, false); 00169 _i2c.read(ACC_ADR, &peek, 1, false); //Get current register value 00170 00171 poke[1] = ( peek | bw ); //Insert bandwidth bits at [2:0] 00172 _i2c.write(ACC_ADR, poke, 2, false); //Write register 00173 00174 } 00175 00176 void IMU::setAccRange(char range) 00177 { 00178 00179 char poke[2] = { 00180 00181 ACC_OPER_REG, 00182 0x00 00183 00184 }; 00185 00186 char peek; 00187 00188 _i2c.write(ACC_ADR, poke, 1, false); 00189 _i2c.read(ACC_ADR, &peek, 1, false); //Get current register value 00190 00191 poke[1] = ( peek | range << 3 ); //Insert bandwidth bits at [4:3] 00192 _i2c.write(ACC_ADR, poke, 2, false); //Write register 00193 00194 } 00195 00196 void IMU::accUpdateImage(void) 00197 { 00198 00199 } 00200 00201 void IMU::accEEWriteEn(bool we) 00202 { 00203 00204 } 00205 00206 /*********************** 00207 * Magnetometer Methods * 00208 ***********************/ 00209 00210
Generated on Fri Jul 15 2022 08:01:50 by 1.7.2