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
- Committer:
- fbob
- Date:
- 2018-09-27
- Revision:
- 14:e4d638ac83af
- Parent:
- 8:930aa9d0f5ae
- Child:
- 15:e07de535b86f
File content as of revision 14:e4d638ac83af:
#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 [px] x = -y_raw * 1.0f; y = -x_raw * 1.0f; // Deselect the device by seting chip select high cs = 1; }