mirror of https://github.com/ArduPilot/ardupilot
363 lines
10 KiB
C++
363 lines
10 KiB
C++
/*
|
|
This program is free software: you can redistribute it and/or modify
|
|
it under the terms of the GNU General Public License as published by
|
|
the Free Software Foundation, either version 3 of the License, or
|
|
(at your option) any later version.
|
|
|
|
This program is distributed in the hope that it will be useful,
|
|
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
GNU General Public License for more details.
|
|
|
|
You should have received a copy of the GNU General Public License
|
|
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
|
*/
|
|
/*
|
|
driver for Pixart PMW3900DH optical flow sensor
|
|
|
|
|
|
NOTE: This sensor does not use traditional SPI register access. The
|
|
timing for register reads and writes is critical
|
|
*/
|
|
|
|
#include <AP_HAL/AP_HAL.h>
|
|
#include <AP_Math/crc.h>
|
|
#include <AP_AHRS/AP_AHRS.h>
|
|
#include <utility>
|
|
#include "OpticalFlow.h"
|
|
#include "AP_OpticalFlow_Pixart.h"
|
|
#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
|
|
#define PIXART_REG_REVISION_ID 0x01
|
|
#define PIXART_REG_MOTION 0x02
|
|
#define PIXART_REG_DELTA_X_L 0x03
|
|
#define PIXART_REG_DELTA_X_H 0x04
|
|
#define PIXART_REG_DELTA_Y_L 0x05
|
|
#define PIXART_REG_DELTA_Y_H 0x06
|
|
#define PIXART_REG_SQUAL 0x07
|
|
#define PIXART_REG_RAWDATA_SUM 0x08
|
|
#define PIXART_REG_RAWDATA_MAX 0x09
|
|
#define PIXART_REG_RAWDATA_MIN 0x0A
|
|
#define PIXART_REG_SHUTTER_LOW 0x0B
|
|
#define PIXART_REG_SHUTTER_HI 0x0C
|
|
#define PIXART_REG_CONFIG1 0x0F
|
|
#define PIXART_REG_CONFIG2 0x10
|
|
#define PIXART_REG_FRAME_CAP 0x12
|
|
#define PIXART_REG_SROM_EN 0x13
|
|
#define PIXART_REG_RUN_DS 0x14
|
|
#define PIXART_REG_REST1_RATE 0x15
|
|
#define PIXART_REG_REST1_DS 0x16
|
|
#define PIXART_REG_REST2_RATE 0x17
|
|
#define PIXART_REG_REST2_DS 0x18
|
|
#define PIXART_REG_REST3_RATE 0x19
|
|
#define PIXART_REG_OBS 0x24
|
|
#define PIXART_REG_DOUT_L 0x25
|
|
#define PIXART_REG_DOUT_H 0x26
|
|
#define PIXART_REG_RAW_GRAB 0x29
|
|
#define PIXART_REG_SROM_ID 0x2A
|
|
#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
|
|
|
|
// writing to registers needs this flag
|
|
#define PIXART_WRITE_FLAG 0x80
|
|
|
|
// timings in microseconds
|
|
#define PIXART_Tsrad 300
|
|
|
|
// correct result for SROM CRC
|
|
#define PIXART_SROM_CRC_RESULT 0xBEEF
|
|
|
|
// constructor
|
|
AP_OpticalFlow_Pixart::AP_OpticalFlow_Pixart(const char *devname, OpticalFlow &_frontend) :
|
|
OpticalFlow_backend(_frontend)
|
|
{
|
|
_dev = std::move(hal.spi->get_device(devname));
|
|
}
|
|
|
|
// detect the device
|
|
AP_OpticalFlow_Pixart *AP_OpticalFlow_Pixart::detect(const char *devname, OpticalFlow &_frontend)
|
|
{
|
|
AP_OpticalFlow_Pixart *sensor = new AP_OpticalFlow_Pixart(devname, _frontend);
|
|
if (!sensor) {
|
|
return nullptr;
|
|
}
|
|
if (!sensor->setup_sensor()) {
|
|
delete sensor;
|
|
return nullptr;
|
|
}
|
|
return sensor;
|
|
}
|
|
|
|
// setup the device
|
|
bool AP_OpticalFlow_Pixart::setup_sensor(void)
|
|
{
|
|
if (!_dev) {
|
|
return false;
|
|
}
|
|
WITH_SEMAPHORE(_dev->get_semaphore());
|
|
|
|
uint8_t id;
|
|
uint16_t crc;
|
|
|
|
// power-up sequence
|
|
reg_write(PIXART_REG_POWER_RST, 0x5A);
|
|
hal.scheduler->delay(50);
|
|
|
|
// check product ID
|
|
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);
|
|
}
|
|
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");
|
|
return false;
|
|
}
|
|
|
|
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);
|
|
return false;
|
|
}
|
|
|
|
reg_write(PIXART_REG_SROM_EN, 0x15);
|
|
hal.scheduler->delay(10);
|
|
|
|
crc = reg_read16u(PIXART_REG_DOUT_L);
|
|
if (crc != 0xBEEF) {
|
|
debug("Pixart: bad SROM CRC: 0x%04x\n", crc);
|
|
return false;
|
|
}
|
|
}
|
|
|
|
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));
|
|
}
|
|
|
|
hal.scheduler->delay(50);
|
|
|
|
debug("Pixart %s ready\n", model==PIXART_3900?"3900":"3901");
|
|
|
|
integral.last_frame_us = AP_HAL::micros();
|
|
|
|
_dev->register_periodic_callback(2000, FUNCTOR_BIND_MEMBER(&AP_OpticalFlow_Pixart::timer, void));
|
|
return true;
|
|
}
|
|
|
|
// write an 8 bit register
|
|
void AP_OpticalFlow_Pixart::reg_write(uint8_t reg, uint8_t value)
|
|
{
|
|
_dev->set_chip_select(true);
|
|
reg |= PIXART_WRITE_FLAG;
|
|
_dev->transfer(®, 1, nullptr, 0);
|
|
hal.scheduler->delay_microseconds(PIXART_Tsrad);
|
|
_dev->transfer(&value, 1, nullptr, 0);
|
|
_dev->set_chip_select(false);
|
|
hal.scheduler->delay_microseconds(120);
|
|
}
|
|
|
|
// read from an 8 bit register
|
|
uint8_t AP_OpticalFlow_Pixart::reg_read(uint8_t reg)
|
|
{
|
|
uint8_t v = 0;
|
|
_dev->set_chip_select(true);
|
|
_dev->transfer(®, 1, nullptr, 0);
|
|
hal.scheduler->delay_microseconds(35);
|
|
_dev->transfer(nullptr, 0, &v, 1);
|
|
_dev->set_chip_select(false);
|
|
hal.scheduler->delay_microseconds(200);
|
|
return v;
|
|
}
|
|
|
|
// read from a 16 bit unsigned register
|
|
uint16_t AP_OpticalFlow_Pixart::reg_read16u(uint8_t reg)
|
|
{
|
|
uint16_t low = reg_read(reg);
|
|
uint16_t high = reg_read(reg+1);
|
|
return low | (high<<8);
|
|
}
|
|
|
|
// read from a 16 bit signed register
|
|
int16_t AP_OpticalFlow_Pixart::reg_read16s(uint8_t reg)
|
|
{
|
|
return (int16_t)reg_read16u(reg);
|
|
}
|
|
|
|
void AP_OpticalFlow_Pixart::srom_download(void)
|
|
{
|
|
reg_write(0x39, 0x02);
|
|
hal.scheduler->delay(1);
|
|
reg_write(PIXART_REG_SROM_EN, 0x1D);
|
|
hal.scheduler->delay(10);
|
|
reg_write(PIXART_REG_SROM_EN, 0x18);
|
|
|
|
if (!_dev->set_chip_select(true)) {
|
|
debug("Failed to force CS\n");
|
|
}
|
|
hal.scheduler->delay_microseconds(1);
|
|
uint8_t reg = PIXART_REG_SROM_BURST | PIXART_WRITE_FLAG;
|
|
_dev->transfer(®, 1, nullptr, 0);
|
|
|
|
for (uint16_t i = 0; i < ARRAY_SIZE(srom_data); i++) {
|
|
hal.scheduler->delay_microseconds(15);
|
|
_dev->transfer(&srom_data[i], 1, nullptr, 0);
|
|
}
|
|
|
|
hal.scheduler->delay_microseconds(125);
|
|
if (!_dev->set_chip_select(false)) {
|
|
debug("Failed to force CS off\n");
|
|
}
|
|
hal.scheduler->delay_microseconds(160);
|
|
}
|
|
|
|
void AP_OpticalFlow_Pixart::load_configuration(const RegData *init_data, uint16_t n)
|
|
{
|
|
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);
|
|
uint8_t v = reg_read(init_data[i].reg);
|
|
if (v == init_data[i].value) {
|
|
break;
|
|
}
|
|
//debug("reg[%u:%02x] 0x%02x 0x%02x\n", (unsigned)i, (unsigned)init_data[i].reg, (unsigned)init_data[i].value, (unsigned)v);
|
|
}
|
|
}
|
|
}
|
|
|
|
void AP_OpticalFlow_Pixart::motion_burst(void)
|
|
{
|
|
uint8_t *b = (uint8_t *)&burst;
|
|
|
|
burst.delta_x = 0;
|
|
burst.delta_y = 0;
|
|
|
|
_dev->set_chip_select(true);
|
|
uint8_t reg = model==PIXART_3900?PIXART_REG_MOT_BURST:PIXART_REG_MOT_BURST2;
|
|
|
|
_dev->transfer(®, 1, nullptr, 0);
|
|
hal.scheduler->delay_microseconds(150);
|
|
|
|
for (uint8_t i=0; i<sizeof(burst); i++) {
|
|
_dev->transfer(nullptr, 0, &b[i], 1);
|
|
if (i == 0 && (burst.motion & 0x80) == 0) {
|
|
// no motion, save some bus bandwidth
|
|
_dev->set_chip_select(false);
|
|
return;
|
|
}
|
|
}
|
|
_dev->set_chip_select(false);
|
|
}
|
|
|
|
void AP_OpticalFlow_Pixart::timer(void)
|
|
{
|
|
if (AP_HAL::micros() - last_burst_us < 500) {
|
|
return;
|
|
}
|
|
motion_burst();
|
|
last_burst_us = AP_HAL::micros();
|
|
|
|
uint32_t dt_us = last_burst_us - integral.last_frame_us;
|
|
float dt = dt_us * 1.0e-6;
|
|
const Vector3f &gyro = AP::ahrs_navekf().get_gyro();
|
|
|
|
{
|
|
WITH_SEMAPHORE(_sem);
|
|
|
|
integral.sum.x += burst.delta_x;
|
|
integral.sum.y += burst.delta_y;
|
|
integral.sum_us += dt_us;
|
|
integral.last_frame_us = last_burst_us;
|
|
integral.gyro += Vector2f(gyro.x, gyro.y) * dt;
|
|
}
|
|
|
|
#if 0
|
|
static uint32_t last_print_ms;
|
|
static int fd = -1;
|
|
if (fd == -1) {
|
|
fd = open("/dev/ttyACM0", O_WRONLY);
|
|
}
|
|
// used for debugging
|
|
static int32_t sum_x;
|
|
static int32_t sum_y;
|
|
sum_x += burst.delta_x;
|
|
sum_y += burst.delta_y;
|
|
|
|
uint32_t now = AP_HAL::millis();
|
|
if (now - last_print_ms >= 100 && (sum_x != 0 || sum_y != 0)) {
|
|
last_print_ms = now;
|
|
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;
|
|
}
|
|
#endif
|
|
}
|
|
|
|
// update - read latest values from sensor and fill in x,y and totals.
|
|
void AP_OpticalFlow_Pixart::update(void)
|
|
{
|
|
uint32_t now = AP_HAL::millis();
|
|
if (now - last_update_ms < 100) {
|
|
return;
|
|
}
|
|
last_update_ms = now;
|
|
|
|
struct OpticalFlow::OpticalFlow_state state;
|
|
state.surface_quality = burst.squal;
|
|
|
|
if (integral.sum_us > 0) {
|
|
WITH_SEMAPHORE(_sem);
|
|
|
|
const Vector2f flowScaler = _flowScaler();
|
|
float flowScaleFactorX = 1.0f + 0.001f * flowScaler.x;
|
|
float flowScaleFactorY = 1.0f + 0.001f * flowScaler.y;
|
|
float dt = integral.sum_us * 1.0e-6;
|
|
|
|
state.flowRate = Vector2f(integral.sum.x * flowScaleFactorX,
|
|
integral.sum.y * flowScaleFactorY);
|
|
state.flowRate *= flow_pixel_scaling / dt;
|
|
|
|
// we only apply yaw to flowRate as body rate comes from AHRS
|
|
_applyYaw(state.flowRate);
|
|
|
|
state.bodyRate = integral.gyro / dt;
|
|
|
|
integral.sum.zero();
|
|
integral.sum_us = 0;
|
|
integral.gyro.zero();
|
|
} else {
|
|
state.flowRate.zero();
|
|
state.bodyRate.zero();
|
|
}
|
|
|
|
// copy results to front end
|
|
_update_frontend(state);
|
|
}
|