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
MPU9250/MPU9250.h@15:e07de535b86f, 2018-10-17 (annotated)
- Committer:
- fbob
- Date:
- Wed Oct 17 13:26:29 2018 +0000
- Revision:
- 15:e07de535b86f
- Parent:
- 12:2bbe233d25fb
Changed range and optical flow parameters names
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
fbob | 8:930aa9d0f5ae | 1 | #ifndef MPU9250_h |
fbob | 8:930aa9d0f5ae | 2 | #define MPU9250_h |
fbob | 8:930aa9d0f5ae | 3 | |
fbob | 8:930aa9d0f5ae | 4 | #include "mbed.h" |
fbob | 8:930aa9d0f5ae | 5 | |
fbob | 8:930aa9d0f5ae | 6 | // Physical constants |
fbob | 8:930aa9d0f5ae | 7 | #define GRAVITY 9.80665f |
fbob | 8:930aa9d0f5ae | 8 | #define PI 3.14159f |
fbob | 8:930aa9d0f5ae | 9 | |
fbob | 8:930aa9d0f5ae | 10 | // MPU9250 I2C bus address |
fbob | 8:930aa9d0f5ae | 11 | #define MPU9250_ADDRESS 0b1101001 << 1 // Shift 1 bit left because mbed utilizes 8-bit addresses and not 7-bit |
fbob | 8:930aa9d0f5ae | 12 | |
fbob | 8:930aa9d0f5ae | 13 | // Device identity |
fbob | 8:930aa9d0f5ae | 14 | #define WHO_AM_I 0x75 |
fbob | 8:930aa9d0f5ae | 15 | |
fbob | 8:930aa9d0f5ae | 16 | // Accelerometer configuration registers addresses |
fbob | 8:930aa9d0f5ae | 17 | #define ACCEL_CONFIG_1 0x1C |
fbob | 8:930aa9d0f5ae | 18 | #define ACCEL_CONFIG_2 0x1D |
fbob | 8:930aa9d0f5ae | 19 | // Accelerometer output register addresses |
fbob | 8:930aa9d0f5ae | 20 | #define ACCEL_XOUT_H 0x3B |
fbob | 8:930aa9d0f5ae | 21 | #define ACCEL_XOUT_L 0x3C |
fbob | 8:930aa9d0f5ae | 22 | #define ACCEL_YOUT_H 0x3D |
fbob | 8:930aa9d0f5ae | 23 | #define ACCEL_YOUT_L 0x3E |
fbob | 8:930aa9d0f5ae | 24 | #define ACCEL_ZOUT_H 0x3F |
fbob | 8:930aa9d0f5ae | 25 | #define ACCEL_ZOUT_L 0x40 |
fbob | 8:930aa9d0f5ae | 26 | |
fbob | 8:930aa9d0f5ae | 27 | // Gyroscope configuration registers addresses |
fbob | 8:930aa9d0f5ae | 28 | #define GYRO_CONFIG 0x1B |
fbob | 8:930aa9d0f5ae | 29 | // Accelerometer output register addresses |
fbob | 8:930aa9d0f5ae | 30 | #define GYRO_XOUT_H 0x43 |
fbob | 8:930aa9d0f5ae | 31 | #define GYRO_XOUT_L 0x44 |
fbob | 8:930aa9d0f5ae | 32 | #define GYRO_YOUT_H 0x45 |
fbob | 8:930aa9d0f5ae | 33 | #define GYRO_YOUT_L 0x46 |
fbob | 8:930aa9d0f5ae | 34 | #define GYRO_ZOUT_H 0x47 |
fbob | 8:930aa9d0f5ae | 35 | #define GYRO_ZOUT_L 0x48 |
fbob | 8:930aa9d0f5ae | 36 | |
fbob | 8:930aa9d0f5ae | 37 | // Auxiliary I2C configuration registers addresses |
fbob | 8:930aa9d0f5ae | 38 | #define INT_PIN_CFG 0x37 |
fbob | 8:930aa9d0f5ae | 39 | |
fbob | 8:930aa9d0f5ae | 40 | // Accelerometer full-scale ranges |
fbob | 8:930aa9d0f5ae | 41 | enum accel_scale |
fbob | 8:930aa9d0f5ae | 42 | { |
fbob | 8:930aa9d0f5ae | 43 | ACCEL_SCALE_2G = 0b00, |
fbob | 8:930aa9d0f5ae | 44 | ACCEL_SCALE_4G = 0b01, |
fbob | 8:930aa9d0f5ae | 45 | ACCEL_SCALE_8G = 0b10, |
fbob | 8:930aa9d0f5ae | 46 | ACCEL_SCALE_16G = 0b11 |
fbob | 8:930aa9d0f5ae | 47 | }; |
fbob | 8:930aa9d0f5ae | 48 | |
fbob | 8:930aa9d0f5ae | 49 | // Gyroscope full-scale ranges |
fbob | 8:930aa9d0f5ae | 50 | enum gyro_scale |
fbob | 8:930aa9d0f5ae | 51 | { |
fbob | 8:930aa9d0f5ae | 52 | GYRO_SCALE_250DPS = 0b00, |
fbob | 8:930aa9d0f5ae | 53 | GYRO_SCALE_500DPS = 0b01, |
fbob | 8:930aa9d0f5ae | 54 | GYRO_SCALE_1000DPS = 0b10, |
fbob | 8:930aa9d0f5ae | 55 | GYRO_SCALE_2000DPS = 0b11 |
fbob | 8:930aa9d0f5ae | 56 | }; |
fbob | 8:930aa9d0f5ae | 57 | |
fbob | 8:930aa9d0f5ae | 58 | /** MPU9250 (IMU sensor) class |
fbob | 8:930aa9d0f5ae | 59 | * |
fbob | 8:930aa9d0f5ae | 60 | * Example code (print accelerometer and gyroscope data on serial port every 0.2 seconds): |
fbob | 8:930aa9d0f5ae | 61 | * @code |
fbob | 8:930aa9d0f5ae | 62 | * #include "mbed.h" |
fbob | 8:930aa9d0f5ae | 63 | * #include "USBSerial.h" |
fbob | 8:930aa9d0f5ae | 64 | * #include "MPU9250.h" |
fbob | 8:930aa9d0f5ae | 65 | * |
fbob | 8:930aa9d0f5ae | 66 | * USBSerial pc; |
fbob | 8:930aa9d0f5ae | 67 | * MPU9250 imu(PC_9,PA_8); |
fbob | 8:930aa9d0f5ae | 68 | * |
fbob | 8:930aa9d0f5ae | 69 | * int main() |
fbob | 8:930aa9d0f5ae | 70 | * { |
fbob | 12:2bbe233d25fb | 71 | * imu.init(); |
fbob | 8:930aa9d0f5ae | 72 | * while(1) |
fbob | 8:930aa9d0f5ae | 73 | * { |
fbob | 8:930aa9d0f5ae | 74 | * imu.read(); |
fbob | 8:930aa9d0f5ae | 75 | * pc.printf("Accel [m/s^2]: %6.2f %6.2f %6.2f \n", imu.ax, imu.ay, imu.az); |
fbob | 8:930aa9d0f5ae | 76 | * pc.printf("Gyro [rad/s]: %6.2f %6.2f %6.2f \n\n", imu.gx, imu.gy, imu.gz); |
fbob | 8:930aa9d0f5ae | 77 | * wait(0.2); |
fbob | 8:930aa9d0f5ae | 78 | * } |
fbob | 8:930aa9d0f5ae | 79 | * } |
fbob | 8:930aa9d0f5ae | 80 | * @endcode |
fbob | 8:930aa9d0f5ae | 81 | * (Need to target to NUCLEO-F401RE board platform) |
fbob | 8:930aa9d0f5ae | 82 | */ |
fbob | 8:930aa9d0f5ae | 83 | class MPU9250 |
fbob | 8:930aa9d0f5ae | 84 | { |
fbob | 8:930aa9d0f5ae | 85 | public: |
fbob | 8:930aa9d0f5ae | 86 | /** Class constructor */ |
fbob | 8:930aa9d0f5ae | 87 | MPU9250(PinName sda, PinName scl); |
fbob | 8:930aa9d0f5ae | 88 | |
fbob | 8:930aa9d0f5ae | 89 | /** Initialize sensor */ |
fbob | 8:930aa9d0f5ae | 90 | bool init(); |
fbob | 8:930aa9d0f5ae | 91 | /** Read sensor data */ |
fbob | 8:930aa9d0f5ae | 92 | void read(); |
fbob | 8:930aa9d0f5ae | 93 | |
fbob | 8:930aa9d0f5ae | 94 | uint8_t bob(); |
fbob | 8:930aa9d0f5ae | 95 | |
fbob | 8:930aa9d0f5ae | 96 | /** Accelerometer data in x-axis [m/s^2]*/ |
fbob | 8:930aa9d0f5ae | 97 | float ax; |
fbob | 8:930aa9d0f5ae | 98 | /** Accelerometer data in y-axis [m/s^2]*/ |
fbob | 8:930aa9d0f5ae | 99 | float ay; |
fbob | 8:930aa9d0f5ae | 100 | /** Accelerometer data in z-axis [m/s^2]*/ |
fbob | 8:930aa9d0f5ae | 101 | float az; |
fbob | 8:930aa9d0f5ae | 102 | /** Gyroscope data in x-axis [rad/s]*/ |
fbob | 8:930aa9d0f5ae | 103 | float gx; |
fbob | 8:930aa9d0f5ae | 104 | /** Gyroscope data in y-axis [rad/s]*/ |
fbob | 8:930aa9d0f5ae | 105 | float gy; |
fbob | 8:930aa9d0f5ae | 106 | /** Gyroscope data in z-axis [rad/s]*/ |
fbob | 8:930aa9d0f5ae | 107 | float gz; |
fbob | 8:930aa9d0f5ae | 108 | private: |
fbob | 8:930aa9d0f5ae | 109 | /** I2C bus */ |
fbob | 8:930aa9d0f5ae | 110 | I2C i2c; |
fbob | 8:930aa9d0f5ae | 111 | |
fbob | 8:930aa9d0f5ae | 112 | /** Setup I2C bus */ |
fbob | 8:930aa9d0f5ae | 113 | void setup_i2c(); |
fbob | 8:930aa9d0f5ae | 114 | /** Test I2C bus */ |
fbob | 8:930aa9d0f5ae | 115 | bool test_i2c(); |
fbob | 8:930aa9d0f5ae | 116 | |
fbob | 8:930aa9d0f5ae | 117 | /** Setup auxiliary I2C bus pins */ |
fbob | 8:930aa9d0f5ae | 118 | void setup_aux_i2c(); |
fbob | 8:930aa9d0f5ae | 119 | |
fbob | 8:930aa9d0f5ae | 120 | /** Setup accelerometer configurations (full-scale range) */ |
fbob | 8:930aa9d0f5ae | 121 | void setup_accel(accel_scale a_scale = ACCEL_SCALE_16G); |
fbob | 8:930aa9d0f5ae | 122 | /** Setup gyroscope configurations (full-scale range) */ |
fbob | 8:930aa9d0f5ae | 123 | void setup_gyro(gyro_scale g_scale = GYRO_SCALE_500DPS); |
fbob | 8:930aa9d0f5ae | 124 | /** Read accelerometer data */ |
fbob | 8:930aa9d0f5ae | 125 | void read_accel(); |
fbob | 8:930aa9d0f5ae | 126 | /** Read gyroscope data */ |
fbob | 8:930aa9d0f5ae | 127 | void read_gyro(); |
fbob | 8:930aa9d0f5ae | 128 | |
fbob | 8:930aa9d0f5ae | 129 | /** Write data into register on MPU9250 I2C bus address */ |
fbob | 8:930aa9d0f5ae | 130 | void write_reg(uint8_t reg, uint8_t data); |
fbob | 8:930aa9d0f5ae | 131 | /** Read data from register on MPU9250 I2C bus address */ |
fbob | 8:930aa9d0f5ae | 132 | char read_reg(uint8_t reg); |
fbob | 8:930aa9d0f5ae | 133 | |
fbob | 8:930aa9d0f5ae | 134 | /** Accelerometer resolution [m/s^2 / bit]*/ |
fbob | 8:930aa9d0f5ae | 135 | float a_res; |
fbob | 8:930aa9d0f5ae | 136 | /** Gyroscope resolution [rad/s / bit]*/ |
fbob | 8:930aa9d0f5ae | 137 | float g_res; |
fbob | 8:930aa9d0f5ae | 138 | |
fbob | 8:930aa9d0f5ae | 139 | }; |
fbob | 8:930aa9d0f5ae | 140 | |
fbob | 8:930aa9d0f5ae | 141 | #endif |