Fabio Bobrow / CrazyflieSensors

Library containing Crazyflie 2.0 sensors drivers: - LPS25H (barometer) - MPU9250 (IMU) - PMW3901 (optical flow) - VL53L0X (range)

Dependents:   Drones-Controlador controladoatitude_cteste Drone_Controlador_Atitude optical_test

LPS25H/LPS25H.cpp@8:930aa9d0f5ae, 2018-05-14 (annotated)

Committer:
fbob
Date:
Mon May 14 20:18:20 2018 +0000
Revision:
8:930aa9d0f5ae
Update init functions

Who changed what in which revision?

UserRevisionLine numberNew contents of line
fbob 8:930aa9d0f5ae 1 #include "LPS25H.h"
fbob 8:930aa9d0f5ae 2
fbob 8:930aa9d0f5ae 3 /** Class constructor */
fbob 8:930aa9d0f5ae 4 LPS25H::LPS25H(PinName sda, PinName scl) : i2c(sda, scl)
fbob 8:930aa9d0f5ae 5 {
fbob 8:930aa9d0f5ae 6 }
fbob 8:930aa9d0f5ae 7
fbob 8:930aa9d0f5ae 8 /** Init barometer */
fbob 8:930aa9d0f5ae 9 bool LPS25H::init()
fbob 8:930aa9d0f5ae 10 {
fbob 8:930aa9d0f5ae 11 // Setup I2C bus
fbob 8:930aa9d0f5ae 12 setup_i2c();
fbob 8:930aa9d0f5ae 13
fbob 8:930aa9d0f5ae 14 // Test I2C bus
fbob 8:930aa9d0f5ae 15 if (test_i2c()) {
fbob 8:930aa9d0f5ae 16 // Setup accelerometer and gyroscope configurations
fbob 8:930aa9d0f5ae 17 setup_baro();
fbob 8:930aa9d0f5ae 18 return true;
fbob 8:930aa9d0f5ae 19 } else {
fbob 8:930aa9d0f5ae 20 return false;
fbob 8:930aa9d0f5ae 21 }
fbob 8:930aa9d0f5ae 22 }
fbob 8:930aa9d0f5ae 23 /** Read barometer */
fbob 8:930aa9d0f5ae 24 void LPS25H::read()
fbob 8:930aa9d0f5ae 25 {
fbob 8:930aa9d0f5ae 26 read_baro();
fbob 8:930aa9d0f5ae 27 }
fbob 8:930aa9d0f5ae 28
fbob 8:930aa9d0f5ae 29 /** Setup barometer */
fbob 8:930aa9d0f5ae 30 void LPS25H::setup_baro()
fbob 8:930aa9d0f5ae 31 {
fbob 8:930aa9d0f5ae 32 //
fbob 8:930aa9d0f5ae 33 write_reg(CTRL_REG1, 0b10110000);
fbob 8:930aa9d0f5ae 34 }
fbob 8:930aa9d0f5ae 35
fbob 8:930aa9d0f5ae 36
fbob 8:930aa9d0f5ae 37 /** Setup I2C bus */
fbob 8:930aa9d0f5ae 38 void LPS25H::setup_i2c()
fbob 8:930aa9d0f5ae 39 {
fbob 8:930aa9d0f5ae 40 // Setup I2C bus frequency to 100kHz
fbob 8:930aa9d0f5ae 41 i2c.frequency(400000);
fbob 8:930aa9d0f5ae 42 }
fbob 8:930aa9d0f5ae 43
fbob 8:930aa9d0f5ae 44 /** Test I2C bus */
fbob 8:930aa9d0f5ae 45 bool LPS25H::test_i2c()
fbob 8:930aa9d0f5ae 46 {
fbob 8:930aa9d0f5ae 47 // Read device identity
fbob 8:930aa9d0f5ae 48 uint8_t device_id = read_reg(WHO_AM_I);
fbob 8:930aa9d0f5ae 49
fbob 8:930aa9d0f5ae 50 // Check if device identity is 0xBD
fbob 8:930aa9d0f5ae 51 if (device_id == 0xBD) {
fbob 8:930aa9d0f5ae 52 return true;
fbob 8:930aa9d0f5ae 53 } else {
fbob 8:930aa9d0f5ae 54 return false;
fbob 8:930aa9d0f5ae 55 }
fbob 8:930aa9d0f5ae 56 }
fbob 8:930aa9d0f5ae 57
fbob 8:930aa9d0f5ae 58 /** Read barometer */
fbob 8:930aa9d0f5ae 59 void LPS25H::read_baro()
fbob 8:930aa9d0f5ae 60 {
fbob 8:930aa9d0f5ae 61 // Read raw data (three 8 bit data)
fbob 8:930aa9d0f5ae 62 uint8_t p_raw_xl = read_reg(PRESS_OUT_XL);
fbob 8:930aa9d0f5ae 63 uint8_t p_raw_l = read_reg(PRESS_OUT_L);
fbob 8:930aa9d0f5ae 64 uint8_t p_raw_h = read_reg(PRESS_OUT_H);
fbob 8:930aa9d0f5ae 65
fbob 8:930aa9d0f5ae 66 // Reassemble the data (one 24 bit data)
fbob 8:930aa9d0f5ae 67 int32_t p_raw = (p_raw_h << 16 ) | (p_raw_l << 8 ) | p_raw_xl;
fbob 8:930aa9d0f5ae 68
fbob 8:930aa9d0f5ae 69 // Convert raw data to SI units [hPa]
fbob 8:930aa9d0f5ae 70 p = p_raw / 4096.0;
fbob 8:930aa9d0f5ae 71
fbob 8:930aa9d0f5ae 72 // Calculate altitude [m] from pressure [hPa]
fbob 8:930aa9d0f5ae 73 z = 44330.8f*(1.0f-pow((p/1025.0f),0.190263f));
fbob 8:930aa9d0f5ae 74 }
fbob 8:930aa9d0f5ae 75
fbob 8:930aa9d0f5ae 76 /** Write data into register on MPU9250 I2C bus address */
fbob 8:930aa9d0f5ae 77 void LPS25H::write_reg(uint8_t reg, uint8_t data)
fbob 8:930aa9d0f5ae 78 {
fbob 8:930aa9d0f5ae 79 // Register address and data that will be writed
fbob 8:930aa9d0f5ae 80 char i2c_reg_data[2] = {reg, data};
fbob 8:930aa9d0f5ae 81
fbob 8:930aa9d0f5ae 82 // Point to register address and write data
fbob 8:930aa9d0f5ae 83 i2c.write(LPS25H_ADDRESS, i2c_reg_data, 2);
fbob 8:930aa9d0f5ae 84 }
fbob 8:930aa9d0f5ae 85
fbob 8:930aa9d0f5ae 86 /** Read data from register on MPU9250 I2C bus address */
fbob 8:930aa9d0f5ae 87 char LPS25H::read_reg(uint8_t reg)
fbob 8:930aa9d0f5ae 88 {
fbob 8:930aa9d0f5ae 89 // Register address
fbob 8:930aa9d0f5ae 90 char i2c_reg[1] = {reg};
fbob 8:930aa9d0f5ae 91 // Data that will be readed
fbob 8:930aa9d0f5ae 92 char i2c_data[1];
fbob 8:930aa9d0f5ae 93
fbob 8:930aa9d0f5ae 94 // Point to register address
fbob 8:930aa9d0f5ae 95 i2c.write(LPS25H_ADDRESS, i2c_reg, 1);
fbob 8:930aa9d0f5ae 96 // Read data
fbob 8:930aa9d0f5ae 97 i2c.read(LPS25H_ADDRESS, i2c_data, 1);
fbob 8:930aa9d0f5ae 98
fbob 8:930aa9d0f5ae 99 // Return readed data
fbob 8:930aa9d0f5ae 100 return i2c_data[0];
fbob 8:930aa9d0f5ae 101 }