APM:Libraries
AP_OpticalFlow_Pixart.cpp
Go to the documentation of this file.
1 /*
2  This program is free software: you can redistribute it and/or modify
3  it under the terms of the GNU General Public License as published by
4  the Free Software Foundation, either version 3 of the License, or
5  (at your option) any later version.
6 
7  This program is distributed in the hope that it will be useful,
8  but WITHOUT ANY WARRANTY; without even the implied warranty of
9  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
10  GNU General Public License for more details.
11 
12  You should have received a copy of the GNU General Public License
13  along with this program. If not, see <http://www.gnu.org/licenses/>.
14  */
15 /*
16  driver for Pixart PMW3900DH optical flow sensor
17 
18 
19  NOTE: This sensor does not use traditional SPI register access. The
20  timing for register reads and writes is critical
21  */
22 
23 #include <AP_HAL/AP_HAL.h>
24 #include <AP_Math/edc.h>
25 #include <AP_AHRS/AP_AHRS.h>
26 #include <utility>
27 #include "OpticalFlow.h"
28 #include "AP_OpticalFlow_Pixart.h"
30 #include <stdio.h>
31 
32 #define debug(fmt, args ...) do {printf(fmt, ## args); } while(0)
33 
34 extern const AP_HAL::HAL& hal;
35 
36 #define PIXART_REG_PRODUCT_ID 0x00
37 #define PIXART_REG_REVISION_ID 0x01
38 #define PIXART_REG_MOTION 0x02
39 #define PIXART_REG_DELTA_X_L 0x03
40 #define PIXART_REG_DELTA_X_H 0x04
41 #define PIXART_REG_DELTA_Y_L 0x05
42 #define PIXART_REG_DELTA_Y_H 0x06
43 #define PIXART_REG_SQUAL 0x07
44 #define PIXART_REG_RAWDATA_SUM 0x08
45 #define PIXART_REG_RAWDATA_MAX 0x09
46 #define PIXART_REG_RAWDATA_MIN 0x0A
47 #define PIXART_REG_SHUTTER_LOW 0x0B
48 #define PIXART_REG_SHUTTER_HI 0x0C
49 #define PIXART_REG_CONFIG1 0x0F
50 #define PIXART_REG_CONFIG2 0x10
51 #define PIXART_REG_FRAME_CAP 0x12
52 #define PIXART_REG_SROM_EN 0x13
53 #define PIXART_REG_RUN_DS 0x14
54 #define PIXART_REG_REST1_RATE 0x15
55 #define PIXART_REG_REST1_DS 0x16
56 #define PIXART_REG_REST2_RATE 0x17
57 #define PIXART_REG_REST2_DS 0x18
58 #define PIXART_REG_REST3_RATE 0x19
59 #define PIXART_REG_OBS 0x24
60 #define PIXART_REG_DOUT_L 0x25
61 #define PIXART_REG_DOUT_H 0x26
62 #define PIXART_REG_RAW_GRAB 0x29
63 #define PIXART_REG_SROM_ID 0x2A
64 #define PIXART_REG_POWER_RST 0x3A
65 #define PIXART_REG_SHUTDOWN 0x3B
66 #define PIXART_REG_INV_PROD_ID 0x3F
67 #define PIXART_REG_INV_PROD_ID2 0x5F // for 3901
68 #define PIXART_REG_MOT_BURST 0x50
69 #define PIXART_REG_MOT_BURST2 0x16
70 #define PIXART_REG_SROM_BURST 0x62
71 #define PIXART_REG_RAW_BURST 0x64
72 
73 // writing to registers needs this flag
74 #define PIXART_WRITE_FLAG 0x80
75 
76 // timings in microseconds
77 #define PIXART_Tsrad 300
78 
79 // correct result for SROM CRC
80 #define PIXART_SROM_CRC_RESULT 0xBEEF
81 
82 // constructor
84  OpticalFlow_backend(_frontend)
85 {
86  _dev = std::move(hal.spi->get_device(devname));
87 }
88 
89 
90 // detect the device
92 {
93  AP_OpticalFlow_Pixart *sensor = new AP_OpticalFlow_Pixart(devname, _frontend);
94  if (!sensor) {
95  return nullptr;
96  }
97  if (!sensor->setup_sensor()) {
98  delete sensor;
99  return nullptr;
100  }
101  return sensor;
102 }
103 
104 // setup the device
106 {
107  if (!_dev) {
108  return false;
109  }
111  AP_HAL::panic("Unable to get bus semaphore");
112  }
113 
114  uint8_t id;
115  uint16_t crc;
116 
117  // power-up sequence
119  hal.scheduler->delay(50);
120 
121  // check product ID
122  uint8_t id1 = reg_read(PIXART_REG_PRODUCT_ID);
123  uint8_t id2;
124  if (id1 == 0x3f) {
126  } else {
128  }
129  debug("id1=0x%02x id2=0x%02x ~id1=0x%02x\n", id1, id2, uint8_t(~id1));
130  if (id1 == 0x3F && id2 == uint8_t(~id1)) {
131  model = PIXART_3900;
132  } else if (id1 == 0x49 && id2 == uint8_t(~id1)) {
133  model = PIXART_3901;
134  } else {
135  debug("Not a recognised device\n");
136  goto failed;
137  }
138 
139  if (model == PIXART_3900) {
140  srom_download();
141 
143  if (id != srom_id) {
144  debug("Pixart: bad SROM ID: 0x%02x\n", id);
145  goto failed;
146  }
147 
149  hal.scheduler->delay(10);
150 
152  if (crc != 0xBEEF) {
153  debug("Pixart: bad SROM CRC: 0x%04x\n", crc);
154  goto failed;
155  }
156  }
157 
158  if (model == PIXART_3900) {
160  } else {
162  hal.scheduler->delay(100);
164  }
165 
166  hal.scheduler->delay(50);
167 
168  debug("Pixart %s ready\n", model==PIXART_3900?"3900":"3901");
169 
170  _dev->get_semaphore()->give();
171 
172  integral.last_frame_us = AP_HAL::micros();
173 
175  return true;
176 
177 failed:
178  _dev->get_semaphore()->give();
179  return false;
180 }
181 
182 
183 // write an 8 bit register
184 void AP_OpticalFlow_Pixart::reg_write(uint8_t reg, uint8_t value)
185 {
186  _dev->set_chip_select(true);
187  reg |= PIXART_WRITE_FLAG;
188  _dev->transfer(&reg, 1, nullptr, 0);
190  _dev->transfer(&value, 1, nullptr, 0);
191  _dev->set_chip_select(false);
192  hal.scheduler->delay_microseconds(120);
193 }
194 
195 // read from an 8 bit register
197 {
198  uint8_t v = 0;
199  _dev->set_chip_select(true);
200  _dev->transfer(&reg, 1, nullptr, 0);
201  hal.scheduler->delay_microseconds(35);
202  _dev->transfer(nullptr, 0, &v, 1);
203  _dev->set_chip_select(false);
204  hal.scheduler->delay_microseconds(200);
205  return v;
206 }
207 
208 // read from a 16 bit unsigned register
210 {
211  uint16_t low = reg_read(reg);
212  uint16_t high = reg_read(reg+1);
213  return low | (high<<8);
214 }
215 
216 // read from a 16 bit signed register
218 {
219  return (int16_t)reg_read16u(reg);
220 }
221 
222 
224 {
225  reg_write(0x39, 0x02);
226  hal.scheduler->delay(1);
228  hal.scheduler->delay(10);
230 
231  if (!_dev->set_chip_select(true)) {
232  debug("Failed to force CS\n");
233  }
236  _dev->transfer(&reg, 1, nullptr, 0);
237 
238  for (uint16_t i = 0; i < ARRAY_SIZE(srom_data); i++) {
239  hal.scheduler->delay_microseconds(15);
240  _dev->transfer(&srom_data[i], 1, nullptr, 0);
241  }
242 
243  hal.scheduler->delay_microseconds(125);
244  if (!_dev->set_chip_select(false)) {
245  debug("Failed to force CS off\n");
246  }
247  hal.scheduler->delay_microseconds(160);
248 }
249 
250 void AP_OpticalFlow_Pixart::load_configuration(const RegData *init_data, uint16_t n)
251 {
252  for (uint16_t i = 0; i < n; i++) {
253  // writing a config register can fail - retry up to 5 times
254  for (uint8_t tries=0; tries<5; tries++) {
255  reg_write(init_data[i].reg, init_data[i].value);
256  uint8_t v = reg_read(init_data[i].reg);
257  if (v == init_data[i].value) {
258  break;
259  }
260  //debug("reg[%u:%02x] 0x%02x 0x%02x\n", (unsigned)i, (unsigned)init_data[i].reg, (unsigned)init_data[i].value, (unsigned)v);
261  }
262  }
263 }
264 
265 
267 {
268  uint8_t *b = (uint8_t *)&burst;
269 
270  burst.delta_x = 0;
271  burst.delta_y = 0;
272 
273  _dev->set_chip_select(true);
275 
276  _dev->transfer(&reg, 1, nullptr, 0);
277  hal.scheduler->delay_microseconds(150);
278 
279  for (uint8_t i=0; i<sizeof(burst); i++) {
280  _dev->transfer(nullptr, 0, &b[i], 1);
281  if (i == 0 && (burst.motion & 0x80) == 0) {
282  // no motion, save some bus bandwidth
283  _dev->set_chip_select(false);
284  return;
285  }
286  }
287  _dev->set_chip_select(false);
288 }
289 
291 {
292  if (AP_HAL::micros() - last_burst_us < 500) {
293  return;
294  }
295  motion_burst();
297 
298  uint32_t dt_us = last_burst_us - integral.last_frame_us;
299  float dt = dt_us * 1.0e-6;
300  const Vector3f &gyro = get_ahrs().get_gyro();
301 
303  integral.sum.x += burst.delta_x;
304  integral.sum.y += burst.delta_y;
305  integral.sum_us += dt_us;
306  integral.last_frame_us = last_burst_us;
307  integral.gyro += Vector2f(gyro.x, gyro.y) * dt;
308  _sem->give();
309  }
310 
311 #if 0
312  static uint32_t last_print_ms;
313  static int fd = -1;
314  if (fd == -1) {
315  fd = open("/dev/ttyACM0", O_WRONLY);
316  }
317  // used for debugging
318  static int32_t sum_x;
319  static int32_t sum_y;
320  sum_x += burst.delta_x;
321  sum_y += burst.delta_y;
322 
323  uint32_t now = AP_HAL::millis();
324  if (now - last_print_ms >= 100 && (sum_x != 0 || sum_y != 0)) {
325  last_print_ms = now;
326  dprintf(fd, "Motion: %d %d obs:0x%02x squal:%u rds:%u maxr:%u minr:%u sup:%u slow:%u\n",
327  (int)sum_x, (int)sum_y, (unsigned)burst.squal, (unsigned)burst.rawdata_sum, (unsigned)burst.max_raw,
328  (unsigned)burst.max_raw, (unsigned)burst.min_raw, (unsigned)burst.shutter_upper, (unsigned)burst.shutter_lower);
329  sum_x = sum_y = 0;
330  }
331 #endif
332 }
333 
334 // update - read latest values from sensor and fill in x,y and totals.
336 {
337  uint32_t now = AP_HAL::millis();
338  if (now - last_update_ms < 100) {
339  return;
340  }
341  last_update_ms = now;
342 
343  struct OpticalFlow::OpticalFlow_state state;
344  state.device_id = 1;
345  state.surface_quality = burst.squal;
346 
347  if (integral.sum_us > 0 && _sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
348  const Vector2f flowScaler = _flowScaler();
349  float flowScaleFactorX = 1.0f + 0.001f * flowScaler.x;
350  float flowScaleFactorY = 1.0f + 0.001f * flowScaler.y;
351  float dt = integral.sum_us * 1.0e-6;
352 
353  state.flowRate = Vector2f(integral.sum.x * flowScaleFactorX,
354  integral.sum.y * flowScaleFactorY);
355  state.flowRate *= flow_pixel_scaling / dt;
356 
357  // we only apply yaw to flowRate as body rate comes from AHRS
358  _applyYaw(state.flowRate);
359 
360  state.bodyRate = integral.gyro / dt;
361 
362  integral.sum.zero();
363  integral.sum_us = 0;
364  integral.gyro.zero();
365  _sem->give();
366  } else {
367  state.flowRate.zero();
368  state.bodyRate.zero();
369  }
370 
371  // copy results to front end
372  _update_frontend(state);
373 }
enum AP_OpticalFlow_Pixart::@159 model
Vector2< float > Vector2f
Definition: vector2.h:239
#define PIXART_REG_MOT_BURST
void _applyYaw(Vector2f &v)
#define debug(fmt, args ...)
AP_AHRS_NavEKF & get_ahrs(void)
const Vector3f & get_gyro(void) const override
#define PIXART_REG_INV_PROD_ID2
int open(const char *pathname, int flags)
POSIX Open a file with integer mode flags.
Definition: posix.c:885
#define PIXART_REG_POWER_RST
AP_OpticalFlow_Pixart(const char *devname, OpticalFlow &_frontend)
constructor
Vector2f _flowScaler(void) const
#define PIXART_REG_SROM_BURST
virtual Device::PeriodicHandle register_periodic_callback(uint32_t period_usec, Device::PeriodicCb) override=0
void load_configuration(const RegData *init_data, uint16_t n)
#define HAL_SEMAPHORE_BLOCK_FOREVER
Definition: Semaphores.h:5
struct PACKED AP_OpticalFlow_Pixart::MotionBurst burst
virtual bool take(uint32_t timeout_ms) WARN_IF_UNUSED=0
#define PIXART_REG_DOUT_L
#define PIXART_REG_PRODUCT_ID
uint16_t reg_read16u(uint8_t reg)
#define PIXART_REG_SROM_ID
void reg_write(uint8_t reg, uint8_t value)
virtual void delay(uint16_t ms)=0
T y
Definition: vector2.h:37
#define PIXART_REG_INV_PROD_ID
virtual OwnPtr< SPIDevice > get_device(const char *name)
Definition: SPIDevice.h:68
static const RegData init_data_3901_1[]
virtual Semaphore * get_semaphore() override=0
void _update_frontend(const struct OpticalFlow::OpticalFlow_state &state)
#define PIXART_Tsrad
int16_t reg_read16s(uint8_t reg)
T y
Definition: vector3.h:67
#define PIXART_WRITE_FLAG
uint32_t millis()
Definition: system.cpp:157
virtual bool set_chip_select(bool set)
Definition: Device.h:214
virtual bool give()=0
static const RegData init_data_3900[]
float v
Definition: Printf.cpp:15
AP_HAL::SPIDeviceManager * spi
Definition: HAL.h:107
#define ARRAY_SIZE(_arr)
Definition: AP_Common.h:80
virtual bool transfer(const uint8_t *send, uint32_t send_len, uint8_t *recv, uint32_t recv_len) override=0
T x
Definition: vector2.h:37
static const RegData init_data_3901_2[]
void zero()
Definition: vector2.h:125
static AP_OpticalFlow_Pixart * detect(const char *devname, OpticalFlow &_frontend)
virtual void delay_microseconds(uint16_t us)=0
struct AP_OpticalFlow_Pixart::@160 integral
static const uint8_t srom_data[]
float value
#define PIXART_REG_MOT_BURST2
uint8_t reg_read(uint8_t reg)
#define FUNCTOR_BIND_MEMBER(func, rettype,...)
Definition: functor.h:31
AP_HAL::Semaphore * _sem
void panic(const char *errormsg,...) FMT_PRINTF(1
Definition: system.cpp:140
const AP_HAL::HAL & hal
-*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
Definition: AC_PID_test.cpp:14
uint32_t micros()
Definition: system.cpp:152
AP_HAL::OwnPtr< AP_HAL::SPIDevice > _dev
AP_HAL::Scheduler * scheduler
Definition: HAL.h:114
T x
Definition: vector3.h:67
#define PIXART_REG_SROM_EN
static const uint8_t srom_id