Erik van de Coevering / quadV3

This is my quadcopter prototype software, still in development!

quadv3/ITG3200.cpp@1:ac68f0368a77, 2013-07-23 (annotated)

Committer:
Anaesthetix
Date:
Tue Jul 23 14:01:42 2013 +0000
Revision:
1:ac68f0368a77
Parent:
0:978110f7f027 
Other accelerometer added

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Anaesthetix 0:978110f7f027 1 /**
Anaesthetix 0:978110f7f027 2 * @author Aaron Berk
Anaesthetix 0:978110f7f027 3 *
Anaesthetix 0:978110f7f027 4 * @section LICENSE
Anaesthetix 0:978110f7f027 5 *
Anaesthetix 0:978110f7f027 6 * Copyright (c) 2010 ARM Limited
Anaesthetix 0:978110f7f027 7 *
Anaesthetix 0:978110f7f027 8 * Permission is hereby granted, free of charge, to any person obtaining a copy
Anaesthetix 0:978110f7f027 9 * of this software and associated documentation files (the "Software"), to deal
Anaesthetix 0:978110f7f027 10 * in the Software without restriction, including without limitation the rights
Anaesthetix 0:978110f7f027 11 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
Anaesthetix 0:978110f7f027 12 * copies of the Software, and to permit persons to whom the Software is
Anaesthetix 0:978110f7f027 13 * furnished to do so, subject to the following conditions:
Anaesthetix 0:978110f7f027 14 *
Anaesthetix 0:978110f7f027 15 * The above copyright notice and this permission notice shall be included in
Anaesthetix 0:978110f7f027 16 * all copies or substantial portions of the Software.
Anaesthetix 0:978110f7f027 17 *
Anaesthetix 0:978110f7f027 18 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
Anaesthetix 0:978110f7f027 19 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
Anaesthetix 0:978110f7f027 20 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
Anaesthetix 0:978110f7f027 21 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
Anaesthetix 0:978110f7f027 22 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
Anaesthetix 0:978110f7f027 23 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
Anaesthetix 0:978110f7f027 24 * THE SOFTWARE.
Anaesthetix 0:978110f7f027 25 *
Anaesthetix 0:978110f7f027 26 * @section DESCRIPTION
Anaesthetix 0:978110f7f027 27 *
Anaesthetix 0:978110f7f027 28 * ITG-3200 triple axis, digital interface, gyroscope.
Anaesthetix 0:978110f7f027 29 *
Anaesthetix 0:978110f7f027 30 * Datasheet:
Anaesthetix 0:978110f7f027 31 *
Anaesthetix 0:978110f7f027 32 * http://invensense.com/mems/gyro/documents/PS-ITG-3200-00-01.4.pdf
Anaesthetix 0:978110f7f027 33 */
Anaesthetix 0:978110f7f027 34
Anaesthetix 0:978110f7f027 35 /**
Anaesthetix 0:978110f7f027 36 * Includes
Anaesthetix 0:978110f7f027 37 */
Anaesthetix 0:978110f7f027 38
Anaesthetix 0:978110f7f027 39
Anaesthetix 0:978110f7f027 40 #include "ITG3200.h"
Anaesthetix 0:978110f7f027 41
Anaesthetix 0:978110f7f027 42 ITG3200::ITG3200(PinName sda, PinName scl) : i2c_(sda, scl) {
Anaesthetix 0:978110f7f027 43
Anaesthetix 0:978110f7f027 44 //400kHz, fast mode.
Anaesthetix 0:978110f7f027 45 i2c_.frequency(400000);
Anaesthetix 0:978110f7f027 46 //Set FS_SEL to 0x03 for proper operation.
Anaesthetix 0:978110f7f027 47 //See datasheet for details.
Anaesthetix 0:978110f7f027 48 char tx[2];
Anaesthetix 0:978110f7f027 49 tx[0] = DLPF_FS_REG;
Anaesthetix 0:978110f7f027 50 //FS_SEL bits sit in bits 4 and 3 of DLPF_FS register.
Anaesthetix 0:978110f7f027 51 tx[1] = 0x03 << 3;
Anaesthetix 0:978110f7f027 52 i2c_.write((ITG3200_I2C_ADDRESS << 1) & 0xFE, tx, 2);
Anaesthetix 0:978110f7f027 53 }
Anaesthetix 0:978110f7f027 54 char ITG3200::getWhoAmI(void){
Anaesthetix 0:978110f7f027 55 //WhoAmI Register address.
Anaesthetix 0:978110f7f027 56 char tx = WHO_AM_I_REG;
Anaesthetix 0:978110f7f027 57 char rx;
Anaesthetix 0:978110f7f027 58 i2c_.write((ITG3200_I2C_ADDRESS << 1) & 0xFE, &tx, 1);
Anaesthetix 0:978110f7f027 59 i2c_.read((ITG3200_I2C_ADDRESS << 1) | 0x01, &rx, 1);
Anaesthetix 0:978110f7f027 60 return rx;
Anaesthetix 0:978110f7f027 61 }
Anaesthetix 0:978110f7f027 62 void ITG3200::setWhoAmI(char address){
Anaesthetix 0:978110f7f027 63 char tx[2];
Anaesthetix 0:978110f7f027 64 tx[0] = WHO_AM_I_REG;
Anaesthetix 0:978110f7f027 65 tx[1] = address;
Anaesthetix 0:978110f7f027 66 i2c_.write((ITG3200_I2C_ADDRESS << 1) & 0xFE, tx, 2);
Anaesthetix 0:978110f7f027 67 }
Anaesthetix 0:978110f7f027 68 char ITG3200::getSampleRateDivider(void){
Anaesthetix 0:978110f7f027 69 char tx = SMPLRT_DIV_REG;
Anaesthetix 0:978110f7f027 70 char rx;
Anaesthetix 0:978110f7f027 71 i2c_.write((ITG3200_I2C_ADDRESS << 1) & 0xFE, &tx, 1);
Anaesthetix 0:978110f7f027 72 i2c_.read((ITG3200_I2C_ADDRESS << 1) | 0x01, &rx, 1);
Anaesthetix 0:978110f7f027 73 return rx;
Anaesthetix 0:978110f7f027 74 }
Anaesthetix 0:978110f7f027 75 void ITG3200::setSampleRateDivider(char divider){
Anaesthetix 0:978110f7f027 76 char tx[2];
Anaesthetix 0:978110f7f027 77 tx[0] = SMPLRT_DIV_REG;
Anaesthetix 0:978110f7f027 78 tx[1] = divider;
Anaesthetix 0:978110f7f027 79 i2c_.write((ITG3200_I2C_ADDRESS << 1) & 0xFE, tx, 2);
Anaesthetix 0:978110f7f027 80 }
Anaesthetix 0:978110f7f027 81 int ITG3200::getInternalSampleRate(void){
Anaesthetix 0:978110f7f027 82 char tx = DLPF_FS_REG;
Anaesthetix 0:978110f7f027 83 char rx;
Anaesthetix 0:978110f7f027 84 i2c_.write((ITG3200_I2C_ADDRESS << 1) & 0xFE, &tx, 1);
Anaesthetix 0:978110f7f027 85 i2c_.read((ITG3200_I2C_ADDRESS << 1) | 0x01, &rx, 1);
Anaesthetix 0:978110f7f027 86 //DLPF_CFG == 0 -> sample rate = 8kHz.
Anaesthetix 0:978110f7f027 87 if(rx == 0){
Anaesthetix 0:978110f7f027 88 return 8;
Anaesthetix 0:978110f7f027 89 }
Anaesthetix 0:978110f7f027 90 //DLPF_CFG = 1..7 -> sample rate = 1kHz.
Anaesthetix 0:978110f7f027 91 else if(rx >= 1 && rx <= 7){
Anaesthetix 0:978110f7f027 92 return 1;
Anaesthetix 0:978110f7f027 93 }
Anaesthetix 0:978110f7f027 94 //DLPF_CFG = anything else -> something's wrong!
Anaesthetix 0:978110f7f027 95 else{
Anaesthetix 0:978110f7f027 96 return -1;
Anaesthetix 0:978110f7f027 97 }
Anaesthetix 0:978110f7f027 98 }
Anaesthetix 0:978110f7f027 99 void ITG3200::setLpBandwidth(char bandwidth){
Anaesthetix 0:978110f7f027 100 char tx[2];
Anaesthetix 0:978110f7f027 101 tx[0] = DLPF_FS_REG;
Anaesthetix 0:978110f7f027 102 //Bits 4,3 are required to be 0x03 for proper operation.
Anaesthetix 0:978110f7f027 103 tx[1] = bandwidth | (0x03 << 3);
Anaesthetix 0:978110f7f027 104 i2c_.write((ITG3200_I2C_ADDRESS << 1) & 0xFE, tx, 2);
Anaesthetix 0:978110f7f027 105 }
Anaesthetix 0:978110f7f027 106 char ITG3200::getInterruptConfiguration(void){
Anaesthetix 0:978110f7f027 107 char tx = INT_CFG_REG;
Anaesthetix 0:978110f7f027 108 char rx;
Anaesthetix 0:978110f7f027 109 i2c_.write((ITG3200_I2C_ADDRESS << 1) & 0xFE, &tx, 1);
Anaesthetix 0:978110f7f027 110 i2c_.read((ITG3200_I2C_ADDRESS << 1) | 0x01, &rx, 1);
Anaesthetix 0:978110f7f027 111 return rx;
Anaesthetix 0:978110f7f027 112 }
Anaesthetix 0:978110f7f027 113 void ITG3200::setInterruptConfiguration(char config){
Anaesthetix 0:978110f7f027 114 char tx[2];
Anaesthetix 0:978110f7f027 115 tx[0] = INT_CFG_REG;
Anaesthetix 0:978110f7f027 116 tx[1] = config;
Anaesthetix 0:978110f7f027 117 i2c_.write((ITG3200_I2C_ADDRESS << 1) & 0xFE, tx, 2);
Anaesthetix 0:978110f7f027 118 }
Anaesthetix 0:978110f7f027 119 bool ITG3200::isPllReady(void){
Anaesthetix 0:978110f7f027 120 char tx = INT_STATUS;
Anaesthetix 0:978110f7f027 121 char rx;
Anaesthetix 0:978110f7f027 122 i2c_.write((ITG3200_I2C_ADDRESS << 1) & 0xFE, &tx, 1);
Anaesthetix 0:978110f7f027 123 i2c_.read((ITG3200_I2C_ADDRESS << 1) | 0x01, &rx, 1);
Anaesthetix 0:978110f7f027 124 //ITG_RDY bit is bit 4 of INT_STATUS register.
Anaesthetix 0:978110f7f027 125 if(rx & 0x04){
Anaesthetix 0:978110f7f027 126 return true;
Anaesthetix 0:978110f7f027 127 }
Anaesthetix 0:978110f7f027 128 else{
Anaesthetix 0:978110f7f027 129 return false;
Anaesthetix 0:978110f7f027 130 }
Anaesthetix 0:978110f7f027 131 }
Anaesthetix 0:978110f7f027 132 bool ITG3200::isRawDataReady(void){
Anaesthetix 0:978110f7f027 133 char tx = INT_STATUS;
Anaesthetix 0:978110f7f027 134 char rx;
Anaesthetix 0:978110f7f027 135 i2c_.write((ITG3200_I2C_ADDRESS << 1) & 0xFE, &tx, 1);
Anaesthetix 0:978110f7f027 136 i2c_.read((ITG3200_I2C_ADDRESS << 1) | 0x01, &rx, 1);
Anaesthetix 0:978110f7f027 137 //RAW_DATA_RDY bit is bit 1 of INT_STATUS register.
Anaesthetix 0:978110f7f027 138 if(rx & 0x01){
Anaesthetix 0:978110f7f027 139 return true;
Anaesthetix 0:978110f7f027 140 }
Anaesthetix 0:978110f7f027 141 else{
Anaesthetix 0:978110f7f027 142 return false;
Anaesthetix 0:978110f7f027 143 }
Anaesthetix 0:978110f7f027 144 }
Anaesthetix 0:978110f7f027 145 float ITG3200::getTemperature(void){
Anaesthetix 0:978110f7f027 146 char tx = TEMP_OUT_H_REG;
Anaesthetix 0:978110f7f027 147 char rx[2];
Anaesthetix 0:978110f7f027 148 i2c_.write((ITG3200_I2C_ADDRESS << 1) & 0xFE, &tx, 1);
Anaesthetix 0:978110f7f027 149 i2c_.read((ITG3200_I2C_ADDRESS << 1) | 0x01, rx, 2);
Anaesthetix 0:978110f7f027 150 int16_t temperature = ((int) rx[0] << 8) | ((int) rx[1]);
Anaesthetix 0:978110f7f027 151 //Offset = -35 degrees, 13200 counts. 280 counts/degrees C.
Anaesthetix 0:978110f7f027 152 return 35.0 + ((temperature + 13200)/280.0);
Anaesthetix 0:978110f7f027 153 }
Anaesthetix 0:978110f7f027 154 int ITG3200::getGyroX(void){
Anaesthetix 0:978110f7f027 155 char tx = GYRO_XOUT_H_REG;
Anaesthetix 0:978110f7f027 156 char rx[2];
Anaesthetix 0:978110f7f027 157 i2c_.write((ITG3200_I2C_ADDRESS << 1) & 0xFE, &tx, 1);
Anaesthetix 0:978110f7f027 158 i2c_.read((ITG3200_I2C_ADDRESS << 1) | 0x01, rx, 2);
Anaesthetix 0:978110f7f027 159 int16_t output = ((int) rx[0] << 8) | ((int) rx[1]);
Anaesthetix 0:978110f7f027 160 return output;
Anaesthetix 0:978110f7f027 161 }
Anaesthetix 0:978110f7f027 162 int ITG3200::getGyroY(void){
Anaesthetix 0:978110f7f027 163 char tx = GYRO_YOUT_H_REG;
Anaesthetix 0:978110f7f027 164 char rx[2];
Anaesthetix 0:978110f7f027 165 i2c_.write((ITG3200_I2C_ADDRESS << 1) & 0xFE, &tx, 1);
Anaesthetix 0:978110f7f027 166 i2c_.read((ITG3200_I2C_ADDRESS << 1) | 0x01, rx, 2);
Anaesthetix 0:978110f7f027 167 int16_t output = ((int) rx[0] << 8) | ((int) rx[1]);
Anaesthetix 0:978110f7f027 168 return output;
Anaesthetix 0:978110f7f027 169 }
Anaesthetix 0:978110f7f027 170 int ITG3200::getGyroZ(void){
Anaesthetix 0:978110f7f027 171 char tx = GYRO_ZOUT_H_REG;
Anaesthetix 0:978110f7f027 172 char rx[2];
Anaesthetix 0:978110f7f027 173 i2c_.write((ITG3200_I2C_ADDRESS << 1) & 0xFE, &tx, 1);
Anaesthetix 0:978110f7f027 174 i2c_.read((ITG3200_I2C_ADDRESS << 1) | 0x01, rx, 2);
Anaesthetix 0:978110f7f027 175 int16_t output = ((int) rx[0] << 8) | ((int) rx[1]);
Anaesthetix 0:978110f7f027 176 return output;
Anaesthetix 0:978110f7f027 177 }
Anaesthetix 0:978110f7f027 178 char ITG3200::getPowerManagement(void){
Anaesthetix 0:978110f7f027 179 char tx = PWR_MGM_REG;
Anaesthetix 0:978110f7f027 180 char rx;
Anaesthetix 0:978110f7f027 181 i2c_.write((ITG3200_I2C_ADDRESS << 1) & 0xFE, &tx, 1);
Anaesthetix 0:978110f7f027 182 i2c_.read((ITG3200_I2C_ADDRESS << 1) | 0x01, &rx, 1);
Anaesthetix 0:978110f7f027 183 return rx;
Anaesthetix 0:978110f7f027 184 }
Anaesthetix 0:978110f7f027 185 void ITG3200::setPowerManagement(char config){
Anaesthetix 0:978110f7f027 186 char tx[2];
Anaesthetix 0:978110f7f027 187 tx[0] = PWR_MGM_REG;
Anaesthetix 0:978110f7f027 188 tx[1] = config;
Anaesthetix 0:978110f7f027 189 i2c_.write((ITG3200_I2C_ADDRESS << 1) & 0xFE, tx, 2);
Anaesthetix 0:978110f7f027 190 }
Anaesthetix 0:978110f7f027 191