Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-21 07:13:56 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_OpticalFlow: support pixart 3901

Browse Source
This commit is contained in:
Andrew Tridgell 2017-03-10 10:45:01 +11:00 committed by Francisco Ferreira
parent 762d5a649d
commit 97e88501c6
3 changed files with 151 additions and 38 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

90
libraries/AP_OpticalFlow/AP_OpticalFlow_Pixart.cpp
View File

@ -29,6 +29,8 @@
#include "AP_OpticalFlow_Pixart_SROM.h" #include "AP_OpticalFlow_Pixart_SROM.h"
#include <stdio.h> #include <stdio.h>
#define debug(fmt, args ...) do {printf(fmt, ## args); } while(0)
extern const AP_HAL::HAL& hal; extern const AP_HAL::HAL& hal;
#define PIXART_REG_PRODUCT_ID 0x00 #define PIXART_REG_PRODUCT_ID 0x00
@ -62,7 +64,9 @@ extern const AP_HAL::HAL& hal;
#define PIXART_REG_POWER_RST 0x3A #define PIXART_REG_POWER_RST 0x3A
#define PIXART_REG_SHUTDOWN 0x3B #define PIXART_REG_SHUTDOWN 0x3B
#define PIXART_REG_INV_PROD_ID 0x3F #define PIXART_REG_INV_PROD_ID 0x3F
#define PIXART_REG_INV_PROD_ID2 0x5F // for 3901
#define PIXART_REG_MOT_BURST 0x50 #define PIXART_REG_MOT_BURST 0x50
#define PIXART_REG_MOT_BURST2 0x16
#define PIXART_REG_SROM_BURST 0x62 #define PIXART_REG_SROM_BURST 0x62
#define PIXART_REG_RAW_BURST 0x64 #define PIXART_REG_RAW_BURST 0x64
@ -70,7 +74,7 @@ extern const AP_HAL::HAL& hal;
#define PIXART_WRITE_FLAG 0x80 #define PIXART_WRITE_FLAG 0x80
// timings in microseconds // timings in microseconds
#define PIXART_Tsrad 150 #define PIXART_Tsrad 300
// correct result for SROM CRC // correct result for SROM CRC
#define PIXART_SROM_CRC_RESULT 0xBEEF #define PIXART_SROM_CRC_RESULT 0xBEEF
@ -112,39 +116,53 @@ bool AP_OpticalFlow_Pixart::setup_sensor(void)
hal.scheduler->delay(50); hal.scheduler->delay(50);
// check product ID // check product ID
if (reg_read(PIXART_REG_PRODUCT_ID) != 0x3F || uint8_t id1 = reg_read(PIXART_REG_PRODUCT_ID);
reg_read(PIXART_REG_INV_PROD_ID) != 0xC0) { uint8_t id2;
goto failed; if (id1 == 0x3f) {
id2 = reg_read(PIXART_REG_INV_PROD_ID);
} else {
id2 = reg_read(PIXART_REG_INV_PROD_ID2);
} }
debug("id1=0x%02x id2=0x%02x ~id1=0x%02x\n", id1, id2, uint8_t(~id1));
srom_download(); if (id1 == 0x3F && id2 == uint8_t(~id1)) {
model = PIXART_3900;
id = reg_read(PIXART_REG_SROM_ID); } else if (id1 == 0x49 && id2 == uint8_t(~id1)) {
if (id != srom_id) { model = PIXART_3901;
printf("Pixart: bad SROM ID: 0x%02x\n", id); } else {
debug("Not a recognised device\n");
goto failed; goto failed;
} }
reg_write(PIXART_REG_SROM_EN, 0x15); if (model == PIXART_3900) {
hal.scheduler->delay(10); srom_download();
id = reg_read(PIXART_REG_SROM_ID);
if (id != srom_id) {
debug("Pixart: bad SROM ID: 0x%02x\n", id);
goto failed;
}
reg_write(PIXART_REG_SROM_EN, 0x15);
hal.scheduler->delay(10);
crc = reg_read16u(PIXART_REG_DOUT_L); crc = reg_read16u(PIXART_REG_DOUT_L);
if (crc != 0xBEEF) { if (crc != 0xBEEF) {
printf("Pixart: bad SROM CRC: 0x%04x\n", crc); debug("Pixart: bad SROM CRC: 0x%04x\n", crc);
goto failed; goto failed;
}
}
if (model == PIXART_3900) {
load_configuration(init_data_3900, ARRAY_SIZE(init_data_3900));
} else {
load_configuration(init_data_3901_1, ARRAY_SIZE(init_data_3901_1));
hal.scheduler->delay(100);
load_configuration(init_data_3901_2, ARRAY_SIZE(init_data_3901_2));
} }
load_configuration();
hal.scheduler->delay(50); hal.scheduler->delay(50);
printf("Pixart ready: prod:0x%02x rev:0x%02x iprod:0x%02x shi:0x%02x cfg1:0x%02x cfg2:0x%02x\n", debug("Pixart %s ready\n", model==PIXART_3900?"3900":"3901");
(unsigned)reg_read(PIXART_REG_PRODUCT_ID),
(unsigned)reg_read(PIXART_REG_REVISION_ID),
(unsigned)reg_read(PIXART_REG_INV_PROD_ID),
(unsigned)reg_read(PIXART_REG_SHUTTER_HI),
(unsigned)reg_read(PIXART_REG_CONFIG1),
(unsigned)reg_read(PIXART_REG_CONFIG2));
_dev->get_semaphore()->give(); _dev->get_semaphore()->give();
@ -177,10 +195,10 @@ uint8_t AP_OpticalFlow_Pixart::reg_read(uint8_t reg)
uint8_t v = 0; uint8_t v = 0;
_dev->set_chip_select(true); _dev->set_chip_select(true);
_dev->transfer(&reg, 1, nullptr, 0); _dev->transfer(&reg, 1, nullptr, 0);
hal.scheduler->delay_microseconds(PIXART_Tsrad); hal.scheduler->delay_microseconds(35);
_dev->transfer(nullptr, 0, &v, 1); _dev->transfer(nullptr, 0, &v, 1);
_dev->set_chip_select(false); _dev->set_chip_select(false);
hal.scheduler->delay_microseconds(120); hal.scheduler->delay_microseconds(200);
return v; return v;
} }
@ -208,7 +226,7 @@ void AP_OpticalFlow_Pixart::srom_download(void)
reg_write(PIXART_REG_SROM_EN, 0x18); reg_write(PIXART_REG_SROM_EN, 0x18);
if (!_dev->set_chip_select(true)) { if (!_dev->set_chip_select(true)) {
printf("Failed to force CS\n"); debug("Failed to force CS\n");
} }
hal.scheduler->delay_microseconds(1); hal.scheduler->delay_microseconds(1);
uint8_t reg = PIXART_REG_SROM_BURST | PIXART_WRITE_FLAG; uint8_t reg = PIXART_REG_SROM_BURST | PIXART_WRITE_FLAG;
@ -221,14 +239,14 @@ void AP_OpticalFlow_Pixart::srom_download(void)
hal.scheduler->delay_microseconds(125); hal.scheduler->delay_microseconds(125);
if (!_dev->set_chip_select(false)) { if (!_dev->set_chip_select(false)) {
printf("Failed to force CS off\n"); debug("Failed to force CS off\n");
} }
hal.scheduler->delay_microseconds(160); hal.scheduler->delay_microseconds(160);
} }
void AP_OpticalFlow_Pixart::load_configuration(void) void AP_OpticalFlow_Pixart::load_configuration(const RegData *init_data, uint16_t n)
{ {
for (uint16_t i = 0; i < ARRAY_SIZE(init_data); i++) { for (uint16_t i = 0; i < n; i++) {
// writing a config register can fail - retry up to 5 times // writing a config register can fail - retry up to 5 times
for (uint8_t tries=0; tries<5; tries++) { for (uint8_t tries=0; tries<5; tries++) {
reg_write(init_data[i].reg, init_data[i].value); reg_write(init_data[i].reg, init_data[i].value);
@ -236,7 +254,7 @@ void AP_OpticalFlow_Pixart::load_configuration(void)
if (v == init_data[i].value) { if (v == init_data[i].value) {
break; break;
} }
//printf("reg[%u:%02x] 0x%02x 0x%02x\n", (unsigned)i, (unsigned)init_data[i].reg, (unsigned)init_data[i].value, (unsigned)v); //debug("reg[%u:%02x] 0x%02x 0x%02x\n", (unsigned)i, (unsigned)init_data[i].reg, (unsigned)init_data[i].value, (unsigned)v);
} }
} }
} }
@ -250,7 +268,7 @@ void AP_OpticalFlow_Pixart::motion_burst(void)
burst.delta_y = 0; burst.delta_y = 0;
_dev->set_chip_select(true); _dev->set_chip_select(true);
uint8_t reg = PIXART_REG_MOT_BURST; uint8_t reg = model==PIXART_3900?PIXART_REG_MOT_BURST:PIXART_REG_MOT_BURST2;
_dev->transfer(&reg, 1, nullptr, 0); _dev->transfer(&reg, 1, nullptr, 0);
hal.scheduler->delay_microseconds(150); hal.scheduler->delay_microseconds(150);
@ -288,6 +306,10 @@ void AP_OpticalFlow_Pixart::timer(void)
} }
#if 0 #if 0
static int fd = -1;
if (fd == -1) {
fd = open("/dev/ttyACM0", O_WRONLY);
}
// used for debugging // used for debugging
sum_x += burst.delta_x; sum_x += burst.delta_x;
sum_y += burst.delta_y; sum_y += burst.delta_y;
@ -295,7 +317,7 @@ void AP_OpticalFlow_Pixart::timer(void)
uint32_t now = AP_HAL::millis(); uint32_t now = AP_HAL::millis();
if (now - last_print_ms >= 100 && (sum_x != 0 || sum_y != 0)) { if (now - last_print_ms >= 100 && (sum_x != 0 || sum_y != 0)) {
last_print_ms = now; last_print_ms = now;
printf("Motion: %d %d obs:0x%02x squal:%u rds:%u maxr:%u minr:%u sup:%u slow:%u\n", dprintf(fd, "Motion: %d %d obs:0x%02x squal:%u rds:%u maxr:%u minr:%u sup:%u slow:%u\n",
(int)sum_x, (int)sum_y, (unsigned)burst.squal, (unsigned)burst.rawdata_sum, (unsigned)burst.max_raw, (int)sum_x, (int)sum_y, (unsigned)burst.squal, (unsigned)burst.rawdata_sum, (unsigned)burst.max_raw,
(unsigned)burst.max_raw, (unsigned)burst.min_raw, (unsigned)burst.shutter_upper, (unsigned)burst.shutter_lower); (unsigned)burst.max_raw, (unsigned)burst.min_raw, (unsigned)burst.shutter_upper, (unsigned)burst.shutter_lower);
sum_x = sum_y = 0; sum_x = sum_y = 0;

11
libraries/AP_OpticalFlow/AP_OpticalFlow_Pixart.h
View File

@ -21,6 +21,11 @@ public:
private: private:
AP_HAL::OwnPtr<AP_HAL::SPIDevice> _dev; AP_HAL::OwnPtr<AP_HAL::SPIDevice> _dev;
enum {
PIXART_3900=0,
PIXART_3901=1
} model;
struct RegData { struct RegData {
uint8_t reg; uint8_t reg;
uint8_t value; uint8_t value;
@ -48,7 +53,9 @@ private:
static const uint8_t srom_data[]; static const uint8_t srom_data[];
static const uint8_t srom_id; static const uint8_t srom_id;
static const RegData init_data[]; static const RegData init_data_3900[];
static const RegData init_data_3901_1[];
static const RegData init_data_3901_2[];
const float flow_pixel_scaling = 1.26e-3; const float flow_pixel_scaling = 1.26e-3;
// setup sensor // setup sensor
@ -60,7 +67,7 @@ private:
uint16_t reg_read16u(uint8_t reg); uint16_t reg_read16u(uint8_t reg);
void srom_download(void); void srom_download(void);
void load_configuration(void); void load_configuration(const RegData *init_data, uint16_t n);
void timer(void); void timer(void);
void motion_burst(void); void motion_burst(void);

88
libraries/AP_OpticalFlow/AP_OpticalFlow_Pixart_SROM.h
View File

@ -4,6 +4,7 @@
const uint8_t AP_OpticalFlow_Pixart::srom_id = 0xED; const uint8_t AP_OpticalFlow_Pixart::srom_id = 0xED;
// SROM data for 3900
const uint8_t AP_OpticalFlow_Pixart::srom_data[] = const uint8_t AP_OpticalFlow_Pixart::srom_data[] =
{ {
0x03, 0xed, 0xb5, 0x32, 0x26, 0xfc, 0x1e, 0xbe, 0xd8, 0x1d, 0xb8, 0xd3, 0x05, 0x88, 0x73, 0x45, 0x03, 0xed, 0xb5, 0x32, 0x26, 0xfc, 0x1e, 0xbe, 0xd8, 0x1d, 0xb8, 0xd3, 0x05, 0x88, 0x73, 0x45,
@ -202,9 +203,9 @@ const uint8_t AP_OpticalFlow_Pixart::srom_data[] =
/* /*
initialisation data (register config) initialisation data (register config) for 3900
*/ */
const AP_OpticalFlow_Pixart::RegData AP_OpticalFlow_Pixart::init_data[] = const AP_OpticalFlow_Pixart::RegData AP_OpticalFlow_Pixart::init_data_3900[] =
{ {
{ 0x30, 0x44 }, { 0x30, 0x44 },
@ -278,3 +279,86 @@ const AP_OpticalFlow_Pixart::RegData AP_OpticalFlow_Pixart::init_data[] =
{ 0x52, 0x10 }, { 0x52, 0x10 },
}; };
// init data for 3901
const AP_OpticalFlow_Pixart::RegData AP_OpticalFlow_Pixart::init_data_3901_1[] =
{
{ 0x7F, 0x00 },
{ 0x61, 0xAD },
{ 0x7F, 0x03 },
{ 0x40, 0x00 },
{ 0x7F, 0x05 },
{ 0x41, 0xB3 },
{ 0x43, 0xF1 },
{ 0x45, 0x14 },
{ 0x5B, 0x32 },
{ 0x5F, 0x34 },
{ 0x7B, 0x08 },
{ 0x7F, 0x06 },
{ 0x44, 0x1B },
{ 0x40, 0xBF },
{ 0x4E, 0x3F },
{ 0x7F, 0x08 },
{ 0x65, 0x20 },
{ 0x6A, 0x18 },
{ 0x7F, 0x09 },
{ 0x4F, 0xAF },
{ 0x5F, 0x40 },
{ 0x48, 0x80 },
{ 0x49, 0x80 },
{ 0x57, 0x77 },
{ 0x60, 0x78 },
{ 0x61, 0x78 },
{ 0x62, 0x08 },
{ 0x63, 0x50 },
{ 0x7F, 0x0A },
{ 0x45, 0x60 },
{ 0x7F, 0x00 },
{ 0x4D, 0x11 },
{ 0x55, 0x80 },
{ 0x74, 0x1F },
{ 0x75, 0x1F },
{ 0x4A, 0x78 },
{ 0x4B, 0x78 },
{ 0x44, 0x08 },
{ 0x45, 0x50 },
{ 0x64, 0xFF },
{ 0x65, 0x1F },
{ 0x7F, 0x14 },
{ 0x65, 0x60 },
{ 0x66, 0x08 },
{ 0x63, 0x78 },
{ 0x7F, 0x15 },
{ 0x48, 0x58 },
{ 0x7F, 0x07 },
{ 0x41, 0x0D },
{ 0x43, 0x14 },
{ 0x4B, 0x0E },
{ 0x45, 0x0F },
{ 0x44, 0x42 },
{ 0x4C, 0x80 },
{ 0x7F, 0x10 },
{ 0x5B, 0x02 },
{ 0x7F, 0x07 },
{ 0x40, 0x41 },
{ 0x70, 0x00 }
};
// Delay 100 ms before resuming the below register writes
const AP_OpticalFlow_Pixart::RegData AP_OpticalFlow_Pixart::init_data_3901_2[] =
{
{ 0x32, 0x44 },
{ 0x7F, 0x07 },
{ 0x40, 0x40 },
{ 0x7F, 0x06 },
{ 0x62, 0xF0 },
{ 0x63, 0x00 },
{ 0x7F, 0x0D },
{ 0x48, 0xC0 },
{ 0x6F, 0xD5 },
{ 0x7F, 0x00 },
{ 0x5B, 0xA0 },
{ 0x4E, 0xA8 },
{ 0x5A, 0x50 },
{ 0x40, 0x80 },
};