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
Diff: PMW3901/PMW3901.cpp
- Revision:
- 8:930aa9d0f5ae
- Child:
- 14:e4d638ac83af
diff -r 56608cce81ee -r 930aa9d0f5ae PMW3901/PMW3901.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/PMW3901/PMW3901.cpp Mon May 14 20:18:20 2018 +0000 @@ -0,0 +1,188 @@ +#include "PMW3901.h" + +/** Class constructor */ +PMW3901::PMW3901(PinName mosi, PinName miso, PinName sclk, PinName csel) : spi(mosi, miso, sclk), cs(csel) +{ +} + +/** Initialize optical flow */ +bool PMW3901::init() +{ + setup_spi(); + if (test_spi()) + { + setup_flow(); + return true; + } + else + { + return false; + } +} + +void PMW3901::read() +{ + read_flow(); +} + +void PMW3901::setup_spi() +{ + // Deselect the device by seting chip select high + cs = 1; + + // Setup the spi format for 8 bit data and mode 3 (high steady state clock and second edge capture) + spi.format(8,3); + // Setup the spi frequency to 2MHz + spi.frequency(2000000); +} + +bool PMW3901::test_spi() +{ + // + char id; + + // Select the device by seting chip select low + cs = 0; + // Write the register to read sensor ID (set MSB to 0 for read) + spi.write(PRODUCT_ID & 0b01111111); + // Write a dummy byte 0x00 to receive the chip ID + id = spi.write(0x00); + // Deselect the device by seting chip select high + cs = 1; + + // + if (id == 0x49) + { + return true; + } + else + { + return false; + } +} + +void PMW3901::setup_flow() +{ + // Select the device by seting chip select low + cs = 0; + + // + spi.write(0x3A | 0b10000000); spi.write(0x5A); + + // + spi.write(0x02 & 0b01111111); spi.write(0x00); + // + spi.write(0x03 & 0b01111111); spi.write(0x00); + spi.write(0x04 & 0b01111111); spi.write(0x00); + spi.write(0x05 & 0b01111111); spi.write(0x00); + spi.write(0x06 & 0b01111111); spi.write(0x00); + + // + spi.write(0x7F | 0b10000000); spi.write(0x00); + spi.write(0x61 | 0b10000000); spi.write(0xAD); + spi.write(0x7F | 0b10000000); spi.write(0x03); + spi.write(0x40 | 0b10000000); spi.write(0x00); + spi.write(0x7F | 0b10000000); spi.write(0x05); + spi.write(0x41 | 0b10000000); spi.write(0xB3); + spi.write(0x43 | 0b10000000); spi.write(0xF1); + spi.write(0x45 | 0b10000000); spi.write(0x14); + spi.write(0x5B | 0b10000000); spi.write(0x32); + spi.write(0x5F | 0b10000000); spi.write(0x34); + spi.write(0x7B | 0b10000000); spi.write(0x08); + spi.write(0x7F | 0b10000000); spi.write(0x06); + spi.write(0x44 | 0b10000000); spi.write(0x1B); + spi.write(0x40 | 0b10000000); spi.write(0xBF); + spi.write(0x4E | 0b10000000); spi.write(0x3F); + spi.write(0x7F | 0b10000000); spi.write(0x08); + spi.write(0x65 | 0b10000000); spi.write(0x20); + spi.write(0x6A | 0b10000000); spi.write(0x18); + spi.write(0x7F | 0b10000000); spi.write(0x09); + spi.write(0x4F | 0b10000000); spi.write(0xAF); + spi.write(0x5F | 0b10000000); spi.write(0x40); + spi.write(0x48 | 0b10000000); spi.write(0x80); + spi.write(0x49 | 0b10000000); spi.write(0x80); + spi.write(0x57 | 0b10000000); spi.write(0x77); + spi.write(0x60 | 0b10000000); spi.write(0x78); + spi.write(0x61 | 0b10000000); spi.write(0x78); + spi.write(0x62 | 0b10000000); spi.write(0x08); + spi.write(0x63 | 0b10000000); spi.write(0x50); + spi.write(0x7F | 0b10000000); spi.write(0x0A); + spi.write(0x45 | 0b10000000); spi.write(0x60); + spi.write(0x7F | 0b10000000); spi.write(0x00); + spi.write(0x4D | 0b10000000); spi.write(0x11); + spi.write(0x55 | 0b10000000); spi.write(0x80); + spi.write(0x74 | 0b10000000); spi.write(0x1F); + spi.write(0x75 | 0b10000000); spi.write(0x1F); + spi.write(0x4A | 0b10000000); spi.write(0x78); + spi.write(0x4B | 0b10000000); spi.write(0x78); + spi.write(0x44 | 0b10000000); spi.write(0x08); + spi.write(0x45 | 0b10000000); spi.write(0x50); + spi.write(0x64 | 0b10000000); spi.write(0xFF); + spi.write(0x65 | 0b10000000); spi.write(0x1F); + spi.write(0x7F | 0b10000000); spi.write(0x14); + spi.write(0x65 | 0b10000000); spi.write(0x60); + spi.write(0x66 | 0b10000000); spi.write(0x08); + spi.write(0x63 | 0b10000000); spi.write(0x78); + spi.write(0x7F | 0b10000000); spi.write(0x15); + spi.write(0x48 | 0b10000000); spi.write(0x58); + spi.write(0x7F | 0b10000000); spi.write(0x07); + spi.write(0x41 | 0b10000000); spi.write(0x0D); + spi.write(0x43 | 0b10000000); spi.write(0x14); + spi.write(0x4B | 0b10000000); spi.write(0x0E); + spi.write(0x45 | 0b10000000); spi.write(0x0F); + spi.write(0x44 | 0b10000000); spi.write(0x42); + spi.write(0x4C | 0b10000000); spi.write(0x80); + spi.write(0x7F | 0b10000000); spi.write(0x10); + spi.write(0x5B | 0b10000000); spi.write(0x02); + spi.write(0x7F | 0b10000000); spi.write(0x07); + spi.write(0x40 | 0b10000000); spi.write(0x41); + spi.write(0x70 | 0b10000000); spi.write(0x00); + + // + wait(0.1); + spi.write(0x32 | 0b10000000); spi.write(0x44); + spi.write(0x7F | 0b10000000); spi.write(0x07); + spi.write(0x40 | 0b10000000); spi.write(0x40); + spi.write(0x7F | 0b10000000); spi.write(0x06); + spi.write(0x62 | 0b10000000); spi.write(0xf0); + spi.write(0x63 | 0b10000000); spi.write(0x00); + spi.write(0x7F | 0b10000000); spi.write(0x0D); + spi.write(0x48 | 0b10000000); spi.write(0xC0); + spi.write(0x6F | 0b10000000); spi.write(0xd5); + spi.write(0x7F | 0b10000000); spi.write(0x00); + spi.write(0x5B | 0b10000000); spi.write(0xa0); + spi.write(0x4E | 0b10000000); spi.write(0xA8); + spi.write(0x5A | 0b10000000); spi.write(0x50); + spi.write(0x40 | 0b10000000); spi.write(0x80); + + // Deselect the device by seting chip select high + cs = 1; +} + +void PMW3901::read_flow() +{ + // Data that we're going to read + char data[4]; + + // Select the device by seting chip select low + cs = 0; + + // + spi.write(MOTION & 0b01111111); spi.write(0x00); + // Write the register to read sensor ID (set MSB to 0 for read) + spi.write(DELTA_X_L & 0b01111111); data[0] = spi.write(0x00); + spi.write(DELTA_X_H & 0b01111111); data[1] = spi.write(0x00); + spi.write(DELTA_Y_L & 0b01111111); data[2] = spi.write(0x00); + spi.write(DELTA_Y_H & 0b01111111); data[3] = spi.write(0x00); + + // Reassemble the data (two 8 bit data into one 16 bit data) + int16_t x_raw = (data[1] << 8 ) | data[0]; + int16_t y_raw = (data[3] << 8 ) | data[2]; + // Convert to SI units [rad/s] + x = x_raw * 1.0f; + y = y_raw * 1.0f; + + // Deselect the device by seting chip select high + cs = 1; +} + \ No newline at end of file