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

PMW3901/PMW3901.cpp@15:e07de535b86f, 2018-10-17 (annotated)

Committer:
fbob
Date:
Wed Oct 17 13:26:29 2018 +0000
Revision:
15:e07de535b86f
Parent:
14:e4d638ac83af 
Changed range and optical flow parameters names

Who changed what in which revision?

UserRevisionLine numberNew 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 15:e07de535b86f 179 int16_t px_raw = (data[1] << 8 ) | data[0];
fbob 15:e07de535b86f 180 int16_t py_raw = (data[3] << 8 ) | data[2];
fbob 14:e4d638ac83af 181 // Convert to SI units [px]
fbob 15:e07de535b86f 182 px = -py_raw * 1.0f;
fbob 15:e07de535b86f 183 py = -px_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