Line data Source code
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_OpticalFlow_config.h"
24 :
25 : #if AP_OPTICALFLOW_PIXART_ENABLED
26 :
27 : #include "AP_OpticalFlow_Pixart.h"
28 :
29 : #include <AP_HAL/AP_HAL.h>
30 : #include <AP_Math/crc.h>
31 : #include <AP_AHRS/AP_AHRS.h>
32 : #include <utility>
33 : #include "AP_OpticalFlow.h"
34 : #include "AP_OpticalFlow_Pixart_SROM.h"
35 : #include <stdio.h>
36 :
37 : #define debug(fmt, args ...) do {printf(fmt, ## args); } while(0)
38 :
39 : extern const AP_HAL::HAL& hal;
40 :
41 : #define PIXART_REG_PRODUCT_ID 0x00
42 : #define PIXART_REG_REVISION_ID 0x01
43 : #define PIXART_REG_MOTION 0x02
44 : #define PIXART_REG_DELTA_X_L 0x03
45 : #define PIXART_REG_DELTA_X_H 0x04
46 : #define PIXART_REG_DELTA_Y_L 0x05
47 : #define PIXART_REG_DELTA_Y_H 0x06
48 : #define PIXART_REG_SQUAL 0x07
49 : #define PIXART_REG_RAWDATA_SUM 0x08
50 : #define PIXART_REG_RAWDATA_MAX 0x09
51 : #define PIXART_REG_RAWDATA_MIN 0x0A
52 : #define PIXART_REG_SHUTTER_LOW 0x0B
53 : #define PIXART_REG_SHUTTER_HI 0x0C
54 : #define PIXART_REG_CONFIG1 0x0F
55 : #define PIXART_REG_CONFIG2 0x10
56 : #define PIXART_REG_FRAME_CAP 0x12
57 : #define PIXART_REG_SROM_EN 0x13
58 : #define PIXART_REG_RUN_DS 0x14
59 : #define PIXART_REG_REST1_RATE 0x15
60 : #define PIXART_REG_REST1_DS 0x16
61 : #define PIXART_REG_REST2_RATE 0x17
62 : #define PIXART_REG_REST2_DS 0x18
63 : #define PIXART_REG_REST3_RATE 0x19
64 : #define PIXART_REG_OBS 0x24
65 : #define PIXART_REG_DOUT_L 0x25
66 : #define PIXART_REG_DOUT_H 0x26
67 : #define PIXART_REG_RAW_GRAB 0x29
68 : #define PIXART_REG_SROM_ID 0x2A
69 : #define PIXART_REG_POWER_RST 0x3A
70 : #define PIXART_REG_SHUTDOWN 0x3B
71 : #define PIXART_REG_INV_PROD_ID 0x3F
72 : #define PIXART_REG_INV_PROD_ID2 0x5F // for 3901
73 : #define PIXART_REG_MOT_BURST 0x50
74 : #define PIXART_REG_MOT_BURST2 0x16
75 : #define PIXART_REG_SROM_BURST 0x62
76 : #define PIXART_REG_RAW_BURST 0x64
77 :
78 : // writing to registers needs this flag
79 : #define PIXART_WRITE_FLAG 0x80
80 :
81 : // timings in microseconds
82 : #define PIXART_Tsrad 300
83 :
84 : // correct result for SROM CRC
85 : #define PIXART_SROM_CRC_RESULT 0xBEEF
86 :
87 : // constructor
88 0 : AP_OpticalFlow_Pixart::AP_OpticalFlow_Pixart(const char *devname, AP_OpticalFlow &_frontend) :
89 0 : OpticalFlow_backend(_frontend)
90 : {
91 0 : _dev = std::move(hal.spi->get_device(devname));
92 0 : }
93 :
94 : // detect the device
95 0 : AP_OpticalFlow_Pixart *AP_OpticalFlow_Pixart::detect(const char *devname, AP_OpticalFlow &_frontend)
96 : {
97 0 : AP_OpticalFlow_Pixart *sensor = NEW_NOTHROW AP_OpticalFlow_Pixart(devname, _frontend);
98 0 : if (!sensor) {
99 0 : return nullptr;
100 : }
101 0 : if (!sensor->setup_sensor()) {
102 0 : delete sensor;
103 0 : return nullptr;
104 : }
105 0 : return sensor;
106 : }
107 :
108 : // setup the device
109 0 : bool AP_OpticalFlow_Pixart::setup_sensor(void)
110 : {
111 0 : if (!_dev) {
112 0 : return false;
113 : }
114 0 : WITH_SEMAPHORE(_dev->get_semaphore());
115 :
116 : // power-up sequence
117 0 : reg_write(PIXART_REG_POWER_RST, 0x5A);
118 0 : hal.scheduler->delay(50);
119 :
120 : // check product ID
121 0 : uint8_t id1 = reg_read(PIXART_REG_PRODUCT_ID);
122 : uint8_t id2;
123 0 : if (id1 == 0x3f) {
124 0 : id2 = reg_read(PIXART_REG_INV_PROD_ID);
125 : } else {
126 0 : id2 = reg_read(PIXART_REG_INV_PROD_ID2);
127 : }
128 0 : debug("id1=0x%02x id2=0x%02x ~id1=0x%02x\n", id1, id2, uint8_t(~id1));
129 0 : if (id1 == 0x3F && id2 == uint8_t(~id1)) {
130 0 : model = PIXART_3900;
131 0 : } else if (id1 == 0x49 && id2 == uint8_t(~id1)) {
132 0 : model = PIXART_3901;
133 : } else {
134 0 : debug("Not a recognised device\n");
135 0 : return false;
136 : }
137 :
138 0 : if (model == PIXART_3900) {
139 0 : srom_download();
140 :
141 0 : const uint8_t id = reg_read(PIXART_REG_SROM_ID);
142 0 : if (id != srom_id) {
143 0 : debug("Pixart: bad SROM ID: 0x%02x\n", id);
144 0 : return false;
145 : }
146 :
147 0 : reg_write(PIXART_REG_SROM_EN, 0x15);
148 0 : hal.scheduler->delay(10);
149 :
150 0 : const uint16_t crc = reg_read16u(PIXART_REG_DOUT_L);
151 0 : if (crc != 0xBEEF) {
152 0 : debug("Pixart: bad SROM CRC: 0x%04x\n", crc);
153 0 : return false;
154 : }
155 : }
156 :
157 0 : if (model == PIXART_3900) {
158 0 : load_configuration(init_data_3900, ARRAY_SIZE(init_data_3900));
159 : } else {
160 0 : load_configuration(init_data_3901_1, ARRAY_SIZE(init_data_3901_1));
161 0 : hal.scheduler->delay(100);
162 0 : load_configuration(init_data_3901_2, ARRAY_SIZE(init_data_3901_2));
163 : }
164 :
165 0 : hal.scheduler->delay(50);
166 :
167 0 : debug("Pixart %s ready\n", model==PIXART_3900?"3900":"3901");
168 :
169 0 : integral.last_frame_us = AP_HAL::micros();
170 :
171 0 : _dev->register_periodic_callback(2000, FUNCTOR_BIND_MEMBER(&AP_OpticalFlow_Pixart::timer, void));
172 0 : return true;
173 0 : }
174 :
175 : // write an 8 bit register
176 0 : void AP_OpticalFlow_Pixart::reg_write(uint8_t reg, uint8_t value)
177 : {
178 0 : _dev->set_chip_select(true);
179 0 : reg |= PIXART_WRITE_FLAG;
180 0 : _dev->transfer(®, 1, nullptr, 0);
181 0 : hal.scheduler->delay_microseconds(PIXART_Tsrad);
182 0 : _dev->transfer(&value, 1, nullptr, 0);
183 0 : _dev->set_chip_select(false);
184 0 : hal.scheduler->delay_microseconds(120);
185 0 : }
186 :
187 : // read from an 8 bit register
188 0 : uint8_t AP_OpticalFlow_Pixart::reg_read(uint8_t reg)
189 : {
190 0 : uint8_t v = 0;
191 0 : _dev->set_chip_select(true);
192 0 : _dev->transfer(®, 1, nullptr, 0);
193 0 : hal.scheduler->delay_microseconds(35);
194 0 : _dev->transfer(nullptr, 0, &v, 1);
195 0 : _dev->set_chip_select(false);
196 0 : hal.scheduler->delay_microseconds(200);
197 0 : return v;
198 : }
199 :
200 : // read from a 16 bit unsigned register
201 0 : uint16_t AP_OpticalFlow_Pixart::reg_read16u(uint8_t reg)
202 : {
203 0 : uint16_t low = reg_read(reg);
204 0 : uint16_t high = reg_read(reg+1);
205 0 : return low | (high<<8);
206 : }
207 :
208 : // read from a 16 bit signed register
209 0 : int16_t AP_OpticalFlow_Pixart::reg_read16s(uint8_t reg)
210 : {
211 0 : return (int16_t)reg_read16u(reg);
212 : }
213 :
214 0 : void AP_OpticalFlow_Pixart::srom_download(void)
215 : {
216 0 : reg_write(0x39, 0x02);
217 0 : hal.scheduler->delay(1);
218 0 : reg_write(PIXART_REG_SROM_EN, 0x1D);
219 0 : hal.scheduler->delay(10);
220 0 : reg_write(PIXART_REG_SROM_EN, 0x18);
221 :
222 0 : if (!_dev->set_chip_select(true)) {
223 0 : debug("Failed to force CS\n");
224 : }
225 0 : hal.scheduler->delay_microseconds(1);
226 0 : uint8_t reg = PIXART_REG_SROM_BURST | PIXART_WRITE_FLAG;
227 0 : _dev->transfer(®, 1, nullptr, 0);
228 :
229 0 : for (uint16_t i = 0; i < ARRAY_SIZE(srom_data); i++) {
230 0 : hal.scheduler->delay_microseconds(15);
231 0 : _dev->transfer(&srom_data[i], 1, nullptr, 0);
232 : }
233 :
234 0 : hal.scheduler->delay_microseconds(125);
235 0 : if (!_dev->set_chip_select(false)) {
236 0 : debug("Failed to force CS off\n");
237 : }
238 0 : hal.scheduler->delay_microseconds(160);
239 0 : }
240 :
241 0 : void AP_OpticalFlow_Pixart::load_configuration(const RegData *init_data, uint16_t n)
242 : {
243 0 : for (uint16_t i = 0; i < n; i++) {
244 : // writing a config register can fail - retry up to 5 times
245 0 : for (uint8_t tries=0; tries<5; tries++) {
246 0 : reg_write(init_data[i].reg, init_data[i].value);
247 0 : uint8_t v = reg_read(init_data[i].reg);
248 0 : if (v == init_data[i].value) {
249 0 : break;
250 : }
251 : //debug("reg[%u:%02x] 0x%02x 0x%02x\n", (unsigned)i, (unsigned)init_data[i].reg, (unsigned)init_data[i].value, (unsigned)v);
252 : }
253 : }
254 0 : }
255 :
256 0 : void AP_OpticalFlow_Pixart::motion_burst(void)
257 : {
258 0 : uint8_t *b = (uint8_t *)&burst;
259 :
260 0 : burst.delta_x = 0;
261 0 : burst.delta_y = 0;
262 :
263 0 : _dev->set_chip_select(true);
264 0 : uint8_t reg = model==PIXART_3900?PIXART_REG_MOT_BURST:PIXART_REG_MOT_BURST2;
265 :
266 0 : _dev->transfer(®, 1, nullptr, 0);
267 0 : hal.scheduler->delay_microseconds(150);
268 :
269 0 : for (uint8_t i=0; i<sizeof(burst); i++) {
270 0 : _dev->transfer(nullptr, 0, &b[i], 1);
271 0 : if (i == 0 && (burst.motion & 0x80) == 0) {
272 : // no motion, save some bus bandwidth
273 0 : _dev->set_chip_select(false);
274 0 : return;
275 : }
276 : }
277 0 : _dev->set_chip_select(false);
278 : }
279 :
280 0 : void AP_OpticalFlow_Pixart::timer(void)
281 : {
282 0 : if (AP_HAL::micros() - last_burst_us < 500) {
283 0 : return;
284 : }
285 0 : motion_burst();
286 0 : last_burst_us = AP_HAL::micros();
287 :
288 0 : uint32_t dt_us = last_burst_us - integral.last_frame_us;
289 0 : float dt = dt_us * 1.0e-6;
290 : #if AP_AHRS_ENABLED
291 0 : const Vector3f &gyro = AP::ahrs().get_gyro();
292 : #else
293 : const Vector3f &gyro = AP::ins().get_gyro();
294 : #endif
295 :
296 : {
297 0 : WITH_SEMAPHORE(_sem);
298 :
299 0 : integral.sum.x += burst.delta_x;
300 0 : integral.sum.y += burst.delta_y;
301 0 : integral.sum_us += dt_us;
302 0 : integral.last_frame_us = last_burst_us;
303 0 : integral.gyro += Vector2f(gyro.x, gyro.y) * dt;
304 0 : }
305 :
306 : #if 0
307 : static uint32_t last_print_ms;
308 : static int fd = -1;
309 : if (fd == -1) {
310 : fd = open("/dev/ttyACM0", O_WRONLY);
311 : }
312 : // used for debugging
313 : static int32_t sum_x;
314 : static int32_t sum_y;
315 : sum_x += burst.delta_x;
316 : sum_y += burst.delta_y;
317 :
318 : uint32_t now = AP_HAL::millis();
319 : if (now - last_print_ms >= 100 && (sum_x != 0 || sum_y != 0)) {
320 : last_print_ms = now;
321 : dprintf(fd, "Motion: %d %d obs:0x%02x squal:%u rds:%u maxr:%u minr:%u sup:%u slow:%u\n",
322 : (int)sum_x, (int)sum_y, (unsigned)burst.squal, (unsigned)burst.rawdata_sum, (unsigned)burst.max_raw,
323 : (unsigned)burst.max_raw, (unsigned)burst.min_raw, (unsigned)burst.shutter_upper, (unsigned)burst.shutter_lower);
324 : sum_x = sum_y = 0;
325 : }
326 : #endif
327 : }
328 :
329 : // update - read latest values from sensor and fill in x,y and totals.
330 0 : void AP_OpticalFlow_Pixart::update(void)
331 : {
332 0 : uint32_t now = AP_HAL::millis();
333 0 : if (now - last_update_ms < 100) {
334 0 : return;
335 : }
336 0 : last_update_ms = now;
337 :
338 0 : struct AP_OpticalFlow::OpticalFlow_state state;
339 0 : state.surface_quality = burst.squal;
340 :
341 0 : if (integral.sum_us > 0) {
342 0 : WITH_SEMAPHORE(_sem);
343 :
344 0 : const Vector2f flowScaler = _flowScaler();
345 0 : float flowScaleFactorX = 1.0f + 0.001f * flowScaler.x;
346 0 : float flowScaleFactorY = 1.0f + 0.001f * flowScaler.y;
347 0 : float dt = integral.sum_us * 1.0e-6;
348 :
349 0 : state.flowRate = Vector2f(integral.sum.x * flowScaleFactorX,
350 0 : integral.sum.y * flowScaleFactorY);
351 0 : state.flowRate *= flow_pixel_scaling / dt;
352 :
353 : // we only apply yaw to flowRate as body rate comes from AHRS
354 0 : _applyYaw(state.flowRate);
355 :
356 0 : state.bodyRate = integral.gyro / dt;
357 :
358 0 : integral.sum.zero();
359 0 : integral.sum_us = 0;
360 0 : integral.gyro.zero();
361 0 : } else {
362 0 : state.flowRate.zero();
363 0 : state.bodyRate.zero();
364 : }
365 :
366 : // copy results to front end
367 0 : _update_frontend(state);
368 : }
369 :
370 : #endif // AP_OPTICALFLOW_PIXART_ENABLED
|
