Paparazzi UAS  v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
pmw3901.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) Tom van Dijk
3  *
4  * This file is part of paparazzi
5  *
6  * paparazzi is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2, or (at your option)
9  * any later version.
10  *
11  * paparazzi is distributed in the hope that it will be useful,
12  * but WITHOUT ANY WARRANTY; without even the implied warranty of
13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14  * GNU General Public License for more details.
15  *
16  * You should have received a copy of the GNU General Public License
17  * along with paparazzi; see the file COPYING. If not, see
18  * <http://www.gnu.org/licenses/>.
19  */
30 #include "pmw3901.h"
31 
32 #include "mcu_periph/sys_time.h"
33 
34 
35 // Based on crazyflie-firmware
36 #ifndef PMW3901_RAD_PER_PX
37 #define PMW3901_RAD_PER_PX 0.002443389
38 #endif
39 
40 // SPI divisor, to adjust the clock speed according to the PCLK
41 // Don't exceed 2MHz
42 #ifndef PMW3901_SPI_CDIV
43 #define PMW3901_SPI_CDIV SPIDiv256
44 #endif
45 
46 #define PMW3901_REG_MOTION 0x02
47 #define PMW3901_REG_DELTA_X_L 0x03
48 #define PMW3901_REG_DELTA_X_H 0x04
49 #define PMW3901_REG_DELTA_Y_L 0x05
50 #define PMW3901_REG_DELTA_Y_H 0x06
51 
52 
53 // Non-blocking read function
54 // returns true upon completion
55 static bool readRegister_nonblocking(struct pmw3901_t *pmw, uint8_t addr, uint8_t *value) {
56  switch (pmw->readwrite_state) {
57  case 0:
58  if (get_sys_time_usec() < pmw->readwrite_timeout) return false;
59  pmw->trans.output_buf[0] = addr & 0x7F; // MSB 0 => read
60  pmw->trans.output_length = 1;
61  pmw->trans.input_length = 0;
62  pmw->trans.select = SPISelect;
63  spi_submit(pmw->periph, &pmw->trans);
64  pmw->readwrite_state++;
65  /* Falls through. */
66  case 1:
67  if (pmw->trans.status == SPITransPending || pmw->trans.status == SPITransRunning) return false;
68  // Write addr complete
70  pmw->readwrite_state++;
71  /* Falls through. */
72  case 2:
73  if (get_sys_time_usec() < pmw->readwrite_timeout) return false;
74  // Addr-read delay passed
75  pmw->trans.output_length = 0;
76  pmw->trans.input_length = 1;
77  pmw->trans.select = SPIUnselect;
78  spi_submit(pmw->periph, &pmw->trans);
79  pmw->readwrite_state++;
80  /* Falls through. */
81  case 3:
82  if (pmw->trans.status == SPITransPending || pmw->trans.status == SPITransRunning) return false;
83  // Read complete
85  *value = pmw->trans.input_buf[0];
87  pmw->readwrite_state = 0;
88  return true;
89  default: return false;
90  }
91 }
92 
93 
94 // Blocking read/write functions
95 static uint8_t readRegister_blocking(struct pmw3901_t *pmw, uint8_t addr) {
96  pmw->trans.output_buf[0] = addr & 0x7F; // MSB 0 => read
97  pmw->trans.output_length = 1;
98  pmw->trans.input_length = 0;
99  pmw->trans.select = SPISelect;
100  spi_blocking_transceive(pmw->periph, &pmw->trans);
101  sys_time_usleep(35); // See ref firmware and datasheet
102  pmw->trans.output_length = 0;
103  pmw->trans.input_length = 1;
104  pmw->trans.select = SPIUnselect;
105  spi_blocking_transceive(pmw->periph, &pmw->trans);
107  return pmw->trans.input_buf[0];
108 }
109 
110 static void writeRegister_blocking(struct pmw3901_t *pmw, uint8_t addr, uint8_t data) {
111  pmw->trans.output_buf[0] = addr | 0x80; // MSB 1 => write
112  pmw->trans.output_buf[1] = data;
113  pmw->trans.output_length = 2;
114  pmw->trans.input_length = 0;
115  spi_blocking_transceive(pmw->periph, &pmw->trans);
116 }
117 
118 // For PixArt firmware compatibility:
119 #define writeRegister(_addr, _data) writeRegister_blocking(pmw, (_addr), (_data))
120 #define readRegister(_addr) readRegister_blocking(pmw, (_addr))
121 #define wait_ms(_ms) sys_time_usleep((_ms) * 1000)
122 
123 
124 static void initializeSensor(struct pmw3901_t *pmw) {
125  // Try to detect sensor before initializing
126  int tries = 0;
127  while (readRegister(0x00) != 0x49 && tries < 100) {
128  sys_time_usleep(50);
129  tries++;
130  }
131 
132  // From reference firmware
133  writeRegister(0x7F, 0x00);
134  writeRegister(0x55, 0x01);
135  writeRegister(0x50, 0x07);
136  writeRegister(0x7F, 0x0E);
137  writeRegister(0x43, 0x10);
138 
139  if (readRegister(0x67) & 0x40)
140  writeRegister(0x48, 0x04);
141 
142  else
143  writeRegister(0x48, 0x02);
144 
145  writeRegister(0x7F, 0x00);
146  writeRegister(0x51, 0x7B);
147  writeRegister(0x50, 0x00);
148  writeRegister(0x55, 0x00);
149  writeRegister(0x7F, 0x0E);
150 
151  if (readRegister(0x73) == 0x00) {
152  writeRegister(0x7F, 0x00);
153  writeRegister(0x61, 0xAD);
154  writeRegister(0x51, 0x70);
155  writeRegister(0x7F, 0x0E);
156 
157  if (readRegister(0x70) <= 28)
158  writeRegister(0x70, readRegister(0x70) + 14);
159 
160  else
161  writeRegister(0x70, readRegister(0x70) + 11);
162 
163  writeRegister(0x71, readRegister(0x71) * 45/100);
164  }
165 
166  writeRegister(0x7F, 0x00);
167  writeRegister(0x61, 0xAD);
168  writeRegister(0x7F, 0x03);
169  writeRegister(0x40, 0x00);
170  writeRegister(0x7F, 0x05);
171  writeRegister(0x41, 0xB3);
172  writeRegister(0x43, 0xF1);
173  writeRegister(0x45, 0x14);
174  writeRegister(0x5B, 0x32);
175  writeRegister(0x5F, 0x34);
176  writeRegister(0x7B, 0x08);
177  writeRegister(0x7F, 0x06);
178  writeRegister(0x44, 0x1B);
179  writeRegister(0x40, 0xBF);
180  writeRegister(0x4E, 0x3F);
181  writeRegister(0x7F, 0x06);
182  writeRegister(0x44, 0x1B);
183  writeRegister(0x40, 0xBF);
184  writeRegister(0x4E, 0x3F);
185  writeRegister(0x7F, 0x08);
186  writeRegister(0x65, 0x20);
187  writeRegister(0x6A, 0x18);
188  writeRegister(0x7F, 0x09);
189  writeRegister(0x4F, 0xAF);
190  writeRegister(0x5F, 0x40);
191  writeRegister(0x48, 0x80);
192  writeRegister(0x49, 0x80);
193  writeRegister(0x57, 0x77);
194  writeRegister(0x60, 0x78);
195  writeRegister(0x61, 0x78);
196  writeRegister(0x62, 0x08);
197  writeRegister(0x63, 0x50);
198  writeRegister(0x7F, 0x0A);
199  writeRegister(0x45, 0x60);
200  writeRegister(0x7F, 0x00);
201  writeRegister(0x4D, 0x11);
202  writeRegister(0x55, 0x80);
203  writeRegister(0x74, 0x21);
204  writeRegister(0x75, 0x1F);
205  writeRegister(0x4A, 0x78);
206  writeRegister(0x4B, 0x78);
207  writeRegister(0x44, 0x08);
208  writeRegister(0x45, 0x50);
209  writeRegister(0x64, 0xFF);
210  writeRegister(0x65, 0x1F);
211  writeRegister(0x7F, 0x14);
212  writeRegister(0x65, 0x67);
213  writeRegister(0x66, 0x08);
214  writeRegister(0x63, 0x70);
215  writeRegister(0x7F, 0x15);
216  writeRegister(0x48, 0x48);
217  writeRegister(0x7F, 0x07);
218  writeRegister(0x41, 0x0D);
219  writeRegister(0x43, 0x14);
220  writeRegister(0x4B, 0x0E);
221  writeRegister(0x45, 0x0F);
222  writeRegister(0x44, 0x42);
223  writeRegister(0x4C, 0x80);
224  writeRegister(0x7F, 0x10);
225  writeRegister(0x5B, 0x02);
226  writeRegister(0x7F, 0x07);
227  writeRegister(0x40, 0x41);
228  writeRegister(0x70, 0x00);
229 
230  wait_ms(10);
231 
232  writeRegister(0x32, 0x44);
233  writeRegister(0x7F, 0x07);
234  writeRegister(0x40, 0x40);
235  writeRegister(0x7F, 0x06);
236  writeRegister(0x62, 0xF0);
237  writeRegister(0x63, 0x00);
238  writeRegister(0x7F, 0x0D);
239  writeRegister(0x48, 0xC0);
240  writeRegister(0x6F, 0xD5);
241  writeRegister(0x7F, 0x00);
242  writeRegister(0x5B, 0xA0);
243  writeRegister(0x4E, 0xA8);
244  writeRegister(0x5A, 0x50);
245  writeRegister(0x40, 0x80);
246 }
247 
248 
249 void pmw3901_init(struct pmw3901_t *pmw, struct spi_periph *periph, uint8_t slave_idx) {
250  // Set up SPI peripheral and transaction
251  pmw->periph = periph;
252  pmw->trans.input_buf = pmw->spi_input_buf;
253  pmw->trans.output_buf = pmw->spi_output_buf;
254  pmw->trans.slave_idx = slave_idx;
256  pmw->trans.cpol = SPICpolIdleLow;
257  pmw->trans.cpha = SPICphaEdge1;
258  pmw->trans.dss = SPIDss8bit;
259  pmw->trans.bitorder = SPIMSBFirst;
260  pmw->trans.cdiv = PMW3901_SPI_CDIV;
261  pmw->trans.before_cb = NULL;
262  pmw->trans.after_cb = NULL;
263  pmw->trans.status = SPITransDone;
264  // Initialize sensor registers
265  initializeSensor(pmw);
266  // Set up remaining fields
267  pmw->state = PMW3901_IDLE;
268  pmw->delta_x = 0;
269  pmw->delta_y = 0;
270  pmw->data_available = false;
272 }
273 
274 void pmw3901_event(struct pmw3901_t *pmw) {
275  uint8_t temp;
276  switch (pmw->state) {
277  case PMW3901_IDLE:
278  /* Do nothing */
279  return;
280  case PMW3901_READ_MOTION:
281  if (!readRegister_nonblocking(pmw, PMW3901_REG_MOTION, &temp)) return;
282  if (!(temp & 0x80)) return;
283  pmw->delta_x = 0;
284  pmw->delta_y = 0;
285  pmw->state++;
286  /* Falls through. */
288  if (!readRegister_nonblocking(pmw, PMW3901_REG_DELTA_X_L, &temp)) return;
289  pmw->delta_x |= temp;
290  pmw->state++;
291  /* Falls through. */
293  if (!readRegister_nonblocking(pmw, PMW3901_REG_DELTA_X_H, &temp)) return;
294  pmw->delta_x |= (temp << 8) & 0xFF00;
295  pmw->state++;
296  /* Falls through. */
298  if (!readRegister_nonblocking(pmw, PMW3901_REG_DELTA_Y_L, &temp)) return;
299  pmw->delta_y |= temp;
300  pmw->state++;
301  /* Falls through. */
303  if (!readRegister_nonblocking(pmw, PMW3901_REG_DELTA_Y_H, &temp)) return;
304  pmw->delta_y |= (temp << 8) & 0xFF00;
305  pmw->data_available = true;
306  pmw->state = PMW3901_IDLE;
307  return;
308  default: return;
309  }
310 }
311 
312 bool pmw3901_is_idle(struct pmw3901_t *pmw) {
313  return pmw->state == PMW3901_IDLE;
314 }
315 
316 void pmw3901_start_read(struct pmw3901_t *pmw) {
317  if (pmw3901_is_idle(pmw)) {
318  pmw->state = PMW3901_READ_MOTION;
319  }
320 }
321 
323  return pmw->data_available;
324 }
325 
326 bool pmw3901_get_data(struct pmw3901_t *pmw, int16_t *delta_x, int16_t *delta_y) {
327  if (!pmw->data_available) return false;
328  *delta_x = pmw->delta_x;
329  *delta_y = pmw->delta_y;
330  pmw->data_available = false;
331  return true;
332 }
void sys_time_usleep(uint32_t us)
sys_time_usleep(uint32_t us)
uint32_t get_sys_time_usec(void)
Get the time in microseconds since startup.
Definition: sys_time_arch.c:71
enum SPIClockPolarity cpol
clock polarity control
Definition: spi.h:155
enum SPIClockPhase cpha
clock phase control
Definition: spi.h:156
enum SPISlaveSelect select
slave selection behavior
Definition: spi.h:154
SPICallback before_cb
NULL or function called before the transaction.
Definition: spi.h:160
SPICallback after_cb
NULL or function called after the transaction.
Definition: spi.h:161
enum SPIDataSizeSelect dss
data transfer word size
Definition: spi.h:157
volatile uint8_t * output_buf
pointer to transmit buffer for DMA
Definition: spi.h:150
uint16_t input_length
number of data words to read
Definition: spi.h:151
enum SPIClockDiv cdiv
prescaler of main clock to use as SPI clock
Definition: spi.h:159
volatile uint8_t * input_buf
pointer to receive buffer for DMA
Definition: spi.h:149
uint8_t slave_idx
slave id: SPI_SLAVE0 to SPI_SLAVE4
Definition: spi.h:153
enum SPIBitOrder bitorder
MSB/LSB order.
Definition: spi.h:158
uint16_t output_length
number of data words to write
Definition: spi.h:152
enum SPITransactionStatus status
Definition: spi.h:162
bool spi_submit(struct spi_periph *p, struct spi_transaction *t)
Submit SPI transaction.
Definition: spi_arch.c:533
static bool spi_blocking_transceive(struct spi_periph *p, struct spi_transaction *t)
Perform a spi transaction (blocking).
Definition: spi.h:299
@ SPICphaEdge1
CPHA = 0.
Definition: spi.h:74
@ SPITransRunning
Definition: spi.h:98
@ SPITransPending
Definition: spi.h:97
@ SPITransDone
Definition: spi.h:101
@ SPICpolIdleLow
CPOL = 0.
Definition: spi.h:83
@ SPISelect
slave is selected before transaction but not unselected
Definition: spi.h:64
@ SPISelectUnselect
slave is selected before transaction and unselected after
Definition: spi.h:63
@ SPIUnselect
slave is not selected but unselected after transaction
Definition: spi.h:65
@ SPIMSBFirst
Definition: spi.h:112
@ SPIDss8bit
Definition: spi.h:90
SPI peripheral structure.
Definition: spi.h:174
static uint8_t readRegister_blocking(struct pmw3901_t *pmw, uint8_t addr)
Definition: pmw3901.c:95
#define PMW3901_RAD_PER_PX
Definition: pmw3901.c:37
#define PMW3901_REG_MOTION
Definition: pmw3901.c:46
#define wait_ms(_ms)
Definition: pmw3901.c:121
#define PMW3901_SPI_CDIV
Definition: pmw3901.c:43
bool pmw3901_is_idle(struct pmw3901_t *pmw)
Definition: pmw3901.c:312
void pmw3901_start_read(struct pmw3901_t *pmw)
Definition: pmw3901.c:316
static bool readRegister_nonblocking(struct pmw3901_t *pmw, uint8_t addr, uint8_t *value)
Definition: pmw3901.c:55
void pmw3901_init(struct pmw3901_t *pmw, struct spi_periph *periph, uint8_t slave_idx)
Definition: pmw3901.c:249
static void writeRegister_blocking(struct pmw3901_t *pmw, uint8_t addr, uint8_t data)
Definition: pmw3901.c:110
void pmw3901_event(struct pmw3901_t *pmw)
Definition: pmw3901.c:274
#define PMW3901_REG_DELTA_X_H
Definition: pmw3901.c:48
#define readRegister(_addr)
Definition: pmw3901.c:120
bool pmw3901_data_available(struct pmw3901_t *pmw)
Definition: pmw3901.c:322
#define PMW3901_REG_DELTA_X_L
Definition: pmw3901.c:47
#define PMW3901_REG_DELTA_Y_H
Definition: pmw3901.c:50
#define writeRegister(_addr, _data)
Definition: pmw3901.c:119
bool pmw3901_get_data(struct pmw3901_t *pmw, int16_t *delta_x, int16_t *delta_y)
Definition: pmw3901.c:326
#define PMW3901_REG_DELTA_Y_L
Definition: pmw3901.c:49
static void initializeSensor(struct pmw3901_t *pmw)
Definition: pmw3901.c:124
volatile uint8_t spi_output_buf[SPI_BUFFER_SIZE]
Definition: pmw3901.h:64
struct spi_periph * periph
Definition: pmw3901.h:61
uint8_t readwrite_state
Definition: pmw3901.h:66
volatile uint8_t spi_input_buf[SPI_BUFFER_SIZE]
Definition: pmw3901.h:63
float rad_per_px
Definition: pmw3901.h:71
int16_t delta_y
Definition: pmw3901.h:69
struct spi_transaction trans
Definition: pmw3901.h:62
bool data_available
Definition: pmw3901.h:70
uint32_t readwrite_timeout
Definition: pmw3901.h:67
enum pmw3901_state state
Definition: pmw3901.h:65
int16_t delta_x
Definition: pmw3901.h:68
@ PMW3901_IDLE
Definition: pmw3901.h:52
@ PMW3901_READ_DELTAXHIGH
Definition: pmw3901.h:55
@ PMW3901_READ_MOTION
Definition: pmw3901.h:53
@ PMW3901_READ_DELTAYLOW
Definition: pmw3901.h:56
@ PMW3901_READ_DELTAXLOW
Definition: pmw3901.h:54
@ PMW3901_READ_DELTAYHIGH
Definition: pmw3901.h:57
Architecture independent timing functions.
short int16_t
Typedef defining 16 bit short type.
Definition: vl53l1_types.h:93
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
Definition: vl53l1_types.h:98