Fabio Bobrow / CrazyflieSensors

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependents:   Drones-Controlador controladoatitude_cteste Drone_Controlador_Atitude optical_test

PMW3901/PMW3901.cpp@6:c7bc001826ba, 2018-05-08 (annotated)

Committer:
fbob
Date:
Tue May 08 13:35:42 2018 +0000
Revision:
6:c7bc001826ba
Parent:
5:1ef8b91a0318 
Updated optical flow library

Who changed what in which revision?

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