zrzk
/
ardupilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

AP_OpticalFlow: support pixart 3901

mission-4.1.18
Andrew Tridgell 8 years ago committed by Francisco Ferreira
parent
762d5a649d
commit
97e88501c6
3 changed files with 151 additions and 38 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 90
      libraries/AP_OpticalFlow/AP_OpticalFlow_Pixart.cpp
  2. 11
      libraries/AP_OpticalFlow/AP_OpticalFlow_Pixart.h
  3. 88
      libraries/AP_OpticalFlow/AP_OpticalFlow_Pixart_SROM.h

90
libraries/AP_OpticalFlow/AP_OpticalFlow_Pixart.cpp
Unescape View File

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

11
libraries/AP_OpticalFlow/AP_OpticalFlow_Pixart.h
Unescape View File

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

88
libraries/AP_OpticalFlow/AP_OpticalFlow_Pixart_SROM.h
Unescape View File

@ -4,6 +4,7 @@ @@ -4,6 +4,7 @@
const uint8_t AP_OpticalFlow_Pixart::srom_id = 0xED;
// SROM data for 3900
const uint8_t AP_OpticalFlow_Pixart::srom_data[] =
{
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[] = @@ -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 },
@ -278,3 +279,86 @@ const AP_OpticalFlow_Pixart::RegData AP_OpticalFlow_Pixart::init_data[] = @@ -278,3 +279,86 @@ const AP_OpticalFlow_Pixart::RegData AP_OpticalFlow_Pixart::init_data[] =
{ 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 },
};

Write Preview
Loading…
Cancel
Save