David Spillman / Mbed 2 deprecated L3G4200D_SPI

L3G4200D SPI driver. Runs at 10mhz vs I2C 400khz. High pass filtered data sets a INT1 pin high when a reference threshold is reached on an axis. Returns the axis that reached the threshold and the DPS of that threshold. Threshold levels can be tweaked by editing void setupL3G4200D()

Dependencies:   mbed

L3G4200D.h@0:66fe7a32bd59, 2014-11-25 (annotated)

Committer:
Spilly
Date:
Tue Nov 25 15:27:47 2014 +0000
Revision:
0:66fe7a32bd59
First release

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Spilly 0:66fe7a32bd59 1 #include "mbed.h"
Spilly 0:66fe7a32bd59 2
Spilly 0:66fe7a32bd59 3 /*************************
Spilly 0:66fe7a32bd59 4 L3G4200D Registers
Spilly 0:66fe7a32bd59 5 *************************/
Spilly 0:66fe7a32bd59 6 #define WHO_AM_I 0x0F
Spilly 0:66fe7a32bd59 7 #define CTRL_REG1 0x20
Spilly 0:66fe7a32bd59 8 #define CTRL_REG2 0x21
Spilly 0:66fe7a32bd59 9 #define CTRL_REG3 0x22
Spilly 0:66fe7a32bd59 10 #define CTRL_REG4 0x23
Spilly 0:66fe7a32bd59 11 #define CTRL_REG5 0x24
Spilly 0:66fe7a32bd59 12 #define REFERENCE 0x25
Spilly 0:66fe7a32bd59 13 #define OUT_TEMP 0x26
Spilly 0:66fe7a32bd59 14 #define STATUS_REG 0x27
Spilly 0:66fe7a32bd59 15 #define OUT_X_L 0x28
Spilly 0:66fe7a32bd59 16 #define OUT_X_H 0x29
Spilly 0:66fe7a32bd59 17 #define OUT_Y_L 0x2A
Spilly 0:66fe7a32bd59 18 #define OUT_Y_H 0x2B
Spilly 0:66fe7a32bd59 19 #define OUT_Z_L 0x2C
Spilly 0:66fe7a32bd59 20 #define OUT_Z_H 0x2D
Spilly 0:66fe7a32bd59 21 #define FIFO_CTRL_REG 0x2E
Spilly 0:66fe7a32bd59 22 #define FIFO_SRC_REG 0x2F
Spilly 0:66fe7a32bd59 23 #define INT1_CFG 0x30
Spilly 0:66fe7a32bd59 24 #define INT1_SRC 0x31
Spilly 0:66fe7a32bd59 25 #define INT1_TSH_XH 0x32
Spilly 0:66fe7a32bd59 26 #define INT1_TSH_XL 0x33
Spilly 0:66fe7a32bd59 27 #define INT1_TSH_YH 0x34
Spilly 0:66fe7a32bd59 28 #define INT1_TSH_YL 0x35
Spilly 0:66fe7a32bd59 29 #define INT1_TSH_ZH 0x36
Spilly 0:66fe7a32bd59 30 #define INT1_TSH_ZL 0x37
Spilly 0:66fe7a32bd59 31 #define INT1_DURATION 0x38
Spilly 0:66fe7a32bd59 32 #define xHigh 2 //axis x 0b00000010
Spilly 0:66fe7a32bd59 33 #define yHigh 8 //axis y 0b00001000
Spilly 0:66fe7a32bd59 34 #define zHigh 32 //axis z 0b00100000
Spilly 0:66fe7a32bd59 35 #define GYRO_CALIBRATION_COUNT 64 //how many samples to take for calibration
Spilly 0:66fe7a32bd59 36 #define GYRO_SAMPLE_PERIOD .001f //differnece of ODR period and time required to read registers
Spilly 0:66fe7a32bd59 37 #define GYRO_SAMPLE_COUNT 1 //number of samples to average
Spilly 0:66fe7a32bd59 38
Spilly 0:66fe7a32bd59 39 DigitalOut chipSelect(PTD4);
Spilly 0:66fe7a32bd59 40 DigitalIn INT1(D0);
Spilly 0:66fe7a32bd59 41 Serial pc(USBTX, USBRX);
Spilly 0:66fe7a32bd59 42
Spilly 0:66fe7a32bd59 43
Spilly 0:66fe7a32bd59 44 SPI L3G4200D(PTD6, PTD7, PTD5);
Spilly 0:66fe7a32bd59 45
Spilly 0:66fe7a32bd59 46 // 16-bit two's comp gyro readings
Spilly 0:66fe7a32bd59 47 int16_t gyro[3] = {0,0,0}, gyroBias[3] = {0,0,0};
Spilly 0:66fe7a32bd59 48
Spilly 0:66fe7a32bd59 49 int readRegister(int address)
Spilly 0:66fe7a32bd59 50 {
Spilly 0:66fe7a32bd59 51 int toRead;
Spilly 0:66fe7a32bd59 52
Spilly 0:66fe7a32bd59 53 address |= 0x80; // This tells the L3G4200D we're reading;
Spilly 0:66fe7a32bd59 54
Spilly 0:66fe7a32bd59 55 chipSelect.write(0);
Spilly 0:66fe7a32bd59 56 L3G4200D.write(address);
Spilly 0:66fe7a32bd59 57 toRead = L3G4200D.write(0x00);
Spilly 0:66fe7a32bd59 58 chipSelect.write(1);
Spilly 0:66fe7a32bd59 59
Spilly 0:66fe7a32bd59 60 return toRead;
Spilly 0:66fe7a32bd59 61 }
Spilly 0:66fe7a32bd59 62
Spilly 0:66fe7a32bd59 63 void writeRegister(int address, int data)
Spilly 0:66fe7a32bd59 64 {
Spilly 0:66fe7a32bd59 65 address &= 0x7F; // This to tell the L3G4200D we're writing
Spilly 0:66fe7a32bd59 66
Spilly 0:66fe7a32bd59 67 chipSelect.write(0);
Spilly 0:66fe7a32bd59 68 L3G4200D.write(address);
Spilly 0:66fe7a32bd59 69 L3G4200D.write(data);
Spilly 0:66fe7a32bd59 70 chipSelect.write(1);
Spilly 0:66fe7a32bd59 71 }
Spilly 0:66fe7a32bd59 72
Spilly 0:66fe7a32bd59 73
Spilly 0:66fe7a32bd59 74 void setRef()
Spilly 0:66fe7a32bd59 75 {
Spilly 0:66fe7a32bd59 76 readRegister(REFERENCE);
Spilly 0:66fe7a32bd59 77 writeRegister(INT1_CFG, 0x6A); //Enable XH, YH and ZH interrupt generation
Spilly 0:66fe7a32bd59 78 //Interrupt latched
Spilly 0:66fe7a32bd59 79 //Gyro.writeReg(L3G_INT1_CFG, 0x80);
Spilly 0:66fe7a32bd59 80 }
Spilly 0:66fe7a32bd59 81
Spilly 0:66fe7a32bd59 82 void getGyroValues()
Spilly 0:66fe7a32bd59 83 {
Spilly 0:66fe7a32bd59 84 gyro[0] = (readRegister(0x29)&0xFF)<<8;
Spilly 0:66fe7a32bd59 85 gyro[0] |= (readRegister(0x28)&0xFF);
Spilly 0:66fe7a32bd59 86 //gyro[0] = gyro[0] - gyroBias[0];
Spilly 0:66fe7a32bd59 87
Spilly 0:66fe7a32bd59 88 gyro[1] = (readRegister(0x2B)&0xFF)<<8;
Spilly 0:66fe7a32bd59 89 gyro[1] |= (readRegister(0x2A)&0xFF);
Spilly 0:66fe7a32bd59 90 //gyro[1] = gyro[1] - gyroBias[1];
Spilly 0:66fe7a32bd59 91
Spilly 0:66fe7a32bd59 92 gyro[2] = (readRegister(0x2D)&0xFF)<<8;
Spilly 0:66fe7a32bd59 93 gyro[2] |= (readRegister(0x2C)&0xFF);
Spilly 0:66fe7a32bd59 94 //gyro[2] = gyro[2] - gyroBias[2];
Spilly 0:66fe7a32bd59 95 }
Spilly 0:66fe7a32bd59 96
Spilly 0:66fe7a32bd59 97 void getGyroBias()
Spilly 0:66fe7a32bd59 98 {
Spilly 0:66fe7a32bd59 99 float accumulator[3] = {0,0,0}, tempStore[3];
Spilly 0:66fe7a32bd59 100 int sampleCount = 0;
Spilly 0:66fe7a32bd59 101
Spilly 0:66fe7a32bd59 102 //Summation of 64 readings
Spilly 0:66fe7a32bd59 103 while (sampleCount < GYRO_CALIBRATION_COUNT)
Spilly 0:66fe7a32bd59 104 {
Spilly 0:66fe7a32bd59 105 //Make sure the accelerometer has had enough time
Spilly 0:66fe7a32bd59 106 //to take a new sample.
Spilly 0:66fe7a32bd59 107 wait(GYRO_SAMPLE_PERIOD);
Spilly 0:66fe7a32bd59 108
Spilly 0:66fe7a32bd59 109 //get accelerometer data
Spilly 0:66fe7a32bd59 110 getGyroValues();
Spilly 0:66fe7a32bd59 111
Spilly 0:66fe7a32bd59 112 for(int i = 0; i < 3; i++)
Spilly 0:66fe7a32bd59 113 {
Spilly 0:66fe7a32bd59 114 //add current sample to previous samples
Spilly 0:66fe7a32bd59 115 accumulator[i] += tempStore[i];
Spilly 0:66fe7a32bd59 116 }
Spilly 0:66fe7a32bd59 117
Spilly 0:66fe7a32bd59 118 sampleCount++;
Spilly 0:66fe7a32bd59 119 }
Spilly 0:66fe7a32bd59 120 for(int i = 0; i < 3; i++)
Spilly 0:66fe7a32bd59 121 {
Spilly 0:66fe7a32bd59 122 //divide by number of samples
Spilly 0:66fe7a32bd59 123 tempStore[i] = accumulator[i] / GYRO_CALIBRATION_COUNT;
Spilly 0:66fe7a32bd59 124 gyroBias[i] = (int16_t)tempStore[i];
Spilly 0:66fe7a32bd59 125 }
Spilly 0:66fe7a32bd59 126 }
Spilly 0:66fe7a32bd59 127
Spilly 0:66fe7a32bd59 128 void getGyroAvg()
Spilly 0:66fe7a32bd59 129 {
Spilly 0:66fe7a32bd59 130 float accumulator[3] = {0,0,0}, tempStore[3];
Spilly 0:66fe7a32bd59 131 int sampleCount = 0;
Spilly 0:66fe7a32bd59 132
Spilly 0:66fe7a32bd59 133 //Summation of 64 readings
Spilly 0:66fe7a32bd59 134 while (sampleCount < GYRO_SAMPLE_COUNT)
Spilly 0:66fe7a32bd59 135 {
Spilly 0:66fe7a32bd59 136 //Make sure the accelerometer has had enough time
Spilly 0:66fe7a32bd59 137 //to take a new sample.
Spilly 0:66fe7a32bd59 138 wait(GYRO_SAMPLE_PERIOD);
Spilly 0:66fe7a32bd59 139
Spilly 0:66fe7a32bd59 140 //get accelerometer data
Spilly 0:66fe7a32bd59 141 getGyroValues();
Spilly 0:66fe7a32bd59 142
Spilly 0:66fe7a32bd59 143 for(int i = 0; i < 3; i++)
Spilly 0:66fe7a32bd59 144 {
Spilly 0:66fe7a32bd59 145 //add current sample to previous samples
Spilly 0:66fe7a32bd59 146 accumulator[i] += tempStore[i];
Spilly 0:66fe7a32bd59 147 }
Spilly 0:66fe7a32bd59 148
Spilly 0:66fe7a32bd59 149 sampleCount++;
Spilly 0:66fe7a32bd59 150 }
Spilly 0:66fe7a32bd59 151 for(int i = 0; i < 3; i++)
Spilly 0:66fe7a32bd59 152 {
Spilly 0:66fe7a32bd59 153 //divide by number of samples
Spilly 0:66fe7a32bd59 154 tempStore[i] = accumulator[i] / GYRO_CALIBRATION_COUNT;
Spilly 0:66fe7a32bd59 155 gyro[i] = (int16_t)tempStore[i];
Spilly 0:66fe7a32bd59 156 }
Spilly 0:66fe7a32bd59 157 }
Spilly 0:66fe7a32bd59 158
Spilly 0:66fe7a32bd59 159 void setupL3G4200D()
Spilly 0:66fe7a32bd59 160 {
Spilly 0:66fe7a32bd59 161 L3G4200D.format(8, 3); //Set SPI mode to CPOL = 1 and CPHA = 1
Spilly 0:66fe7a32bd59 162 //8-bit transmissions
Spilly 0:66fe7a32bd59 163
Spilly 0:66fe7a32bd59 164 L3G4200D.frequency(10000000); //10MHz SPI frequency
Spilly 0:66fe7a32bd59 165
Spilly 0:66fe7a32bd59 166 chipSelect.write(1);
Spilly 0:66fe7a32bd59 167 wait(.1);
Spilly 0:66fe7a32bd59 168
Spilly 0:66fe7a32bd59 169 chipSelect.write(0);
Spilly 0:66fe7a32bd59 170 L3G4200D.write(0); //Work around for clock not being set high when initiating SPI
Spilly 0:66fe7a32bd59 171 chipSelect.write(1);
Spilly 0:66fe7a32bd59 172
Spilly 0:66fe7a32bd59 173 //writeRegister(CTRL_REG1, 0xCF); //normal power mode, all axes enabled, 800 Hz
Spilly 0:66fe7a32bd59 174 writeRegister(CTRL_REG1, 0xCF); //normal power mode, all axes enabled, 100 Hz
Spilly 0:66fe7a32bd59 175
Spilly 0:66fe7a32bd59 176 writeRegister(CTRL_REG2, 0x00); //High-pass filter disabled
Spilly 0:66fe7a32bd59 177 writeRegister(CTRL_REG3, 0x80); //Interrupt driven to INT1 pad
Spilly 0:66fe7a32bd59 178 writeRegister(CTRL_REG4, 0x00); //250 dps full scale
Spilly 0:66fe7a32bd59 179 writeRegister(CTRL_REG5, 0x05); //Data in DataReg and FIFO are high-pass filtered
Spilly 0:66fe7a32bd59 180 //High-pass-filtered data are used for interrupt
Spilly 0:66fe7a32bd59 181 //generation
Spilly 0:66fe7a32bd59 182
Spilly 0:66fe7a32bd59 183 writeRegister(INT1_TSH_XH, 0x00); //X HIGH threshold
Spilly 0:66fe7a32bd59 184 writeRegister(INT1_TSH_XL, 0x60); //X LOW threshold
Spilly 0:66fe7a32bd59 185 writeRegister(INT1_TSH_YH, 0x00); //Y HIGH threshold
Spilly 0:66fe7a32bd59 186 writeRegister(INT1_TSH_YL, 0x60); //Y LOW threshold
Spilly 0:66fe7a32bd59 187 writeRegister(INT1_TSH_ZH, 0x00); //Z HIGH threshold
Spilly 0:66fe7a32bd59 188 writeRegister(INT1_TSH_ZL, 0x60); //Z LOW threshold
Spilly 0:66fe7a32bd59 189
Spilly 0:66fe7a32bd59 190 writeRegister(INT1_DURATION, 0x01); //Duration = 10ms
Spilly 0:66fe7a32bd59 191
Spilly 0:66fe7a32bd59 192
Spilly 0:66fe7a32bd59 193 setRef(); //Device must remain stationary while setting the gyro reference level
Spilly 0:66fe7a32bd59 194
Spilly 0:66fe7a32bd59 195 //getGyroBias();
Spilly 0:66fe7a32bd59 196 }
Spilly 0:66fe7a32bd59 197
Spilly 0:66fe7a32bd59 198 void checkGyro()
Spilly 0:66fe7a32bd59 199 {
Spilly 0:66fe7a32bd59 200 int interrSource = readRegister(INT1_SRC);
Spilly 0:66fe7a32bd59 201 int compare = interrSource&xHigh;
Spilly 0:66fe7a32bd59 202
Spilly 0:66fe7a32bd59 203 //X-Axis
Spilly 0:66fe7a32bd59 204 if(compare == 2)
Spilly 0:66fe7a32bd59 205 {
Spilly 0:66fe7a32bd59 206 //getGyroAvg();
Spilly 0:66fe7a32bd59 207 getGyroValues();
Spilly 0:66fe7a32bd59 208 pc.printf("X: %i\n", gyro[0]);
Spilly 0:66fe7a32bd59 209
Spilly 0:66fe7a32bd59 210 compare = interrSource&yHigh;
Spilly 0:66fe7a32bd59 211 //Y-Axis
Spilly 0:66fe7a32bd59 212 if(compare == 8)
Spilly 0:66fe7a32bd59 213 {
Spilly 0:66fe7a32bd59 214 pc.printf("Y: %i\n", gyro[1]);
Spilly 0:66fe7a32bd59 215 }
Spilly 0:66fe7a32bd59 216 compare = interrSource&zHigh;
Spilly 0:66fe7a32bd59 217 //Z-Axis
Spilly 0:66fe7a32bd59 218 if(compare == 32)
Spilly 0:66fe7a32bd59 219 {
Spilly 0:66fe7a32bd59 220 pc.printf("Z: %i\n", gyro[2]);
Spilly 0:66fe7a32bd59 221 }
Spilly 0:66fe7a32bd59 222 }
Spilly 0:66fe7a32bd59 223
Spilly 0:66fe7a32bd59 224 else
Spilly 0:66fe7a32bd59 225 {
Spilly 0:66fe7a32bd59 226
Spilly 0:66fe7a32bd59 227 compare = interrSource&yHigh;
Spilly 0:66fe7a32bd59 228 //Y-Axis
Spilly 0:66fe7a32bd59 229 if(compare == 8)
Spilly 0:66fe7a32bd59 230 {
Spilly 0:66fe7a32bd59 231
Spilly 0:66fe7a32bd59 232 //getGyroAvg();
Spilly 0:66fe7a32bd59 233 getGyroValues();
Spilly 0:66fe7a32bd59 234 pc.printf("Y: %i\n", gyro[1]);
Spilly 0:66fe7a32bd59 235
Spilly 0:66fe7a32bd59 236 //Z-Axis
Spilly 0:66fe7a32bd59 237 compare = interrSource&zHigh;
Spilly 0:66fe7a32bd59 238 if(compare == 32)
Spilly 0:66fe7a32bd59 239 {
Spilly 0:66fe7a32bd59 240 pc.printf("Z: %i\n", gyro[2]);
Spilly 0:66fe7a32bd59 241 }
Spilly 0:66fe7a32bd59 242
Spilly 0:66fe7a32bd59 243 }
Spilly 0:66fe7a32bd59 244 else
Spilly 0:66fe7a32bd59 245 {
Spilly 0:66fe7a32bd59 246 //Z-Axis
Spilly 0:66fe7a32bd59 247 compare = interrSource&zHigh;
Spilly 0:66fe7a32bd59 248 if(compare == 32)
Spilly 0:66fe7a32bd59 249 {
Spilly 0:66fe7a32bd59 250 //getGyroAvg();
Spilly 0:66fe7a32bd59 251 getGyroValues();
Spilly 0:66fe7a32bd59 252
Spilly 0:66fe7a32bd59 253 pc.printf("Z: %i\n", gyro[2]);
Spilly 0:66fe7a32bd59 254 }
Spilly 0:66fe7a32bd59 255 }
Spilly 0:66fe7a32bd59 256 }
Spilly 0:66fe7a32bd59 257 }