This lib was build for reading the 3 IMU unit sensors of Altimu10v4 board (https://www.pololu.com/product/2470).
ALTIMU.cpp@4:d141a3e5531e, 2021-04-04 (annotated)
- Committer:
- renanbmx123
- Date:
- Sun Apr 04 14:37:39 2021 +0000
- Revision:
- 4:d141a3e5531e
- Parent:
- 3:3613e9a0edb1
Refatorando para o mbed-os 6
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
renanbmx123 | 0:08c9e51e6fd6 | 1 | /** |
renanbmx123 | 0:08c9e51e6fd6 | 2 | */ |
renanbmx123 | 0:08c9e51e6fd6 | 3 | #include "ALTIMU.h" |
renanbmx123 | 0:08c9e51e6fd6 | 4 | |
renanbmx123 | 0:08c9e51e6fd6 | 5 | // Public Methods ////////////////////////////////////////////////////////////// |
renanbmx123 | 0:08c9e51e6fd6 | 6 | |
renanbmx123 | 0:08c9e51e6fd6 | 7 | // Constructor |
renanbmx123 | 0:08c9e51e6fd6 | 8 | Altimu::Altimu(PinName sda, PinName scl): |
renanbmx123 | 0:08c9e51e6fd6 | 9 | _ALTIMU(sda, scl) |
renanbmx123 | 0:08c9e51e6fd6 | 10 | { |
renanbmx123 | 0:08c9e51e6fd6 | 11 | // Setting I2C operation frequency. |
renanbmx123 | 0:08c9e51e6fd6 | 12 | _ALTIMU.frequency(200000); |
renanbmx123 | 0:08c9e51e6fd6 | 13 | // L3GD20 configuration. |
renanbmx123 | 0:08c9e51e6fd6 | 14 | // DRDY_HL (DRDY active high);I2C_dis = (I2C & SPI enable); SW = (Normal Mode); Low_ODR = (Low speed disable). |
renanbmx123 | 0:08c9e51e6fd6 | 15 | write_reg(L3GD20_ADDR,0x39,0x00); |
renanbmx123 | 0:08c9e51e6fd6 | 16 | // DR = 01 (200 Hz ODR); BW = 10 (50 Hz bandwidth); PD = 1 (normal mode); Zen = Yen = Xen = 1 (all axes enabled). |
renanbmx123 | 0:08c9e51e6fd6 | 17 | write_reg(L3GD20_ADDR,0x20,0x5F); //Setting CTRL_REG1 |
renanbmx123 | 0:08c9e51e6fd6 | 18 | // End L3GD20 configuration. |
renanbmx123 | 0:08c9e51e6fd6 | 19 | // LSM303 configuration |
renanbmx123 | 0:08c9e51e6fd6 | 20 | // Acell configuration |
renanbmx123 | 0:08c9e51e6fd6 | 21 | // 50 Hz X/Y/Z axis enable. |
renanbmx123 | 0:08c9e51e6fd6 | 22 | write_reg(LSM303_ADDR, 0x20, 0x57); |
renanbmx123 | 0:08c9e51e6fd6 | 23 | // mag |
renanbmx123 | 0:08c9e51e6fd6 | 24 | //continuous mag |
renanbmx123 | 0:08c9e51e6fd6 | 25 | write_reg(LSM303_ADDR, 0x24, 0x78); |
renanbmx123 | 0:08c9e51e6fd6 | 26 | write_reg(LSM303_ADDR, 0x26, 0x00); |
renanbmx123 | 0:08c9e51e6fd6 | 27 | // End LSM303 configuration. |
renanbmx123 | 1:8cc36ccb8d58 | 28 | |
renanbmx123 | 0:08c9e51e6fd6 | 29 | // LPS25H configuration |
renanbmx123 | 1:8cc36ccb8d58 | 30 | //write_reg(LPS25H_ADDR, 0x10, 0x05); |
renanbmx123 | 1:8cc36ccb8d58 | 31 | //write_reg(LPS25H_ADDR, 0x2e, 0xdf); |
renanbmx123 | 1:8cc36ccb8d58 | 32 | //write_reg(LPS25H_ADDR, 0x21, 0x40); |
renanbmx123 | 0:08c9e51e6fd6 | 33 | write_reg(LPS25H_ADDR, 0x20, 0x90); |
renanbmx123 | 0:08c9e51e6fd6 | 34 | } |
renanbmx123 | 0:08c9e51e6fd6 | 35 | // L3GD30 read data function, |
renanbmx123 | 1:8cc36ccb8d58 | 36 | void Altimu::read_L3GD20(float *gx, float *gy, float *gz) { |
renanbmx123 | 0:08c9e51e6fd6 | 37 | char gyr[6]; |
renanbmx123 | 1:8cc36ccb8d58 | 38 | recv(L3GD20_ADDR, 0x28, gyr, 6); |
renanbmx123 | 0:08c9e51e6fd6 | 39 | //scale is 8.75 mdps/digit |
renanbmx123 | 0:08c9e51e6fd6 | 40 | *gx = float(short(gyr[1] << 8 | gyr[0]))*0.00875; |
renanbmx123 | 0:08c9e51e6fd6 | 41 | *gy = float(short(gyr[3] << 8 | gyr[2]))*0.00875; |
renanbmx123 | 0:08c9e51e6fd6 | 42 | *gz = float(short(gyr[5] << 8 | gyr[4]))*0.00875; |
renanbmx123 | 0:08c9e51e6fd6 | 43 | } |
renanbmx123 | 0:08c9e51e6fd6 | 44 | |
renanbmx123 | 0:08c9e51e6fd6 | 45 | // LSM303D read data function. |
renanbmx123 | 1:8cc36ccb8d58 | 46 | void Altimu::read_LSM303D(float *ax, float *ay, float *az, float *mx, float *my, float *mz) { |
renanbmx123 | 1:8cc36ccb8d58 | 47 | char acc[6], mag[6]; |
renanbmx123 | 1:8cc36ccb8d58 | 48 | recv(LSM303_ADDR, 0x28, acc, 6) && recv(LSM303_ADDR, 0x08, mag, 6); |
renanbmx123 | 0:08c9e51e6fd6 | 49 | *ax = float(short(acc[1] << 8 | acc[0]))*0.061; //32768/4=8192 |
renanbmx123 | 0:08c9e51e6fd6 | 50 | *ay = float(short(acc[3] << 8 | acc[2]))*0.061; |
renanbmx123 | 0:08c9e51e6fd6 | 51 | *az = float(short(acc[5] << 8 | acc[4]))*0.061; |
renanbmx123 | 0:08c9e51e6fd6 | 52 | //+-4gauss |
renanbmx123 | 0:08c9e51e6fd6 | 53 | *mx = float(short(mag[0] << 8 | mag[1]))*0.16; |
renanbmx123 | 0:08c9e51e6fd6 | 54 | *mz = float(short(mag[2] << 8 | mag[3]))*0.16; |
renanbmx123 | 0:08c9e51e6fd6 | 55 | *my = float(short(mag[4] << 8 | mag[5]))*0.16; |
renanbmx123 | 0:08c9e51e6fd6 | 56 | } |
renanbmx123 | 0:08c9e51e6fd6 | 57 | |
renanbmx123 | 0:08c9e51e6fd6 | 58 | // LPS25H read data function. |
renanbmx123 | 0:08c9e51e6fd6 | 59 | void Altimu::read_LPS25H(float *press, float *alt) |
renanbmx123 | 0:08c9e51e6fd6 | 60 | { |
renanbmx123 | 1:8cc36ccb8d58 | 61 | char dt[3]; // 3 bytes for reading i2c data(Press_X_L Press_L Press_H, Temp_L Temp_H). |
renanbmx123 | 1:8cc36ccb8d58 | 62 | float t; // Store internal temperature sensor, for compesate altitude calculation. |
renanbmx123 | 1:8cc36ccb8d58 | 63 | |
renanbmx123 | 1:8cc36ccb8d58 | 64 | // Reading 3 bytes from pressure sensor. |
renanbmx123 | 1:8cc36ccb8d58 | 65 | recv(LPS25H_ADDR, 0x28, dt, 3); |
renanbmx123 | 1:8cc36ccb8d58 | 66 | // Put togheter three bytes of pressure and make a calculation to present it on hPa values. |
renanbmx123 | 1:8cc36ccb8d58 | 67 | *press = (double)((dt[2] << 16) | (dt[1] << 8) | dt[0])/4096.0; |
renanbmx123 | 1:8cc36ccb8d58 | 68 | // Reading 2 bytes of internal temperature sensor. |
renanbmx123 | 1:8cc36ccb8d58 | 69 | recv(LPS25H_ADDR, 0x2B, dt, 2); |
renanbmx123 | 1:8cc36ccb8d58 | 70 | // Put the two temperature data togheter. |
renanbmx123 | 1:8cc36ccb8d58 | 71 | t = dt[1] << 8 | dt[0]; |
renanbmx123 | 1:8cc36ccb8d58 | 72 | // Calculate temperature in celcius. |
renanbmx123 | 1:8cc36ccb8d58 | 73 | t = (t/480 + 42.5)/10; |
renanbmx123 | 1:8cc36ccb8d58 | 74 | // Calculate altitude value from pressure and temperature. |
renanbmx123 | 1:8cc36ccb8d58 | 75 | *alt = (1-pow((*press/1013.25), 0.190262525))*((t+273.15)/0.0065); |
renanbmx123 | 0:08c9e51e6fd6 | 76 | } |
renanbmx123 | 0:08c9e51e6fd6 | 77 | |
renanbmx123 | 0:08c9e51e6fd6 | 78 | // I2C functions |
renanbmx123 | 1:8cc36ccb8d58 | 79 | |
renanbmx123 | 1:8cc36ccb8d58 | 80 | // Write a byte in a register address. |
renanbmx123 | 0:08c9e51e6fd6 | 81 | bool Altimu::write_reg(int addr_i2c,int addr_reg, char v) |
renanbmx123 | 0:08c9e51e6fd6 | 82 | { |
renanbmx123 | 4:d141a3e5531e | 83 | uint8_t data[2] = {(uint8_t)addr_reg, (uint8_t)v}; // |
renanbmx123 | 1:8cc36ccb8d58 | 84 | // return boolean value of write operation, if fails return 0, else 1. |
renanbmx123 | 4:d141a3e5531e | 85 | //write (int address, const char *data, int length, bool repeated=false) |
renanbmx123 | 4:d141a3e5531e | 86 | return Altimu::_ALTIMU.write(addr_i2c, (char *)data, 2) == 0; |
renanbmx123 | 0:08c9e51e6fd6 | 87 | } |
renanbmx123 | 1:8cc36ccb8d58 | 88 | // Read a byte from register |
renanbmx123 | 0:08c9e51e6fd6 | 89 | bool Altimu::read_reg(int addr_i2c,int addr_reg, char *v) |
renanbmx123 | 0:08c9e51e6fd6 | 90 | { |
renanbmx123 | 0:08c9e51e6fd6 | 91 | char data = addr_reg; |
renanbmx123 | 0:08c9e51e6fd6 | 92 | bool result = false; |
renanbmx123 | 1:8cc36ccb8d58 | 93 | |
renanbmx123 | 0:08c9e51e6fd6 | 94 | __disable_irq(); |
renanbmx123 | 0:08c9e51e6fd6 | 95 | if ((_ALTIMU.write(addr_i2c, &data, 1) == 0) && (_ALTIMU.read(addr_i2c, &data, 1) == 0)){ |
renanbmx123 | 0:08c9e51e6fd6 | 96 | *v = data; |
renanbmx123 | 0:08c9e51e6fd6 | 97 | result = true; |
renanbmx123 | 0:08c9e51e6fd6 | 98 | } |
renanbmx123 | 0:08c9e51e6fd6 | 99 | __enable_irq(); |
renanbmx123 | 0:08c9e51e6fd6 | 100 | return result; |
renanbmx123 | 0:08c9e51e6fd6 | 101 | } |
renanbmx123 | 2:3e874281c0f0 | 102 | // Read n bytes from mem address. |
renanbmx123 | 0:08c9e51e6fd6 | 103 | bool Altimu::recv(char sad, char sub, char *buf, int length) { |
renanbmx123 | 0:08c9e51e6fd6 | 104 | if (length > 1) sub |= 0x80; |
renanbmx123 | 0:08c9e51e6fd6 | 105 | |
renanbmx123 | 0:08c9e51e6fd6 | 106 | return _ALTIMU.write(sad, &sub, 1, true) == 0 && _ALTIMU.read(sad, buf, length) == 0; |
renanbmx123 | 0:08c9e51e6fd6 | 107 | } |