LSE_PUCRS / ALTIMU

This lib was build for reading the 3 IMU unit sensors of Altimu10v4 board (https://www.pololu.com/product/2470).

ALTIMU.cpp@2:3e874281c0f0, 2018-10-16 (annotated)

Committer:
renanbmx123
Date:
Tue Oct 16 00:16:05 2018 +0000
Revision:
2:3e874281c0f0
Parent:
1:8cc36ccb8d58
Child:
3:3613e9a0edb1
Remove some unnecessary data !!!

Who changed what in which revision?

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