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
PMW3901/PMW3901.cpp@8:930aa9d0f5ae, 2018-05-14 (annotated)
- Committer:
- fbob
- Date:
- Mon May 14 20:18:20 2018 +0000
- Revision:
- 8:930aa9d0f5ae
- Child:
- 14:e4d638ac83af
Update init functions
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
fbob | 8:930aa9d0f5ae | 1 | #include "PMW3901.h" |
fbob | 8:930aa9d0f5ae | 2 | |
fbob | 8:930aa9d0f5ae | 3 | /** Class constructor */ |
fbob | 8:930aa9d0f5ae | 4 | PMW3901::PMW3901(PinName mosi, PinName miso, PinName sclk, PinName csel) : spi(mosi, miso, sclk), cs(csel) |
fbob | 8:930aa9d0f5ae | 5 | { |
fbob | 8:930aa9d0f5ae | 6 | } |
fbob | 8:930aa9d0f5ae | 7 | |
fbob | 8:930aa9d0f5ae | 8 | /** Initialize optical flow */ |
fbob | 8:930aa9d0f5ae | 9 | bool PMW3901::init() |
fbob | 8:930aa9d0f5ae | 10 | { |
fbob | 8:930aa9d0f5ae | 11 | setup_spi(); |
fbob | 8:930aa9d0f5ae | 12 | if (test_spi()) |
fbob | 8:930aa9d0f5ae | 13 | { |
fbob | 8:930aa9d0f5ae | 14 | setup_flow(); |
fbob | 8:930aa9d0f5ae | 15 | return true; |
fbob | 8:930aa9d0f5ae | 16 | } |
fbob | 8:930aa9d0f5ae | 17 | else |
fbob | 8:930aa9d0f5ae | 18 | { |
fbob | 8:930aa9d0f5ae | 19 | return false; |
fbob | 8:930aa9d0f5ae | 20 | } |
fbob | 8:930aa9d0f5ae | 21 | } |
fbob | 8:930aa9d0f5ae | 22 | |
fbob | 8:930aa9d0f5ae | 23 | void PMW3901::read() |
fbob | 8:930aa9d0f5ae | 24 | { |
fbob | 8:930aa9d0f5ae | 25 | read_flow(); |
fbob | 8:930aa9d0f5ae | 26 | } |
fbob | 8:930aa9d0f5ae | 27 | |
fbob | 8:930aa9d0f5ae | 28 | void PMW3901::setup_spi() |
fbob | 8:930aa9d0f5ae | 29 | { |
fbob | 8:930aa9d0f5ae | 30 | // Deselect the device by seting chip select high |
fbob | 8:930aa9d0f5ae | 31 | cs = 1; |
fbob | 8:930aa9d0f5ae | 32 | |
fbob | 8:930aa9d0f5ae | 33 | // Setup the spi format for 8 bit data and mode 3 (high steady state clock and second edge capture) |
fbob | 8:930aa9d0f5ae | 34 | spi.format(8,3); |
fbob | 8:930aa9d0f5ae | 35 | // Setup the spi frequency to 2MHz |
fbob | 8:930aa9d0f5ae | 36 | spi.frequency(2000000); |
fbob | 8:930aa9d0f5ae | 37 | } |
fbob | 8:930aa9d0f5ae | 38 | |
fbob | 8:930aa9d0f5ae | 39 | bool PMW3901::test_spi() |
fbob | 8:930aa9d0f5ae | 40 | { |
fbob | 8:930aa9d0f5ae | 41 | // |
fbob | 8:930aa9d0f5ae | 42 | char id; |
fbob | 8:930aa9d0f5ae | 43 | |
fbob | 8:930aa9d0f5ae | 44 | // Select the device by seting chip select low |
fbob | 8:930aa9d0f5ae | 45 | cs = 0; |
fbob | 8:930aa9d0f5ae | 46 | // Write the register to read sensor ID (set MSB to 0 for read) |
fbob | 8:930aa9d0f5ae | 47 | spi.write(PRODUCT_ID & 0b01111111); |
fbob | 8:930aa9d0f5ae | 48 | // Write a dummy byte 0x00 to receive the chip ID |
fbob | 8:930aa9d0f5ae | 49 | id = spi.write(0x00); |
fbob | 8:930aa9d0f5ae | 50 | // Deselect the device by seting chip select high |
fbob | 8:930aa9d0f5ae | 51 | cs = 1; |
fbob | 8:930aa9d0f5ae | 52 | |
fbob | 8:930aa9d0f5ae | 53 | // |
fbob | 8:930aa9d0f5ae | 54 | if (id == 0x49) |
fbob | 8:930aa9d0f5ae | 55 | { |
fbob | 8:930aa9d0f5ae | 56 | return true; |
fbob | 8:930aa9d0f5ae | 57 | } |
fbob | 8:930aa9d0f5ae | 58 | else |
fbob | 8:930aa9d0f5ae | 59 | { |
fbob | 8:930aa9d0f5ae | 60 | return false; |
fbob | 8:930aa9d0f5ae | 61 | } |
fbob | 8:930aa9d0f5ae | 62 | } |
fbob | 8:930aa9d0f5ae | 63 | |
fbob | 8:930aa9d0f5ae | 64 | void PMW3901::setup_flow() |
fbob | 8:930aa9d0f5ae | 65 | { |
fbob | 8:930aa9d0f5ae | 66 | // Select the device by seting chip select low |
fbob | 8:930aa9d0f5ae | 67 | cs = 0; |
fbob | 8:930aa9d0f5ae | 68 | |
fbob | 8:930aa9d0f5ae | 69 | // |
fbob | 8:930aa9d0f5ae | 70 | spi.write(0x3A | 0b10000000); spi.write(0x5A); |
fbob | 8:930aa9d0f5ae | 71 | |
fbob | 8:930aa9d0f5ae | 72 | // |
fbob | 8:930aa9d0f5ae | 73 | spi.write(0x02 & 0b01111111); spi.write(0x00); |
fbob | 8:930aa9d0f5ae | 74 | // |
fbob | 8:930aa9d0f5ae | 75 | spi.write(0x03 & 0b01111111); spi.write(0x00); |
fbob | 8:930aa9d0f5ae | 76 | spi.write(0x04 & 0b01111111); spi.write(0x00); |
fbob | 8:930aa9d0f5ae | 77 | spi.write(0x05 & 0b01111111); spi.write(0x00); |
fbob | 8:930aa9d0f5ae | 78 | spi.write(0x06 & 0b01111111); spi.write(0x00); |
fbob | 8:930aa9d0f5ae | 79 | |
fbob | 8:930aa9d0f5ae | 80 | // |
fbob | 8:930aa9d0f5ae | 81 | spi.write(0x7F | 0b10000000); spi.write(0x00); |
fbob | 8:930aa9d0f5ae | 82 | spi.write(0x61 | 0b10000000); spi.write(0xAD); |
fbob | 8:930aa9d0f5ae | 83 | spi.write(0x7F | 0b10000000); spi.write(0x03); |
fbob | 8:930aa9d0f5ae | 84 | spi.write(0x40 | 0b10000000); spi.write(0x00); |
fbob | 8:930aa9d0f5ae | 85 | spi.write(0x7F | 0b10000000); spi.write(0x05); |
fbob | 8:930aa9d0f5ae | 86 | spi.write(0x41 | 0b10000000); spi.write(0xB3); |
fbob | 8:930aa9d0f5ae | 87 | spi.write(0x43 | 0b10000000); spi.write(0xF1); |
fbob | 8:930aa9d0f5ae | 88 | spi.write(0x45 | 0b10000000); spi.write(0x14); |
fbob | 8:930aa9d0f5ae | 89 | spi.write(0x5B | 0b10000000); spi.write(0x32); |
fbob | 8:930aa9d0f5ae | 90 | spi.write(0x5F | 0b10000000); spi.write(0x34); |
fbob | 8:930aa9d0f5ae | 91 | spi.write(0x7B | 0b10000000); spi.write(0x08); |
fbob | 8:930aa9d0f5ae | 92 | spi.write(0x7F | 0b10000000); spi.write(0x06); |
fbob | 8:930aa9d0f5ae | 93 | spi.write(0x44 | 0b10000000); spi.write(0x1B); |
fbob | 8:930aa9d0f5ae | 94 | spi.write(0x40 | 0b10000000); spi.write(0xBF); |
fbob | 8:930aa9d0f5ae | 95 | spi.write(0x4E | 0b10000000); spi.write(0x3F); |
fbob | 8:930aa9d0f5ae | 96 | spi.write(0x7F | 0b10000000); spi.write(0x08); |
fbob | 8:930aa9d0f5ae | 97 | spi.write(0x65 | 0b10000000); spi.write(0x20); |
fbob | 8:930aa9d0f5ae | 98 | spi.write(0x6A | 0b10000000); spi.write(0x18); |
fbob | 8:930aa9d0f5ae | 99 | spi.write(0x7F | 0b10000000); spi.write(0x09); |
fbob | 8:930aa9d0f5ae | 100 | spi.write(0x4F | 0b10000000); spi.write(0xAF); |
fbob | 8:930aa9d0f5ae | 101 | spi.write(0x5F | 0b10000000); spi.write(0x40); |
fbob | 8:930aa9d0f5ae | 102 | spi.write(0x48 | 0b10000000); spi.write(0x80); |
fbob | 8:930aa9d0f5ae | 103 | spi.write(0x49 | 0b10000000); spi.write(0x80); |
fbob | 8:930aa9d0f5ae | 104 | spi.write(0x57 | 0b10000000); spi.write(0x77); |
fbob | 8:930aa9d0f5ae | 105 | spi.write(0x60 | 0b10000000); spi.write(0x78); |
fbob | 8:930aa9d0f5ae | 106 | spi.write(0x61 | 0b10000000); spi.write(0x78); |
fbob | 8:930aa9d0f5ae | 107 | spi.write(0x62 | 0b10000000); spi.write(0x08); |
fbob | 8:930aa9d0f5ae | 108 | spi.write(0x63 | 0b10000000); spi.write(0x50); |
fbob | 8:930aa9d0f5ae | 109 | spi.write(0x7F | 0b10000000); spi.write(0x0A); |
fbob | 8:930aa9d0f5ae | 110 | spi.write(0x45 | 0b10000000); spi.write(0x60); |
fbob | 8:930aa9d0f5ae | 111 | spi.write(0x7F | 0b10000000); spi.write(0x00); |
fbob | 8:930aa9d0f5ae | 112 | spi.write(0x4D | 0b10000000); spi.write(0x11); |
fbob | 8:930aa9d0f5ae | 113 | spi.write(0x55 | 0b10000000); spi.write(0x80); |
fbob | 8:930aa9d0f5ae | 114 | spi.write(0x74 | 0b10000000); spi.write(0x1F); |
fbob | 8:930aa9d0f5ae | 115 | spi.write(0x75 | 0b10000000); spi.write(0x1F); |
fbob | 8:930aa9d0f5ae | 116 | spi.write(0x4A | 0b10000000); spi.write(0x78); |
fbob | 8:930aa9d0f5ae | 117 | spi.write(0x4B | 0b10000000); spi.write(0x78); |
fbob | 8:930aa9d0f5ae | 118 | spi.write(0x44 | 0b10000000); spi.write(0x08); |
fbob | 8:930aa9d0f5ae | 119 | spi.write(0x45 | 0b10000000); spi.write(0x50); |
fbob | 8:930aa9d0f5ae | 120 | spi.write(0x64 | 0b10000000); spi.write(0xFF); |
fbob | 8:930aa9d0f5ae | 121 | spi.write(0x65 | 0b10000000); spi.write(0x1F); |
fbob | 8:930aa9d0f5ae | 122 | spi.write(0x7F | 0b10000000); spi.write(0x14); |
fbob | 8:930aa9d0f5ae | 123 | spi.write(0x65 | 0b10000000); spi.write(0x60); |
fbob | 8:930aa9d0f5ae | 124 | spi.write(0x66 | 0b10000000); spi.write(0x08); |
fbob | 8:930aa9d0f5ae | 125 | spi.write(0x63 | 0b10000000); spi.write(0x78); |
fbob | 8:930aa9d0f5ae | 126 | spi.write(0x7F | 0b10000000); spi.write(0x15); |
fbob | 8:930aa9d0f5ae | 127 | spi.write(0x48 | 0b10000000); spi.write(0x58); |
fbob | 8:930aa9d0f5ae | 128 | spi.write(0x7F | 0b10000000); spi.write(0x07); |
fbob | 8:930aa9d0f5ae | 129 | spi.write(0x41 | 0b10000000); spi.write(0x0D); |
fbob | 8:930aa9d0f5ae | 130 | spi.write(0x43 | 0b10000000); spi.write(0x14); |
fbob | 8:930aa9d0f5ae | 131 | spi.write(0x4B | 0b10000000); spi.write(0x0E); |
fbob | 8:930aa9d0f5ae | 132 | spi.write(0x45 | 0b10000000); spi.write(0x0F); |
fbob | 8:930aa9d0f5ae | 133 | spi.write(0x44 | 0b10000000); spi.write(0x42); |
fbob | 8:930aa9d0f5ae | 134 | spi.write(0x4C | 0b10000000); spi.write(0x80); |
fbob | 8:930aa9d0f5ae | 135 | spi.write(0x7F | 0b10000000); spi.write(0x10); |
fbob | 8:930aa9d0f5ae | 136 | spi.write(0x5B | 0b10000000); spi.write(0x02); |
fbob | 8:930aa9d0f5ae | 137 | spi.write(0x7F | 0b10000000); spi.write(0x07); |
fbob | 8:930aa9d0f5ae | 138 | spi.write(0x40 | 0b10000000); spi.write(0x41); |
fbob | 8:930aa9d0f5ae | 139 | spi.write(0x70 | 0b10000000); spi.write(0x00); |
fbob | 8:930aa9d0f5ae | 140 | |
fbob | 8:930aa9d0f5ae | 141 | // |
fbob | 8:930aa9d0f5ae | 142 | wait(0.1); |
fbob | 8:930aa9d0f5ae | 143 | spi.write(0x32 | 0b10000000); spi.write(0x44); |
fbob | 8:930aa9d0f5ae | 144 | spi.write(0x7F | 0b10000000); spi.write(0x07); |
fbob | 8:930aa9d0f5ae | 145 | spi.write(0x40 | 0b10000000); spi.write(0x40); |
fbob | 8:930aa9d0f5ae | 146 | spi.write(0x7F | 0b10000000); spi.write(0x06); |
fbob | 8:930aa9d0f5ae | 147 | spi.write(0x62 | 0b10000000); spi.write(0xf0); |
fbob | 8:930aa9d0f5ae | 148 | spi.write(0x63 | 0b10000000); spi.write(0x00); |
fbob | 8:930aa9d0f5ae | 149 | spi.write(0x7F | 0b10000000); spi.write(0x0D); |
fbob | 8:930aa9d0f5ae | 150 | spi.write(0x48 | 0b10000000); spi.write(0xC0); |
fbob | 8:930aa9d0f5ae | 151 | spi.write(0x6F | 0b10000000); spi.write(0xd5); |
fbob | 8:930aa9d0f5ae | 152 | spi.write(0x7F | 0b10000000); spi.write(0x00); |
fbob | 8:930aa9d0f5ae | 153 | spi.write(0x5B | 0b10000000); spi.write(0xa0); |
fbob | 8:930aa9d0f5ae | 154 | spi.write(0x4E | 0b10000000); spi.write(0xA8); |
fbob | 8:930aa9d0f5ae | 155 | spi.write(0x5A | 0b10000000); spi.write(0x50); |
fbob | 8:930aa9d0f5ae | 156 | spi.write(0x40 | 0b10000000); spi.write(0x80); |
fbob | 8:930aa9d0f5ae | 157 | |
fbob | 8:930aa9d0f5ae | 158 | // Deselect the device by seting chip select high |
fbob | 8:930aa9d0f5ae | 159 | cs = 1; |
fbob | 8:930aa9d0f5ae | 160 | } |
fbob | 8:930aa9d0f5ae | 161 | |
fbob | 8:930aa9d0f5ae | 162 | void PMW3901::read_flow() |
fbob | 8:930aa9d0f5ae | 163 | { |
fbob | 8:930aa9d0f5ae | 164 | // Data that we're going to read |
fbob | 8:930aa9d0f5ae | 165 | char data[4]; |
fbob | 8:930aa9d0f5ae | 166 | |
fbob | 8:930aa9d0f5ae | 167 | // Select the device by seting chip select low |
fbob | 8:930aa9d0f5ae | 168 | cs = 0; |
fbob | 8:930aa9d0f5ae | 169 | |
fbob | 8:930aa9d0f5ae | 170 | // |
fbob | 8:930aa9d0f5ae | 171 | spi.write(MOTION & 0b01111111); spi.write(0x00); |
fbob | 8:930aa9d0f5ae | 172 | // Write the register to read sensor ID (set MSB to 0 for read) |
fbob | 8:930aa9d0f5ae | 173 | spi.write(DELTA_X_L & 0b01111111); data[0] = spi.write(0x00); |
fbob | 8:930aa9d0f5ae | 174 | spi.write(DELTA_X_H & 0b01111111); data[1] = spi.write(0x00); |
fbob | 8:930aa9d0f5ae | 175 | spi.write(DELTA_Y_L & 0b01111111); data[2] = spi.write(0x00); |
fbob | 8:930aa9d0f5ae | 176 | spi.write(DELTA_Y_H & 0b01111111); data[3] = spi.write(0x00); |
fbob | 8:930aa9d0f5ae | 177 | |
fbob | 8:930aa9d0f5ae | 178 | // Reassemble the data (two 8 bit data into one 16 bit data) |
fbob | 8:930aa9d0f5ae | 179 | int16_t x_raw = (data[1] << 8 ) | data[0]; |
fbob | 8:930aa9d0f5ae | 180 | int16_t y_raw = (data[3] << 8 ) | data[2]; |
fbob | 8:930aa9d0f5ae | 181 | // Convert to SI units [rad/s] |
fbob | 8:930aa9d0f5ae | 182 | x = x_raw * 1.0f; |
fbob | 8:930aa9d0f5ae | 183 | y = y_raw * 1.0f; |
fbob | 8:930aa9d0f5ae | 184 | |
fbob | 8:930aa9d0f5ae | 185 | // Deselect the device by seting chip select high |
fbob | 8:930aa9d0f5ae | 186 | cs = 1; |
fbob | 8:930aa9d0f5ae | 187 | } |
fbob | 8:930aa9d0f5ae | 188 |