Esta versión v6 pasa a ser el nuevo master. Funciona correctamente
Dependencies: ADXL345 Display1602 MSCFileSystem SDFileSystem mbed FATFileSystem
BMA180.cpp@2:cc4a43d806e2, 2014-06-06 (annotated)
- Committer:
- JuanManuelAmador
- Date:
- Fri Jun 06 08:52:28 2014 +0000
- Revision:
- 2:cc4a43d806e2
- Parent:
- 0:a5367bd4e404
Correci?n del baudrate de 112500 a 115200
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
jjmedina | 0:a5367bd4e404 | 1 | /** |
jjmedina | 0:a5367bd4e404 | 2 | * @author Jose L. Escalona (virtualmech) |
jjmedina | 0:a5367bd4e404 | 3 | */ |
jjmedina | 0:a5367bd4e404 | 4 | |
jjmedina | 0:a5367bd4e404 | 5 | #include "BMA180.h" |
jjmedina | 0:a5367bd4e404 | 6 | |
jjmedina | 0:a5367bd4e404 | 7 | BMA180::BMA180(PinName mosi, PinName miso, PinName sck, PinName cs) : spi_(mosi, miso, sck), nCS_(cs) |
jjmedina | 0:a5367bd4e404 | 8 | { |
jjmedina | 0:a5367bd4e404 | 9 | nCS_ = 1; |
jjmedina | 0:a5367bd4e404 | 10 | |
jjmedina | 0:a5367bd4e404 | 11 | wait_us(500); |
jjmedina | 0:a5367bd4e404 | 12 | |
jjmedina | 0:a5367bd4e404 | 13 | char readVersion, readID; |
jjmedina | 0:a5367bd4e404 | 14 | char byte; |
jjmedina | 0:a5367bd4e404 | 15 | // Setup the spi for 8 bit data, high steady state clock, |
jjmedina | 0:a5367bd4e404 | 16 | // second edge capture, with a 10MHz clock rate |
jjmedina | 0:a5367bd4e404 | 17 | |
jjmedina | 0:a5367bd4e404 | 18 | spi_.frequency(10000000); // 10 mHz page 59 of the BMA180 datasheet |
jjmedina | 0:a5367bd4e404 | 19 | spi_.format(8,3); // Not conform !!! page 58 of the BMA180 datasheet) |
jjmedina | 0:a5367bd4e404 | 20 | wait_ms(100); |
jjmedina | 0:a5367bd4e404 | 21 | readID = read_reg(BMA180_ID); |
jjmedina | 0:a5367bd4e404 | 22 | readVersion = read_reg(VERSION); |
jjmedina | 0:a5367bd4e404 | 23 | |
jjmedina | 0:a5367bd4e404 | 24 | /*pc.printf("\n\r"); |
jjmedina | 0:a5367bd4e404 | 25 | if (readID == 3) { |
jjmedina | 0:a5367bd4e404 | 26 | pc.printf("Connected to BMA180\n\r"); |
jjmedina | 0:a5367bd4e404 | 27 | pc.printf("BMA180 Version %d\n\r", readVersion); |
jjmedina | 0:a5367bd4e404 | 28 | } else |
jjmedina | 0:a5367bd4e404 | 29 | pc.printf("Sorry not connected to BMA180 !!!\n\r", readID);*/ |
jjmedina | 0:a5367bd4e404 | 30 | |
jjmedina | 0:a5367bd4e404 | 31 | soft_reset(); // to copy EEprom into volatile area |
jjmedina | 0:a5367bd4e404 | 32 | |
jjmedina | 0:a5367bd4e404 | 33 | //--------------------------- |
jjmedina | 0:a5367bd4e404 | 34 | byte = read_reg(CTRL_REG0); // Unlock image writing |
jjmedina | 0:a5367bd4e404 | 35 | byte |= 0x10; // Set bit 4 |
jjmedina | 0:a5367bd4e404 | 36 | write_reg(CTRL_REG0,byte); // Have to set ee_w to |
jjmedina | 0:a5367bd4e404 | 37 | |
jjmedina | 0:a5367bd4e404 | 38 | //------------------------ |
jjmedina | 0:a5367bd4e404 | 39 | byte= read_reg(DIS_I2C); // read |
jjmedina | 0:a5367bd4e404 | 40 | byte |= 0x01; // set bit0 to 1, SPI only |
jjmedina | 0:a5367bd4e404 | 41 | write_reg(DIS_I2C, byte); // Set spi, disable i2c, page 31 |
jjmedina | 0:a5367bd4e404 | 42 | |
jjmedina | 0:a5367bd4e404 | 43 | |
jjmedina | 0:a5367bd4e404 | 44 | // SET RANGO // |
jjmedina | 0:a5367bd4e404 | 45 | |
jjmedina | 0:a5367bd4e404 | 46 | byte= read_reg(RANGE); // read |
jjmedina | 0:a5367bd4e404 | 47 | byte &= 0xF1; // set to 0 all 3 bits related to range |
jjmedina | 0:a5367bd4e404 | 48 | byte |= 0x02; // Rango +- 1.5g |
jjmedina | 0:a5367bd4e404 | 49 | write_reg(RANGE, byte); // Set RANGO, page 27 |
jjmedina | 0:a5367bd4e404 | 50 | |
jjmedina | 0:a5367bd4e404 | 51 | // SET FILTER BAND WITH // |
jjmedina | 0:a5367bd4e404 | 52 | |
jjmedina | 0:a5367bd4e404 | 53 | byte= read_reg(FILTER_BW); // read |
jjmedina | 0:a5367bd4e404 | 54 | byte &= 0x0F; // set to 0 all 4 bits related to filter |
jjmedina | 0:a5367bd4e404 | 55 | byte |= 0x70; // Filtro 1200 Hz (luego resulta ser un filtro de 472 Hz por ultra-low noise, ver p. 28 de datas sheet) |
jjmedina | 0:a5367bd4e404 | 56 | write_reg(FILTER_BW, byte); // Set BW, page 27 |
jjmedina | 0:a5367bd4e404 | 57 | |
jjmedina | 0:a5367bd4e404 | 58 | // SET NOISE Y POWER (mode_config)// |
jjmedina | 0:a5367bd4e404 | 59 | |
jjmedina | 0:a5367bd4e404 | 60 | byte= read_reg(NOISE_POWER); // read |
jjmedina | 0:a5367bd4e404 | 61 | byte &= 0xFC; // set to 0 all 2 bits related to mode configuration |
jjmedina | 0:a5367bd4e404 | 62 | byte |= 0x01; // Ultra low noise |
jjmedina | 0:a5367bd4e404 | 63 | write_reg(NOISE_POWER, byte); // Set mode_config, page 28 |
jjmedina | 0:a5367bd4e404 | 64 | |
jjmedina | 0:a5367bd4e404 | 65 | //------------------------- |
jjmedina | 0:a5367bd4e404 | 66 | //byte = read_reg(CTRL_REG3); |
jjmedina | 0:a5367bd4e404 | 67 | //byte |= 0x02; // set bit 1 enable interrupt |
jjmedina | 0:a5367bd4e404 | 68 | //byte |= 0x40; // set bit 6 slope mode |
jjmedina | 0:a5367bd4e404 | 69 | //byte |= 0x80; // set bit 7 slope alert |
jjmedina | 0:a5367bd4e404 | 70 | //write_reg(CTRL_REG3,byte); // |
jjmedina | 0:a5367bd4e404 | 71 | //pc.printf("Enable interrupt bis is set "); |
jjmedina | 0:a5367bd4e404 | 72 | |
jjmedina | 0:a5367bd4e404 | 73 | |
jjmedina | 0:a5367bd4e404 | 74 | |
jjmedina | 0:a5367bd4e404 | 75 | //------------------------------ |
jjmedina | 0:a5367bd4e404 | 76 | byte = read_reg(CTRL_REG0); // Lock image writing |
jjmedina | 0:a5367bd4e404 | 77 | byte &= 0xEF; // REset bit 4 |
jjmedina | 0:a5367bd4e404 | 78 | write_reg(CTRL_REG0,byte); // Have to set ee_w to |
jjmedina | 0:a5367bd4e404 | 79 | //------------------------------------------------------------------------------------- |
jjmedina | 0:a5367bd4e404 | 80 | //pc.printf("\n\rBMA init done \n\r"); |
jjmedina | 0:a5367bd4e404 | 81 | |
jjmedina | 0:a5367bd4e404 | 82 | } |
jjmedina | 0:a5367bd4e404 | 83 | |
jjmedina | 0:a5367bd4e404 | 84 | void BMA180::soft_reset(void) { |
jjmedina | 0:a5367bd4e404 | 85 | // Write a soft reset |
jjmedina | 0:a5367bd4e404 | 86 | // to copy EEprom into volatile area, see mid page 22 |
jjmedina | 0:a5367bd4e404 | 87 | write_reg(RESET, 0xB6); // page 48 |
jjmedina | 0:a5367bd4e404 | 88 | wait_ms(10); // wait 10 ms, see page 49 |
jjmedina | 0:a5367bd4e404 | 89 | //pc.printf("Soft reset, EEPROM copied \n\r"); |
jjmedina | 0:a5367bd4e404 | 90 | } |
jjmedina | 0:a5367bd4e404 | 91 | |
jjmedina | 0:a5367bd4e404 | 92 | void BMA180::ReadAccels_BMA180(int* Acc) { |
jjmedina | 0:a5367bd4e404 | 93 | char x_lsb, x_msb; |
jjmedina | 0:a5367bd4e404 | 94 | char y_lsb, y_msb; |
jjmedina | 0:a5367bd4e404 | 95 | char z_lsb, z_msb; |
jjmedina | 0:a5367bd4e404 | 96 | signed short ax, ay, az; |
jjmedina | 0:a5367bd4e404 | 97 | |
jjmedina | 0:a5367bd4e404 | 98 | //------------X---------------- |
jjmedina | 0:a5367bd4e404 | 99 | x_lsb = read_reg(ACCXLSB); |
jjmedina | 0:a5367bd4e404 | 100 | x_msb = read_reg(ACCXMSB); |
jjmedina | 0:a5367bd4e404 | 101 | ax = (x_msb << 8) | x_lsb ; // combineer msb en lsb |
jjmedina | 0:a5367bd4e404 | 102 | ax = ax >> 2; // Get rid of two non-value bits in LSB |
jjmedina | 0:a5367bd4e404 | 103 | //------------Y---------------- |
jjmedina | 0:a5367bd4e404 | 104 | y_lsb = read_reg(ACCYLSB); |
jjmedina | 0:a5367bd4e404 | 105 | y_msb = read_reg(ACCYMSB); |
jjmedina | 0:a5367bd4e404 | 106 | ay = (y_msb << 8) | y_lsb; // combineer msb en lsb |
jjmedina | 0:a5367bd4e404 | 107 | ay = ay >> 2; // Get rid of two non-value bits in LSB |
jjmedina | 0:a5367bd4e404 | 108 | //------------Z---------------- |
jjmedina | 0:a5367bd4e404 | 109 | z_lsb = read_reg(ACCZLSB); |
jjmedina | 0:a5367bd4e404 | 110 | z_msb = read_reg(ACCZMSB); |
jjmedina | 0:a5367bd4e404 | 111 | az = (z_msb << 8) | z_lsb; // combineer msb en lsb |
jjmedina | 0:a5367bd4e404 | 112 | az = az >> 2; // Get rid of two non-value bits in LSB |
jjmedina | 0:a5367bd4e404 | 113 | |
jjmedina | 0:a5367bd4e404 | 114 | Acc[0] = ax; |
jjmedina | 0:a5367bd4e404 | 115 | Acc[1] = ay; |
jjmedina | 0:a5367bd4e404 | 116 | Acc[2] = az; |
jjmedina | 0:a5367bd4e404 | 117 | } |
jjmedina | 0:a5367bd4e404 | 118 | |
jjmedina | 0:a5367bd4e404 | 119 | //-----------------READ OUT the X-Y-Z values--------------- |
jjmedina | 0:a5367bd4e404 | 120 | |
jjmedina | 0:a5367bd4e404 | 121 | void BMA180::write_reg(uint8_t address, char data) { |
jjmedina | 0:a5367bd4e404 | 122 | address &= 0x7F; //Force a write (bit 7=0) |
jjmedina | 0:a5367bd4e404 | 123 | nCS_ = 0; //Select SPI device |
jjmedina | 0:a5367bd4e404 | 124 | wait_us(2); |
jjmedina | 0:a5367bd4e404 | 125 | spi_.write(address); //Send register location |
jjmedina | 0:a5367bd4e404 | 126 | wait_us(2); |
jjmedina | 0:a5367bd4e404 | 127 | spi_.write(data); //Send value to record into register |
jjmedina | 0:a5367bd4e404 | 128 | wait_us(2); |
jjmedina | 0:a5367bd4e404 | 129 | nCS_ = 1; |
jjmedina | 0:a5367bd4e404 | 130 | wait_us(2); |
jjmedina | 0:a5367bd4e404 | 131 | } |
jjmedina | 0:a5367bd4e404 | 132 | |
jjmedina | 0:a5367bd4e404 | 133 | char BMA180::read_reg(uint8_t address) { |
jjmedina | 0:a5367bd4e404 | 134 | char byte; |
jjmedina | 0:a5367bd4e404 | 135 | address |= 0x80; //Force a read (bit 7=1) |
jjmedina | 0:a5367bd4e404 | 136 | nCS_ = 0; |
jjmedina | 0:a5367bd4e404 | 137 | wait_us(2); //Select SPI device |
jjmedina | 0:a5367bd4e404 | 138 | spi_.write(address); //Send register location |
jjmedina | 0:a5367bd4e404 | 139 | wait_us(2); |
jjmedina | 0:a5367bd4e404 | 140 | byte=spi_.write(0xFF); //Get the data |
jjmedina | 0:a5367bd4e404 | 141 | wait_us(2); |
jjmedina | 0:a5367bd4e404 | 142 | nCS_ = 1; |
jjmedina | 0:a5367bd4e404 | 143 | wait_us(2); |
jjmedina | 0:a5367bd4e404 | 144 | return byte; |
jjmedina | 0:a5367bd4e404 | 145 | } |
jjmedina | 0:a5367bd4e404 | 146 | |
jjmedina | 0:a5367bd4e404 | 147 | void BMA180::disable_int(void) { |
jjmedina | 0:a5367bd4e404 | 148 | char byte; |
jjmedina | 0:a5367bd4e404 | 149 | byte = read_reg(CTRL_REG0); // Unlock image writing |
jjmedina | 0:a5367bd4e404 | 150 | byte |= 0x10; // Set bit 4 |
jjmedina | 0:a5367bd4e404 | 151 | write_reg(CTRL_REG0,byte); // Have to set ee_w to |
jjmedina | 0:a5367bd4e404 | 152 | //------------------------- |
jjmedina | 0:a5367bd4e404 | 153 | byte = read_reg(CTRL_REG3); |
jjmedina | 0:a5367bd4e404 | 154 | byte &= 0xFD; // REset bit 1 enable interrupt |
jjmedina | 0:a5367bd4e404 | 155 | write_reg(CTRL_REG3,byte); // |
jjmedina | 0:a5367bd4e404 | 156 | //pc.printf("\n\rDisable interrupt bis is set "); |
jjmedina | 0:a5367bd4e404 | 157 | //------------------------------ |
jjmedina | 0:a5367bd4e404 | 158 | byte = read_reg(CTRL_REG0); // Lock image writing |
jjmedina | 0:a5367bd4e404 | 159 | byte &= 0xEF; // REset bit 4 |
jjmedina | 0:a5367bd4e404 | 160 | write_reg(CTRL_REG0,byte); // Have to set ee_w to |
jjmedina | 0:a5367bd4e404 | 161 | //pc.printf("\n\rMBA180 in now switched off "); |
jjmedina | 0:a5367bd4e404 | 162 | } |
jjmedina | 0:a5367bd4e404 | 163 | |
jjmedina | 0:a5367bd4e404 | 164 | void BMA180::AcelerometroSleep(void) //OJO, NO ESTOY SEGURO DE QUE ESTA FUNCION ESTÉ BIEN IMPLEMENTADA |
jjmedina | 0:a5367bd4e404 | 165 | { |
jjmedina | 0:a5367bd4e404 | 166 | char byte; |
jjmedina | 0:a5367bd4e404 | 167 | |
jjmedina | 0:a5367bd4e404 | 168 | byte = read_reg(0x0D); |
jjmedina | 0:a5367bd4e404 | 169 | byte &= 0xFD; |
jjmedina | 0:a5367bd4e404 | 170 | byte |= 0x02; |
jjmedina | 0:a5367bd4e404 | 171 | write_reg(0x0D, byte); |
jjmedina | 0:a5367bd4e404 | 172 | } |
jjmedina | 0:a5367bd4e404 | 173 | void BMA180::AcelerometroWakeUp(void) //OJO, NO ESTOY SEGURO DE QUE ESTA FUNCION ESTÉ BIEN IMPLEMENTADA |
jjmedina | 0:a5367bd4e404 | 174 | { |
jjmedina | 0:a5367bd4e404 | 175 | char byte; |
jjmedina | 0:a5367bd4e404 | 176 | |
jjmedina | 0:a5367bd4e404 | 177 | byte = read_reg(0x0D); |
jjmedina | 0:a5367bd4e404 | 178 | byte &= 0xFD; |
jjmedina | 0:a5367bd4e404 | 179 | write_reg(0x0D, byte); |
jjmedina | 0:a5367bd4e404 | 180 | } |
jjmedina | 0:a5367bd4e404 | 181 | |
jjmedina | 0:a5367bd4e404 | 182 | void BMA180::AcelerometroLeeOffsets(int * offSet) //Lee los valores de los 3 offsets: x, y, z |
jjmedina | 0:a5367bd4e404 | 183 | { |
jjmedina | 0:a5367bd4e404 | 184 | char x_lsb, x_msb; |
jjmedina | 0:a5367bd4e404 | 185 | char y_lsb, y_msb; |
jjmedina | 0:a5367bd4e404 | 186 | char z_lsb, z_msb; |
jjmedina | 0:a5367bd4e404 | 187 | signed short offx, offy, offz; |
jjmedina | 0:a5367bd4e404 | 188 | |
jjmedina | 0:a5367bd4e404 | 189 | offx = 0; |
jjmedina | 0:a5367bd4e404 | 190 | offy = 0; |
jjmedina | 0:a5367bd4e404 | 191 | offz = 0; |
jjmedina | 0:a5367bd4e404 | 192 | |
jjmedina | 0:a5367bd4e404 | 193 | x_msb = read_reg(0x38); |
jjmedina | 0:a5367bd4e404 | 194 | y_msb = read_reg(0x39); |
jjmedina | 0:a5367bd4e404 | 195 | z_msb = read_reg(0x3A); |
jjmedina | 0:a5367bd4e404 | 196 | |
jjmedina | 0:a5367bd4e404 | 197 | x_lsb = read_reg(0x35); |
jjmedina | 0:a5367bd4e404 | 198 | y_lsb = read_reg(0x36) << 4; |
jjmedina | 0:a5367bd4e404 | 199 | z_lsb = read_reg(0x36); |
jjmedina | 0:a5367bd4e404 | 200 | |
jjmedina | 0:a5367bd4e404 | 201 | //------------X---------------- |
jjmedina | 0:a5367bd4e404 | 202 | offx = (x_msb << 8) | x_lsb ; // combineer msb en lsb |
jjmedina | 0:a5367bd4e404 | 203 | offx = offx >> 4; // Get rid of 4 non-value bits in LSB |
jjmedina | 0:a5367bd4e404 | 204 | //------------Y---------------- |
jjmedina | 0:a5367bd4e404 | 205 | offy = (y_msb << 8) | y_lsb; // combineer msb en lsb |
jjmedina | 0:a5367bd4e404 | 206 | offy = offy >> 4; // Get rid of 4 non-value bits in LSB |
jjmedina | 0:a5367bd4e404 | 207 | //------------Z---------------- |
jjmedina | 0:a5367bd4e404 | 208 | offz = (z_msb << 8) | z_lsb; // combineer msb en lsb |
jjmedina | 0:a5367bd4e404 | 209 | offz = offz >> 4; // Get rid of 4 non-value bits in LSB |
jjmedina | 0:a5367bd4e404 | 210 | |
jjmedina | 0:a5367bd4e404 | 211 | offSet[0] = offx; |
jjmedina | 0:a5367bd4e404 | 212 | offSet[1] = offy; |
jjmedina | 0:a5367bd4e404 | 213 | offSet[2] = offz; |
jjmedina | 0:a5367bd4e404 | 214 | } |
jjmedina | 0:a5367bd4e404 | 215 | |
jjmedina | 0:a5367bd4e404 | 216 | |
jjmedina | 0:a5367bd4e404 | 217 | void BMA180::AcelerometroEscribeOffsets(char * offSet) //Escribe los valores de los 3 offsets: x, y, z |
jjmedina | 0:a5367bd4e404 | 218 | { |
jjmedina | 0:a5367bd4e404 | 219 | ////////////////////////// |
jjmedina | 0:a5367bd4e404 | 220 | // Unlock image writing // |
jjmedina | 0:a5367bd4e404 | 221 | ////////////////////////// |
jjmedina | 0:a5367bd4e404 | 222 | |
jjmedina | 0:a5367bd4e404 | 223 | char byte; |
jjmedina | 0:a5367bd4e404 | 224 | byte = read_reg(CTRL_REG0); // Unlock image writing |
jjmedina | 0:a5367bd4e404 | 225 | byte |= 0x10; // Set bit 4 |
jjmedina | 0:a5367bd4e404 | 226 | write_reg(CTRL_REG0,byte); // Have to set ee_w to |
jjmedina | 0:a5367bd4e404 | 227 | |
jjmedina | 0:a5367bd4e404 | 228 | char x_lsb, x_msb; |
jjmedina | 0:a5367bd4e404 | 229 | char y_lsb, y_msb; |
jjmedina | 0:a5367bd4e404 | 230 | char z_lsb, z_msb; |
jjmedina | 0:a5367bd4e404 | 231 | char reg_x, reg_yz; |
jjmedina | 0:a5367bd4e404 | 232 | |
jjmedina | 0:a5367bd4e404 | 233 | |
jjmedina | 0:a5367bd4e404 | 234 | x_msb = offSet[0]; |
jjmedina | 0:a5367bd4e404 | 235 | y_msb = offSet[1]; |
jjmedina | 0:a5367bd4e404 | 236 | z_msb = offSet[2]; |
jjmedina | 0:a5367bd4e404 | 237 | |
jjmedina | 0:a5367bd4e404 | 238 | x_lsb = offSet[3]; |
jjmedina | 0:a5367bd4e404 | 239 | y_lsb = offSet[4]; |
jjmedina | 0:a5367bd4e404 | 240 | z_lsb = offSet[5]; |
jjmedina | 0:a5367bd4e404 | 241 | |
jjmedina | 0:a5367bd4e404 | 242 | |
jjmedina | 0:a5367bd4e404 | 243 | // offset_finetuning to 11 |
jjmedina | 0:a5367bd4e404 | 244 | char fine_tu; |
jjmedina | 0:a5367bd4e404 | 245 | fine_tu = read_reg(0x22); |
jjmedina | 0:a5367bd4e404 | 246 | fine_tu &= 0xFC; |
jjmedina | 0:a5367bd4e404 | 247 | fine_tu |= 0x01; |
jjmedina | 0:a5367bd4e404 | 248 | write_reg(0x22,fine_tu); |
jjmedina | 0:a5367bd4e404 | 249 | |
jjmedina | 0:a5367bd4e404 | 250 | /////////////////////////////// |
jjmedina | 0:a5367bd4e404 | 251 | /// ESTABLECE EL OFFSET EN X // |
jjmedina | 0:a5367bd4e404 | 252 | /////////////////////////////// |
jjmedina | 0:a5367bd4e404 | 253 | |
jjmedina | 0:a5367bd4e404 | 254 | //Enable offset X |
jjmedina | 0:a5367bd4e404 | 255 | fine_tu = read_reg(0x0E); |
jjmedina | 0:a5367bd4e404 | 256 | fine_tu &= 0x7F; |
jjmedina | 0:a5367bd4e404 | 257 | fine_tu |= 0x80; |
jjmedina | 0:a5367bd4e404 | 258 | write_reg(0x0E,fine_tu); |
jjmedina | 0:a5367bd4e404 | 259 | |
jjmedina | 0:a5367bd4e404 | 260 | write_reg(0x38,x_msb); |
jjmedina | 0:a5367bd4e404 | 261 | |
jjmedina | 0:a5367bd4e404 | 262 | //Set X |
jjmedina | 0:a5367bd4e404 | 263 | reg_x = read_reg(0x35); |
jjmedina | 0:a5367bd4e404 | 264 | reg_x &= 0x0F; |
jjmedina | 0:a5367bd4e404 | 265 | x_lsb = x_lsb << 4; |
jjmedina | 0:a5367bd4e404 | 266 | reg_x |= x_lsb; |
jjmedina | 0:a5367bd4e404 | 267 | write_reg(0x35,reg_x); |
jjmedina | 0:a5367bd4e404 | 268 | |
jjmedina | 0:a5367bd4e404 | 269 | //Disable offset X y Enable Y |
jjmedina | 0:a5367bd4e404 | 270 | fine_tu &= 0x7F; |
jjmedina | 0:a5367bd4e404 | 271 | fine_tu |= 0x40; |
jjmedina | 0:a5367bd4e404 | 272 | write_reg(0x0E,fine_tu); |
jjmedina | 0:a5367bd4e404 | 273 | |
jjmedina | 0:a5367bd4e404 | 274 | /////////////////////////////// |
jjmedina | 0:a5367bd4e404 | 275 | /// ESTABLECE EL OFFSET EN Y // |
jjmedina | 0:a5367bd4e404 | 276 | /////////////////////////////// |
jjmedina | 0:a5367bd4e404 | 277 | |
jjmedina | 0:a5367bd4e404 | 278 | write_reg(0x39,y_msb); |
jjmedina | 0:a5367bd4e404 | 279 | |
jjmedina | 0:a5367bd4e404 | 280 | reg_yz = read_reg(0x36); |
jjmedina | 0:a5367bd4e404 | 281 | reg_yz &= 0xF0; |
jjmedina | 0:a5367bd4e404 | 282 | reg_yz |= y_lsb; |
jjmedina | 0:a5367bd4e404 | 283 | write_reg(0x36,reg_yz); |
jjmedina | 0:a5367bd4e404 | 284 | |
jjmedina | 0:a5367bd4e404 | 285 | //Disable offset Y y Enable Z |
jjmedina | 0:a5367bd4e404 | 286 | fine_tu &= 0xBF; |
jjmedina | 0:a5367bd4e404 | 287 | fine_tu |= 0x20; |
jjmedina | 0:a5367bd4e404 | 288 | write_reg(0x0E,fine_tu); |
jjmedina | 0:a5367bd4e404 | 289 | |
jjmedina | 0:a5367bd4e404 | 290 | /////////////////////////////// |
jjmedina | 0:a5367bd4e404 | 291 | /// ESTABLECE EL OFFSET EN Z // |
jjmedina | 0:a5367bd4e404 | 292 | /////////////////////////////// |
jjmedina | 0:a5367bd4e404 | 293 | |
jjmedina | 0:a5367bd4e404 | 294 | write_reg(0x3A,z_msb); |
jjmedina | 0:a5367bd4e404 | 295 | |
jjmedina | 0:a5367bd4e404 | 296 | reg_yz &= 0x0F; |
jjmedina | 0:a5367bd4e404 | 297 | reg_yz |= z_lsb << 4; |
jjmedina | 0:a5367bd4e404 | 298 | write_reg(0x36,reg_yz); |
jjmedina | 0:a5367bd4e404 | 299 | |
jjmedina | 0:a5367bd4e404 | 300 | //Disable offset Z |
jjmedina | 0:a5367bd4e404 | 301 | fine_tu &= 0xDF; |
jjmedina | 0:a5367bd4e404 | 302 | write_reg(0x0E,fine_tu); |
jjmedina | 0:a5367bd4e404 | 303 | |
jjmedina | 0:a5367bd4e404 | 304 | // offset_finetuning to 01 |
jjmedina | 0:a5367bd4e404 | 305 | fine_tu = read_reg(0x22); |
jjmedina | 0:a5367bd4e404 | 306 | fine_tu &= 0xFC; |
jjmedina | 0:a5367bd4e404 | 307 | fine_tu |= 0x01; |
jjmedina | 0:a5367bd4e404 | 308 | write_reg(0x22,fine_tu); |
jjmedina | 0:a5367bd4e404 | 309 | |
jjmedina | 0:a5367bd4e404 | 310 | //////////////////////// |
jjmedina | 0:a5367bd4e404 | 311 | // Lock image writing // |
jjmedina | 0:a5367bd4e404 | 312 | //////////////////////// |
jjmedina | 0:a5367bd4e404 | 313 | |
jjmedina | 0:a5367bd4e404 | 314 | byte = read_reg(CTRL_REG0); // Lock image writing |
jjmedina | 0:a5367bd4e404 | 315 | byte &= 0xEF; // REset bit 4 |
jjmedina | 0:a5367bd4e404 | 316 | write_reg(CTRL_REG0,byte); // Have to set ee_w to |
jjmedina | 0:a5367bd4e404 | 317 | } |