PX4 Firmware
PX4 Autopilot Software http://px4.io
PMW3901.cpp
Go to the documentation of this file.
1 /****************************************************************************
2  *
3  * Copyright (c) 2018 PX4 Development Team. All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions
7  * are met:
8  *
9  * 1. Redistributions of source code must retain the above copyright
10  * notice, this list of conditions and the following disclaimer.
11  * 2. Redistributions in binary form must reproduce the above copyright
12  * notice, this list of conditions and the following disclaimer in
13  * the documentation and/or other materials provided with the
14  * distribution.
15  * 3. Neither the name PX4 nor the names of its contributors may be
16  * used to endorse or promote products derived from this software
17  * without specific prior written permission.
18  *
19  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
20  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
21  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
22  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
23  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
24  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
25  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
26  * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
27  * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
28  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
29  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
30  * POSSIBILITY OF SUCH DAMAGE.
31  *
32  ****************************************************************************/
33 
34 #include "PMW3901.hpp"
35 
36 static constexpr uint32_t TIME_us_TSWW = 11; // - actually 10.5us
37 
38 PMW3901::PMW3901(int bus, enum Rotation yaw_rotation) :
39  SPI("PMW3901", PMW3901_DEVICE_PATH, bus, PMW3901_SPIDEV, SPIDEV_MODE0, PMW3901_SPI_BUS_SPEED),
40  ScheduledWorkItem(MODULE_NAME, px4::device_bus_to_wq(get_device_id())),
41  _sample_perf(perf_alloc(PC_ELAPSED, "pmw3901: read")),
42  _comms_errors(perf_alloc(PC_COUNT, "pmw3901: com err")),
43  _yaw_rotation(yaw_rotation)
44 {
45 }
46 
48 {
49  // make sure we are truly inactive
50  stop();
51 
52  // free perf counters
55 }
56 
57 int
59 {
60  uint8_t data[5] {};
61 
62  // Power on reset
63  writeRegister(0x3A, 0x5A);
64  usleep(5000);
65 
66  // Reading the motion registers one time
67  readRegister(0x02, &data[0], 1);
68  readRegister(0x03, &data[1], 1);
69  readRegister(0x04, &data[2], 1);
70  readRegister(0x05, &data[3], 1);
71  readRegister(0x06, &data[4], 1);
72 
73  usleep(1000);
74 
75  // set performance optimization registers
76  // from PixArt PMW3901MB Optical Motion Tracking chip demo kit V3.20 (21 Aug 2018)
77  unsigned char v = 0;
78  unsigned char c1 = 0;
79  unsigned char c2 = 0;
80 
81  writeRegister(0x7F, 0x00);
82  writeRegister(0x55, 0x01);
83  writeRegister(0x50, 0x07);
84  writeRegister(0x7f, 0x0e);
85  writeRegister(0x43, 0x10);
86 
87  readRegister(0x67, &v, 1);
88 
89  // if bit7 is set
90  if (v & (1 << 7)) {
91  writeRegister(0x48, 0x04);
92 
93  } else {
94  writeRegister(0x48, 0x02);
95  }
96 
97  writeRegister(0x7F, 0x00);
98  writeRegister(0x51, 0x7b);
99  writeRegister(0x50, 0x00);
100  writeRegister(0x55, 0x00);
101 
102  writeRegister(0x7F, 0x0e);
103  readRegister(0x73, &v, 1);
104 
105  if (v == 0) {
106  readRegister(0x70, &c1, 1);
107 
108  if (c1 <= 28) {
109  c1 = c1 + 14;
110 
111  } else {
112  c1 = c1 + 11;
113  }
114 
115  if (c1 > 0x3F) {
116  c1 = 0x3F;
117  }
118 
119  readRegister(0x71, &c2, 1);
120  c2 = ((unsigned short)c2 * 45) / 100;
121 
122  writeRegister(0x7f, 0x00);
123  writeRegister(0x61, 0xAD);
124  writeRegister(0x51, 0x70);
125  writeRegister(0x7f, 0x0e);
126  writeRegister(0x70, c1);
127  writeRegister(0x71, c2);
128  }
129 
130  writeRegister(0x7F, 0x00);
131  writeRegister(0x61, 0xAD);
132  writeRegister(0x7F, 0x03);
133  writeRegister(0x40, 0x00);
134  writeRegister(0x7F, 0x05);
135  writeRegister(0x41, 0xB3);
136  writeRegister(0x43, 0xF1);
137  writeRegister(0x45, 0x14);
138  writeRegister(0x5B, 0x32);
139  writeRegister(0x5F, 0x34);
140  writeRegister(0x7B, 0x08);
141  writeRegister(0x7F, 0x06);
142  writeRegister(0x44, 0x1B);
143  writeRegister(0x40, 0xBF);
144  writeRegister(0x4E, 0x3F);
145  writeRegister(0x7F, 0x08);
146  writeRegister(0x65, 0x20);
147  writeRegister(0x6A, 0x18);
148  writeRegister(0x7F, 0x09);
149  writeRegister(0x4F, 0xAF);
150  writeRegister(0x5F, 0x40);
151  writeRegister(0x48, 0x80);
152  writeRegister(0x49, 0x80);
153  writeRegister(0x57, 0x77);
154  writeRegister(0x60, 0x78);
155  writeRegister(0x61, 0x78);
156  writeRegister(0x62, 0x08);
157  writeRegister(0x63, 0x50);
158  writeRegister(0x7F, 0x0A);
159  writeRegister(0x45, 0x60);
160  writeRegister(0x7F, 0x00);
161  writeRegister(0x4D, 0x11);
162  writeRegister(0x55, 0x80);
163  writeRegister(0x74, 0x21);
164  writeRegister(0x75, 0x1F);
165  writeRegister(0x4A, 0x78);
166  writeRegister(0x4B, 0x78);
167  writeRegister(0x44, 0x08);
168  writeRegister(0x45, 0x50);
169  writeRegister(0x64, 0xFF);
170  writeRegister(0x65, 0x1F);
171  writeRegister(0x7F, 0x14);
172  writeRegister(0x65, 0x67);
173  writeRegister(0x66, 0x08);
174  writeRegister(0x63, 0x70);
175  writeRegister(0x7F, 0x15);
176  writeRegister(0x48, 0x48);
177  writeRegister(0x7F, 0x07);
178  writeRegister(0x41, 0x0D);
179  writeRegister(0x43, 0x14);
180  writeRegister(0x4B, 0x0E);
181  writeRegister(0x45, 0x0F);
182  writeRegister(0x44, 0x42);
183  writeRegister(0x4C, 0x80);
184  writeRegister(0x7F, 0x10);
185  writeRegister(0x5B, 0x02);
186  writeRegister(0x7F, 0x07);
187  writeRegister(0x40, 0x41);
188  writeRegister(0x70, 0x00);
189 
190  px4_usleep(10000); // delay 10ms
191 
192  writeRegister(0x32, 0x44);
193  writeRegister(0x7F, 0x07);
194  writeRegister(0x40, 0x40);
195  writeRegister(0x7F, 0x06);
196  writeRegister(0x62, 0xF0);
197  writeRegister(0x63, 0x00);
198  writeRegister(0x7F, 0x0D);
199  writeRegister(0x48, 0xC0);
200  writeRegister(0x6F, 0xD5);
201  writeRegister(0x7F, 0x00);
202  writeRegister(0x5B, 0xA0);
203  writeRegister(0x4E, 0xA8);
204  writeRegister(0x5A, 0x50);
205  writeRegister(0x40, 0x80);
206 
207  return PX4_OK;
208 }
209 
210 int
212 {
213  // get yaw rotation from sensor frame to body frame
214  param_t rot = param_find("SENS_FLOW_ROT");
215 
216  if (rot != PARAM_INVALID) {
217  int32_t val = 0;
218  param_get(rot, &val);
219 
220  _yaw_rotation = (enum Rotation)val;
221  }
222 
223  /* For devices competing with NuttX SPI drivers on a bus (Crazyflie SD Card expansion board) */
224  SPI::set_lockmode(LOCK_THREADS);
225 
226  /* do SPI init (and probe) first */
227  if (SPI::init() != OK) {
228  return PX4_ERROR;
229  }
230 
231  sensorInit();
232 
234 
235  start();
236 
237  return PX4_OK;
238 }
239 
240 int
242 {
243  uint8_t data[2] {};
244 
245  readRegister(0x00, &data[0], 1); // chip id
246 
247  // Test the SPI communication, checking chipId and inverse chipId
248  if (data[0] == 0x49) {
249  return OK;
250  }
251 
252  // not found on any address
253  return -EIO;
254 }
255 
256 int
257 PMW3901::readRegister(unsigned reg, uint8_t *data, unsigned count)
258 {
259  uint8_t cmd[5]; // read up to 4 bytes
260 
261  cmd[0] = DIR_READ(reg);
262 
263  int ret = transfer(&cmd[0], &cmd[0], count + 1);
264 
265  if (OK != ret) {
267  DEVICE_LOG("spi::transfer returned %d", ret);
268  return ret;
269  }
270 
271  memcpy(&data[0], &cmd[1], count);
272 
273  return ret;
274 }
275 
276 int
277 PMW3901::writeRegister(unsigned reg, uint8_t data)
278 {
279  uint8_t cmd[2]; // write 1 byte
280  int ret;
281 
282  cmd[0] = DIR_WRITE(reg);
283  cmd[1] = data;
284 
285  ret = transfer(&cmd[0], nullptr, 2);
286 
287  if (OK != ret) {
289  DEVICE_LOG("spi::transfer returned %d", ret);
290  return ret;
291  }
292 
293  px4_usleep(TIME_us_TSWW);
294 
295  return ret;
296 }
297 
298 void
300 {
302 
303  int16_t delta_x_raw = 0;
304  int16_t delta_y_raw = 0;
305  uint8_t qual = 0;
306  float delta_x = 0.0f;
307  float delta_y = 0.0f;
308 
309  uint64_t timestamp = hrt_absolute_time();
310  uint64_t dt_flow = timestamp - _previous_collect_timestamp;
311  _previous_collect_timestamp = timestamp;
312 
313  _flow_dt_sum_usec += dt_flow;
314 
315  readMotionCount(delta_x_raw, delta_y_raw, qual);
316 
317  if (qual > 0) {
318  _flow_sum_x += delta_x_raw;
319  _flow_sum_y += delta_y_raw;
321  _flow_quality_sum += qual;
322  }
323 
324  // returns if the collect time has not been reached
326  return;
327  }
328 
329  delta_x = (float)_flow_sum_x / 500.0f; // proportional factor + convert from pixels to radians
330  delta_y = (float)_flow_sum_y / 500.0f; // proportional factor + convert from pixels to radians
331 
332  optical_flow_s report{};
333  report.timestamp = timestamp;
334 
335  report.pixel_flow_x_integral = static_cast<float>(delta_x);
336  report.pixel_flow_y_integral = static_cast<float>(delta_y);
337 
338  // rotate measurements in yaw from sensor frame to body frame according to parameter SENS_FLOW_ROT
339  float zeroval = 0.0f;
340  rotate_3f(_yaw_rotation, report.pixel_flow_x_integral, report.pixel_flow_y_integral, zeroval);
341  rotate_3f(_yaw_rotation, report.gyro_x_rate_integral, report.gyro_y_rate_integral, report.gyro_z_rate_integral);
342 
343  report.frame_count_since_last_readout = _flow_sample_counter; // number of frames
344  report.integration_timespan = _flow_dt_sum_usec; // microseconds
345 
346  report.sensor_id = 0;
347  report.quality = _flow_sample_counter > 0 ? _flow_quality_sum / _flow_sample_counter : 0;
348 
349 
350  /* No gyro on this board */
351  report.gyro_x_rate_integral = NAN;
352  report.gyro_y_rate_integral = NAN;
353  report.gyro_z_rate_integral = NAN;
354 
355  // set (conservative) specs according to datasheet
356  report.max_flow_rate = 5.0f; // Datasheet: 7.4 rad/s
357  report.min_ground_distance = 0.1f; // Datasheet: 80mm
358  report.max_ground_distance = 30.0f; // Datasheet: infinity
359 
360  _flow_dt_sum_usec = 0;
361  _flow_sum_x = 0;
362  _flow_sum_y = 0;
364  _flow_quality_sum = 0;
365 
366  _optical_flow_pub.publish(report);
367 
369 }
370 
371 int
372 PMW3901::readMotionCount(int16_t &deltaX, int16_t &deltaY, uint8_t &qual)
373 {
374  uint8_t data[12] = { DIR_READ(0x02), 0, DIR_READ(0x03), 0, DIR_READ(0x04), 0,
375  DIR_READ(0x05), 0, DIR_READ(0x06), 0, DIR_READ(0x07), 0
376  };
377 
378  int ret = transfer(&data[0], &data[0], 12);
379 
380  if (OK != ret) {
381  qual = 0;
383  DEVICE_LOG("spi::transfer returned %d", ret);
384  return ret;
385  }
386 
387  deltaX = ((int16_t)data[5] << 8) | data[3];
388  deltaY = ((int16_t)data[9] << 8) | data[7];
389 
390  // If the reported flow is impossibly large, we just got garbage from the SPI
391  if (deltaX > 240 || deltaY > 240 || deltaX < -240 || deltaY < -240) {
392  qual = 0;
393 
394  } else {
395  qual = data[11];
396  }
397 
398  ret = OK;
399 
400  return ret;
401 }
402 
403 void
405 {
406  // schedule a cycle to start things
407  ScheduleOnInterval(PMW3901_SAMPLE_INTERVAL, PMW3901_US);
408 }
409 
410 void
412 {
413  ScheduleClear();
414 }
415 
416 void
418 {
421 }
#define PARAM_INVALID
Handle returned when a parameter cannot be found.
Definition: param.h:103
virtual int probe()
Definition: PMW3901.cpp:241
int readRegister(unsigned reg, uint8_t *data, unsigned count)
Definition: PMW3901.cpp:257
measure the time elapsed performing an event
Definition: perf_counter.h:56
static constexpr uint32_t TIME_us_TSWW
Definition: PMW3901.cpp:36
__EXPORT int param_get(param_t param, void *val)
Copy the value of a parameter.
Definition: parameters.cpp:589
uint64_t timestamp
Definition: optical_flow.h:53
uint8_t _flow_sample_counter
Definition: PMW3901.hpp:121
__EXPORT void rotate_3f(enum Rotation rot, float &x, float &y, float &z)
rotate a 3 element float vector in-place
Definition: rotation.cpp:63
int readMotionCount(int16_t &deltaX, int16_t &deltaY, uint8_t &qual)
Definition: PMW3901.cpp:372
uint64_t _previous_collect_timestamp
Definition: PMW3901.hpp:113
#define PMW3901_SPI_BUS_SPEED
Definition: PMW3901.hpp:75
void print_info()
Diagnostics - print some basic information about the driver.
Definition: PMW3901.cpp:417
uORB::PublicationMulti< optical_flow_s > _optical_flow_pub
Definition: PMW3901.hpp:108
count the number of times an event occurs
Definition: perf_counter.h:55
perf_counter_t _comms_errors
Definition: PMW3901.hpp:111
int _flow_sum_x
Definition: PMW3901.hpp:117
#define DIR_READ
Definition: bmp388_spi.cpp:46
uint64_t _flow_dt_sum_usec
Definition: PMW3901.hpp:119
void perf_count(perf_counter_t handle)
Count a performance event.
int sensorInit()
Definition: PMW3901.cpp:58
void perf_free(perf_counter_t handle)
Free a counter.
void init()
Activates/configures the hardware registers.
void Run() override
Perform a poll cycle; collect from the previous measurement and start a new one.
Definition: PMW3901.cpp:299
int _flow_sum_y
Definition: PMW3901.hpp:118
#define DEVICE_LOG(FMT,...)
Definition: Device.hpp:51
#define perf_alloc(a, b)
Definition: px4io.h:59
int writeRegister(unsigned reg, uint8_t data)
Definition: PMW3901.cpp:277
Rotation
Enum for board and external compass rotations.
Definition: rotation.h:51
uint8_t * data
Definition: dataman.cpp:149
#define PMW3901_SAMPLE_INTERVAL
Definition: PMW3901.hpp:84
Vector< float, 6 > f(float t, const Matrix< float, 6, 1 > &, const Matrix< float, 3, 1 > &)
Definition: integration.cpp:8
virtual ~PMW3901()
Definition: PMW3901.cpp:47
void stop()
Stop the automatic measurement state machine.
Definition: PMW3901.cpp:411
void perf_end(perf_counter_t handle)
End a performance event.
enum Rotation _yaw_rotation
Definition: PMW3901.hpp:115
void start()
Initialise the automatic measurement state machine and start it.
Definition: PMW3901.cpp:404
__EXPORT param_t param_find(const char *name)
Look up a parameter by name.
Definition: parameters.cpp:370
#define DIR_WRITE
Definition: bmp388_spi.cpp:47
const uint64_t _collect_time
Definition: PMW3901.hpp:106
void perf_print_counter(perf_counter_t handle)
Print one performance counter to stdout.
PMW3901(int bus=PMW3901_BUS, enum Rotation yaw_rotation=(enum Rotation) 0)
Definition: PMW3901.cpp:38
Definition: bst.cpp:62
#define PMW3901_US
Definition: PMW3901.hpp:83
uint16_t _flow_quality_sum
Definition: PMW3901.hpp:120
perf_counter_t _sample_perf
Definition: PMW3901.hpp:110
#define OK
Definition: uavcan_main.cpp:71
#define PMW3901_DEVICE_PATH
Definition: PMW3901.hpp:80
bool publish(const T &data)
Publish the struct.
virtual int init()
Definition: PMW3901.cpp:211
void perf_begin(perf_counter_t handle)
Begin a performance event.
__EXPORT hrt_abstime hrt_absolute_time(void)
Get absolute time in [us] (does not wrap).
uint32_t param_t
Parameter handle.
Definition: param.h:98