yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-22 01:12:31 +00:00
Code Issues Packages Projects Releases Wiki Activity

pmw3901 driver: cleanup for pull request

Browse Source
This commit is contained in:
DanielePettenuzzo
2018-03-09 09:26:31 +01:00
committed by Beat Küng
parent ddf75db154
commit 6ca341f84e
1 changed files with 352 additions and 363 deletions
Download Patch File Download Diff File Expand all files Collapse all files

715
src/drivers/pmw3901/pmw3901.cpp
View File

@@ -58,6 +58,8 @@
#include <math.h>
#include <unistd.h>
#include <conversion/rotation.h>
#include <systemlib/perf_counter.h>
#include <systemlib/err.h>
@@ -68,8 +70,8 @@
#include <uORB/uORB.h>
#include <uORB/topics/subsystem_info.h>
#include <uORB/topics/distance_sensor.h>
#include <uORB/topics/optical_flow.h>
#include <uORB/topics/distance_sensor.h>
#include <board_config.h>
@@ -80,15 +82,17 @@
#define PMW3901_BUS PX4_SPI_BUS_EXTERNAL1
#endif
#define PMW3901_SPI_BUS_SPEED (2000000L) // 2MHz
#define PMW3901_SPI_BUS_SPEED (2000000L) // 2MHz
#define DIR_WRITE(a) ((a) | (1 << 7))
#define DIR_READ(a) ((a) & 0x7f)
#define DIR_WRITE(a) ((a) | (1 << 7))
#define DIR_READ(a) ((a) & 0x7f)
#define PMW3901_DEVICE_PATH "/dev/pmw3901"
#define PMW3901_DEVICE_PATH "/dev/pmw3901"
/* VL53LXX Registers addresses */
#define VL53LXX_CONVERSION_INTERVAL 1000 /* 1ms */
#define PMW3901_CONVERSION_INTERVAL 1000 /* 10 ms */
#define PMW3901_SAMPLE_RATE 10000 /* 20 ms */
#define PMW3901_INTEGRATOR_RESET_TIME 100000 /* 100 ms */
#ifndef CONFIG_SCHED_WORKQUEUE
# error This requires CONFIG_SCHED_WORKQUEUE.
@@ -97,14 +101,14 @@
class PMW3901 : public device::SPI
{
public:
PMW3901(uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING,
int bus = PMW3901_BUS);
PMW3901(uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING, int bus = PMW3901_BUS);
virtual ~PMW3901();
virtual int init();
//virtual ssize_t read(device::file_t *filp, char *buffer, size_t buflen);
virtual ssize_t read(device::file_t *filp, char *buffer, size_t buflen);
virtual int ioctl(device::file_t *filp, int cmd, unsigned long arg);
/**
@@ -114,27 +118,21 @@ public:
private:
uint8_t _rotation;
float _min_distance;
float _max_distance;
work_s _work;
work_s _work;
ringbuffer::RingBuffer *_reports;
bool _sensor_ok;
uint8_t _valid;
int _measure_ticks;
bool _collect_phase;
int _class_instance;
int _orb_class_instance;
bool _sensor_ok;
int _measure_ticks;
int _class_instance;
int _orb_class_instance;
orb_advert_t _distance_sensor_topic;
orb_advert_t _optical_flow_topic;
int _distance_sensor_sub;
perf_counter_t _sample_perf;
perf_counter_t _comms_errors;
perf_counter_t _sample_perf;
perf_counter_t _comms_errors;
Integrator _flow_int;
uint8_t stop_variable_;
Integrator _flow_int;
enum Rotation _sensor_rotation;
/**
@@ -155,14 +153,13 @@ private:
* and start a new one.
*/
void cycle();
int measure();
int collect();
int readRegister(unsigned reg, uint8_t *data, unsigned count);
int writeRegister(unsigned reg, uint8_t data);
int sensorInit();
void readMotionCount(int16_t &deltaX, int16_t &deltaY);
int readMotionCount(int16_t &deltaX, int16_t &deltaY);
/**
* Static trampoline from the workq context; because we don't have a
@@ -170,7 +167,7 @@ private:
*
* @param arg Instance pointer for the driver that is polling.
*/
static void cycle_trampoline(void *arg);
static void cycle_trampoline(void *arg);
};
@@ -186,23 +183,17 @@ extern "C" __EXPORT int pmw3901_main(int argc, char *argv[]);
PMW3901::PMW3901(uint8_t rotation, int bus) :
SPI("PMW3901", PMW3901_DEVICE_PATH, bus, PX4_SPIDEV_EXTERNAL1_1, SPIDEV_MODE0, PMW3901_SPI_BUS_SPEED),
_rotation(rotation),
_min_distance(-1.0f),
_max_distance(-1.0f),
_reports(nullptr),
_sensor_ok(false),
_valid(0),
_measure_ticks(0),
_collect_phase(false),
_class_instance(-1),
_orb_class_instance(-1),
_distance_sensor_topic(nullptr),
_distance_sensor_sub(-1),
_optical_flow_topic(nullptr),
_sample_perf(perf_alloc(PC_ELAPSED, "tr1_read")),
_comms_errors(perf_alloc(PC_COUNT, "tr1_com_err")),
_flow_int(100000, false)
_flow_int(PMW3901_INTEGRATOR_RESET_TIME, false),
_sensor_rotation((enum Rotation)rotation)
{
// up the retries since the device misses the first measure attempts
//I2C::_retries = 3;
// enable debug() calls
_debug_enabled = false;
@@ -221,9 +212,9 @@ PMW3901::~PMW3901()
delete _reports;
}
if (_class_instance != -1) {
unregister_class_devname(PMW3901_DEVICE_PATH, _class_instance);
}
// if (_class_instance != -1) {
// unregister_class_devname(PMW3901_DEVICE_PATH, _class_instance);
// }
// free perf counters
perf_free(_sample_perf);
@@ -234,113 +225,109 @@ PMW3901::~PMW3901()
int
PMW3901::sensorInit()
{
//uint8_t val = 0;
int ret;
uint8_t data[5];
uint8_t data[5];
// initialize pmw3901 flow sensor
// initialize pmw3901 flow sensor
readRegister(0x00, &data[0], 1); // chip idreadRegister(0x5F, &data[1], 1); // inverse chip id
readRegister(0x00, &data[0], 1); // chip id
readRegister(0x5F, &data[1], 1); // inverse chip id
// Power on reset
writeRegister(0x3A, 0x5A);
usleep(5000);
// Power on reset
writeRegister(0x3A, 0x5A);
usleep(5000);
// Test the SPI communication, checking chipId and inverse chipId
if (data[0] != 0x49 && data[1] != 0xB8) return false;
// Test the SPI communication, checking chipId and inverse chipId
// Reading the motion registers one time
readRegister(0x02, &data[0], 1);
readRegister(0x03, &data[1], 1);
readRegister(0x04, &data[2], 1);
readRegister(0x05, &data[3], 1);
readRegister(0x06, &data[4], 1);
usleep(1000);
//if (data[0] != 0x49 && data[1] != 0xB8) return false;
// set performance optimization registers
writeRegister(0x7F, 0x00);
writeRegister(0x61, 0xAD);
writeRegister(0x7F, 0x03);
writeRegister(0x40, 0x00);
writeRegister(0x7F, 0x05);
writeRegister(0x41, 0xB3);
writeRegister(0x43, 0xF1);
writeRegister(0x45, 0x14);
writeRegister(0x5B, 0x32);
writeRegister(0x5F, 0x34);
writeRegister(0x7B, 0x08);
writeRegister(0x7F, 0x06);
writeRegister(0x44, 0x1B);
writeRegister(0x40, 0xBF);
writeRegister(0x4E, 0x3F);
writeRegister(0x7F, 0x08);
writeRegister(0x65, 0x20);
writeRegister(0x6A, 0x18);
writeRegister(0x7F, 0x09);
writeRegister(0x4F, 0xAF);
writeRegister(0x5F, 0x40);
writeRegister(0x48, 0x80);
writeRegister(0x49, 0x80);
writeRegister(0x57, 0x77);
writeRegister(0x60, 0x78);
writeRegister(0x61, 0x78);
writeRegister(0x62, 0x08);
writeRegister(0x63, 0x50);
writeRegister(0x7F, 0x0A);
writeRegister(0x45, 0x60);
writeRegister(0x7F, 0x00);
writeRegister(0x4D, 0x11);
writeRegister(0x55, 0x80);
writeRegister(0x74, 0x1F);
writeRegister(0x75, 0x1F);
writeRegister(0x4A, 0x78);
writeRegister(0x4B, 0x78);
writeRegister(0x44, 0x08);
writeRegister(0x45, 0x50);
writeRegister(0x64, 0xFF);
writeRegister(0x65, 0x1F);
writeRegister(0x7F, 0x14);
writeRegister(0x65, 0x60);
writeRegister(0x66, 0x08);
writeRegister(0x63, 0x78);
writeRegister(0x7F, 0x15);
writeRegister(0x48, 0x58);
writeRegister(0x7F, 0x07);
writeRegister(0x41, 0x0D);
writeRegister(0x43, 0x14);
writeRegister(0x4B, 0x0E);
writeRegister(0x45, 0x0F);
writeRegister(0x44, 0x42);
writeRegister(0x4C, 0x80);
writeRegister(0x7F, 0x10);
writeRegister(0x5B, 0x02);
writeRegister(0x7F, 0x07);
writeRegister(0x40, 0x41);
writeRegister(0x70, 0x00);
// Reading the motion registers one time
readRegister(0x02, &data[0], 1);
readRegister(0x03, &data[1], 1);
readRegister(0x04, &data[2], 1);
readRegister(0x05, &data[3], 1);
readRegister(0x06, &data[4], 1);
usleep(1000);
usleep(10000);
// set performance optimization registers
writeRegister(0x7F, 0x00);
writeRegister(0x61, 0xAD);
writeRegister(0x7F, 0x03);
writeRegister(0x40, 0x00);
writeRegister(0x7F, 0x05);
writeRegister(0x41, 0xB3);
writeRegister(0x43, 0xF1);
writeRegister(0x45, 0x14);
writeRegister(0x5B, 0x32);
writeRegister(0x5F, 0x34);
writeRegister(0x7B, 0x08);
writeRegister(0x7F, 0x06);
writeRegister(0x44, 0x1B);
writeRegister(0x40, 0xBF);
writeRegister(0x4E, 0x3F);
writeRegister(0x7F, 0x08);
writeRegister(0x65, 0x20);
writeRegister(0x6A, 0x18);
writeRegister(0x7F, 0x09);
writeRegister(0x4F, 0xAF);
writeRegister(0x5F, 0x40);
writeRegister(0x48, 0x80);
writeRegister(0x49, 0x80);
writeRegister(0x57, 0x77);
writeRegister(0x60, 0x78);
writeRegister(0x61, 0x78);
writeRegister(0x62, 0x08);
writeRegister(0x63, 0x50);
writeRegister(0x7F, 0x0A);
writeRegister(0x45, 0x60);
writeRegister(0x7F, 0x00);
writeRegister(0x4D, 0x11);
writeRegister(0x55, 0x80);
writeRegister(0x74, 0x1F);
writeRegister(0x75, 0x1F);
writeRegister(0x4A, 0x78);
writeRegister(0x4B, 0x78);
writeRegister(0x44, 0x08);
writeRegister(0x45, 0x50);
writeRegister(0x64, 0xFF);
writeRegister(0x65, 0x1F);
writeRegister(0x7F, 0x14);
writeRegister(0x65, 0x60);
writeRegister(0x66, 0x08);
writeRegister(0x63, 0x78);
writeRegister(0x7F, 0x15);
writeRegister(0x48, 0x58);
writeRegister(0x7F, 0x07);
writeRegister(0x41, 0x0D);
writeRegister(0x43, 0x14);
writeRegister(0x4B, 0x0E);
writeRegister(0x45, 0x0F);
writeRegister(0x44, 0x42);
writeRegister(0x4C, 0x80);
writeRegister(0x7F, 0x10);
writeRegister(0x5B, 0x02);
writeRegister(0x7F, 0x07);
writeRegister(0x40, 0x41);
writeRegister(0x70, 0x00);
writeRegister(0x32, 0x44);
writeRegister(0x7F, 0x07);
writeRegister(0x40, 0x40);
writeRegister(0x7F, 0x06);
writeRegister(0x62, 0xf0);
writeRegister(0x63, 0x00);
writeRegister(0x7F, 0x0D);
writeRegister(0x48, 0xC0);
writeRegister(0x6F, 0xd5);
writeRegister(0x7F, 0x00);
writeRegister(0x5B, 0xa0);
writeRegister(0x4E, 0xA8);
writeRegister(0x5A, 0x50);
writeRegister(0x40, 0x80);
usleep(10000);
ret = OK;
writeRegister(0x32, 0x44);
writeRegister(0x7F, 0x07);
writeRegister(0x40, 0x40);
writeRegister(0x7F, 0x06);
writeRegister(0x62, 0xf0);
writeRegister(0x63, 0x00);
writeRegister(0x7F, 0x0D);
writeRegister(0x48, 0xC0);
writeRegister(0x6F, 0xd5);
writeRegister(0x7F, 0x00);
writeRegister(0x5B, 0xa0);
writeRegister(0x4E, 0xA8);
writeRegister(0x5A, 0x50);
writeRegister(0x40, 0x80);
ret = OK;
return ret;
return ret;
}
@@ -349,8 +336,6 @@ PMW3901::init()
{
int ret = PX4_ERROR;
_distance_sensor_sub = orb_subscribe(ORB_ID(distance_sensor));
/* do I2C init (and probe) first */
if (SPI::init() != OK) {
goto out;
@@ -372,7 +357,7 @@ PMW3901::init()
//if (_class_instance == CLASS_DEVICE_PRIMARY) { // change to optical flow topic
/* get a publish handle on the range finder topic */
// struct distance_sensor_s ds_report;
//measure();
// _reports->get(&ds_report);
// _distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report,
@@ -389,6 +374,7 @@ PMW3901::init()
out:
return ret;
}
@@ -397,224 +383,164 @@ PMW3901::ioctl(device::file_t *filp, int cmd, unsigned long arg)
{
switch (cmd) {
case SENSORIOCSPOLLRATE: {
switch (arg) {
case SENSORIOCSPOLLRATE: {
switch (arg) {
case SENSOR_POLLRATE_DEFAULT: {
/* do we need to start internal polling? */
//bool want_start = (_measure_ticks == 0);
case 0:
return -EINVAL;
/* set interval for next measurement to minimum legal value */
_measure_ticks = USEC2TICK(VL53LXX_CONVERSION_INTERVAL);
case SENSOR_POLLRATE_DEFAULT: {
/* if we need to start the poll state machine, do it */
// if (want_start) {
// start();
// }
start();
/* do we need to start internal polling? */
bool want_start = (_measure_ticks == 0);
return OK;
}
/* set interval for next measurement to minimum legal value */
_measure_ticks = USEC2TICK(PMW3901_CONVERSION_INTERVAL);
/* adjust to a legal polling interval in Hz */
default: {
/* do we need to start internal polling? */
//bool want_start = (_measure_ticks == 0);
/* if we need to start the poll state machine, do it */
if (want_start) {
start();
}
/* convert hz to tick interval via microseconds */
unsigned ticks = USEC2TICK(1000000 / arg);
/* update interval for next measurement */
_measure_ticks = ticks;
/* if we need to start the poll state machine, do it */
//if (want_start) {
start();
//}
return OK;
}
case SENSOR_POLLRATE_MANUAL: {
stop();
_measure_ticks = 0;
return OK;
}
}
}
default:
/* give it to the superclass */
return SPI::ioctl(filp, cmd, arg);
/* adjust to a legal polling interval in Hz */
default: {
/* do we need to start internal polling? */
bool want_start = (_measure_ticks == 0);
/* convert hz to tick interval via microseconds */
unsigned ticks = USEC2TICK(1000000 / arg);
/* check against maximum rate */
if (ticks < USEC2TICK(PMW3901_CONVERSION_INTERVAL)) {
return -EINVAL;
}
/* update interval for next measurement */
_measure_ticks = ticks;
/* if we need to start the poll state machine, do it */
if (want_start) {
start();
}
return OK;
}
}
}
default:
/* give it to the superclass */
return SPI::ioctl(filp, cmd, arg);
}
}
// ssize_t
// VL53LXX::read(device::file_t *filp, char *buffer, size_t buflen)
// {
// unsigned count = buflen / sizeof(struct distance_sensor_s);
// struct distance_sensor_s *rbuf = reinterpret_cast<struct distance_sensor_s *>(buffer);
// int ret = 0;
ssize_t
PMW3901::read(device::file_t *filp, char *buffer, size_t buflen)
{
unsigned count = buflen / sizeof(struct optical_flow_s);
struct optical_flow_s *rbuf = reinterpret_cast<struct optical_flow_s *>(buffer);
int ret = 0;
// /* buffer must be large enough */
// if (count < 1) {
// return -ENOSPC;
// }
/* buffer must be large enough */
if (count < 1) {
return -ENOSPC;
}
// /* if automatic measurement is enabled */
// if (_measure_ticks > 0) {
/* if automatic measurement is enabled */
if (_measure_ticks > 0) {
// /*
// * While there is space in the caller's buffer, and reports, copy them.
// * Note that we may be pre-empted by the workq thread while we are doing this;
// * we are careful to avoid racing with them.
// */
// while (count--) {
// if (_reports->get(rbuf)) {
// ret += sizeof(*rbuf);
// rbuf++;
// }
// }
/*
* While there is space in the caller's buffer, and reports, copy them.
* Note that we may be pre-empted by the workq thread while we are doing this;
* we are careful to avoid racing with them.
*/
while (count--) {
if (_reports->get(rbuf)) {
ret += sizeof(*rbuf);
rbuf++;
}
}
// /* if there was no data, warn the caller */
// return ret ? ret : -EAGAIN;
// }
/* if there was no data, warn the caller */
return ret ? ret : -EAGAIN;
}
// /* manual measurement - run one conversion */
// do {
// _reports->flush();
/* manual measurement - run one conversion */
do {
_reports->flush();
// /* trigger a measurement */
// if (OK != measure()) {
// ret = -EIO;
// break;
// }
/* trigger a measurement */
if (OK != collect()) {
ret = -EIO;
break;
}
// /* wait for it to complete */
// usleep(VL53LXX_CONVERSION_INTERVAL);
/* state machine will have generated a report, copy it out */
if (_reports->get(rbuf)) {
ret = sizeof(*rbuf);
}
// /* run the collection phase */
// if (OK != collect()) {
// ret = -EIO;
// break;
// }
} while (0);
// /* state machine will have generated a report, copy it out */
// if (_reports->get(rbuf)) {
// ret = sizeof(*rbuf);
// }
// } while (0);
// return ret;
// }
return ret;
}
int
PMW3901::readRegister(unsigned reg, uint8_t *data, unsigned count)
{
uint8_t cmd[5]; // read up to 4 bytes
int ret;
uint8_t cmd[5]; // read up to 4 bytes
int ret;
cmd[0] = DIR_READ(reg);
cmd[0] = DIR_READ(reg);
transfer(&cmd[0], &cmd[0], count+1);
memcpy(&data[0], &cmd[1], count);
ret = transfer(&cmd[0], &cmd[0], count+1);
ret = OK;
if (OK != ret) {
perf_count(_comms_errors);
DEVICE_LOG("spi::transfer returned %d", ret);
return ret;
}
return ret;
memcpy(&data[0], &cmd[1], count);
ret = OK;
return ret;
}
// int
// PMW3901::write(unsigned reg, uint8_t *data, unsigned count)
// {
// uint8_t cmd[5]; // write up to 4 bytes
// int ret;
// if (sizeof(cmd) < (count + 1)) {
// return -EIO;
// }
// cmd[0] = DIR_WRITE(reg);
// memcpy(&cmd[1], &data[0], count);
// transfer(&cmd[0], nullptr, count + 1);
// ret = OK;
// return ret;
// }
int
PMW3901::writeRegister(unsigned reg, uint8_t data)
{
uint8_t cmd[2]; // write up to 4 bytes
int ret;
uint8_t cmd[2]; // write 1 byte
int ret;
cmd[0] = DIR_WRITE(reg);
cmd[1] = data;
cmd[0] = DIR_WRITE(reg);
cmd[1] = data;
transfer(&cmd[0], nullptr, 2);
ret = transfer(&cmd[0], nullptr, 2);
ret = OK;
if (OK != ret) {
perf_count(_comms_errors);
DEVICE_LOG("spi::transfer returned %d", ret);
return ret;
}
return ret;
ret = OK;
}
int
PMW3901::measure()
{
int ret;
int16_t deltaX, deltaY;
readMotionCount(deltaX, deltaY);
uint64_t timestamp = hrt_absolute_time();
uint64_t integral_dt;
math::Vector<3> aval(deltaX, deltaY, 0);
math::Vector<3> aval_integrated;
double x_integral;
double y_integral;
bool accel_notify = _flow_int.put(timestamp, aval, aval_integrated, integral_dt);
x_integral = (double)aval_integrated(0);
y_integral = (double)aval_integrated(1);
printf("Timestamp = %lld\n", timestamp);
printf("deltaX= %d, deltaY= %d\n", deltaX, deltaY);
if(accel_notify) {
printf("Integral: X=%.2lf, Y=%.2lf\n\n", x_integral, y_integral);
}
ret = OK;
return ret;
}
void PMW3901::readMotionCount(int16_t &deltaX, int16_t &deltaY)
{
uint8_t tmp;
uint8_t dX[2];
uint8_t dY[2];
readRegister(0x02, &tmp, 1);
readRegister(0x03, &dX[0], 1);
readRegister(0x04, &dX[1], 1);
deltaX = ((int16_t)dX[1] << 8) | dX[0];
readRegister(0x05, &dY[0], 1);
readRegister(0x06, &dY[1], 1);
deltaY = ((int16_t)dY[1] << 8) | dY[0];
return;
return ret;
}
@@ -622,57 +548,120 @@ void PMW3901::readMotionCount(int16_t &deltaX, int16_t &deltaY)
int
PMW3901::collect()
{
int ret = -EIO;
int ret;
int16_t deltaX, deltaY;
/* read from the sensor */
//uint8_t val[2] = {0, 0};
perf_begin(_sample_perf);
perf_begin(_sample_perf);
readMotionCount(deltaX, deltaY);
uint64_t timestamp = hrt_absolute_time();
uint64_t integral_dt;
// ret = transfer(nullptr, 0, &val[0], 2); // change to spi transfer
//
// if (ret < 0) {
// DEVICE_LOG("error reading from sensor: %d", ret);
// perf_count(_comms_errors);
// perf_end(_sample_perf);
// return ret;
// }
//
// uint16_t distance_mm = (val[0] << 8) | val[1];
// float distance_m = float(distance_mm) * 1e-3f;
// struct distance_sensor_s report;
//
// report.timestamp = hrt_absolute_time(); // change to flow topic
// report.type = distance_sensor_s::MAV_DISTANCE_SENSOR_LASER;
// report.orientation = _rotation;
// report.current_distance = distance_m;
// report.min_distance = 0.0f;
// report.max_distance = 2.0f;
// report.covariance = 0.0f;
// /* TODO: set proper ID */
// report.id = 0;
//
// /* publish it, if we are the primary */
// if (_distance_sensor_topic != nullptr) {
// orb_publish(ORB_ID(distance_sensor), _distance_sensor_topic, &report);
// }
//
// _reports->force(&report);
//
// /* notify anyone waiting for data */
// poll_notify(POLLIN);
math::Vector<3> aval(deltaX, deltaY, 0);
math::Vector<3> aval_integrated;
float x_integral;
float y_integral;
bool accel_notify = _flow_int.put(timestamp, aval, aval_integrated, integral_dt);
printf("Timestamp = %lld\n", timestamp);
printf("deltaX= %d, deltaY= %d\n", deltaX, deltaY);
if(accel_notify) {
x_integral = (float)aval_integrated(0);
y_integral = (float)aval_integrated(1);
printf("Integral: X=%.4lf, Y=%.4lf\n", (double)x_integral, (double)y_integral);
printf("dt = %lld\n\n", integral_dt);
struct optical_flow_s report;
report.timestamp = hrt_absolute_time();
report.pixel_flow_x_integral = static_cast<float>(x_integral); /// 10000.0f; //convert to radians
report.pixel_flow_y_integral = static_cast<float>(y_integral); /// 10000.0f; //convert to radians
report.frame_count_since_last_readout = 10.0f;
report.integration_timespan = 100000; //microseconds
// report.time_since_last_sonar_update = f_integral.sonar_timestamp;//microseconds
report.sensor_id = 0;
/* rotate measurements in yaw from sensor frame to body frame according to parameter SENS_FLOW_ROT */
//float zeroval = 0.0f;
//rotate_3f(_sensor_rotation, report.pixel_flow_x_integral, report.pixel_flow_y_integral, zeroval);
if (_optical_flow_topic == nullptr) {
_optical_flow_topic = orb_advertise(ORB_ID(optical_flow), &report);
} else {
orb_publish(ORB_ID(optical_flow), _optical_flow_topic, &report);
}
/* post a report to the ring */
_reports->force(&report);
/* notify anyone waiting for data */
poll_notify(POLLIN);
}
ret = OK;
perf_end(_sample_perf);
return ret;
}
int
PMW3901::readMotionCount(int16_t &deltaX, int16_t &deltaY)
{
// uint8_t tmp;
// uint8_t dX[2];
// uint8_t dY[2];
int ret;
// readRegister(0x02, &tmp, 1);
// readRegister(0x03, &dX[0], 1);
// readRegister(0x04, &dX[1], 1);
// deltaX = ((int16_t)dX[1] << 8) | dX[0];
// readRegister(0x05, &dY[0], 1);
// readRegister(0x06, &dY[1], 1);
// deltaY = ((int16_t)dY[1] << 8) | dY[0];
uint8_t data[10] = { DIR_READ(0x02), 0, DIR_READ(0x03), 0, DIR_READ(0x04), 0,
DIR_READ(0x05), 0, DIR_READ(0x06), 0 };
// uint8_t data[5] = { DIR_READ(0x03), 0, 0, 0, 0 };
ret = transfer(&data[0], &data[0], 10);
if (OK != ret) {
perf_count(_comms_errors);
DEVICE_LOG("spi::transfer returned %d", ret);
return ret;
}
deltaX = ((int16_t)data[5] << 8) | data[3];
deltaY = ((int16_t)data[9] << 8) | data[7];
ret = OK;
perf_end(_sample_perf);
return ret;
}
void
PMW3901::start()
{
/* reset the report ring and state machine */
_collect_phase = false;
_reports->flush();
/* schedule a cycle to start things */
@@ -683,7 +672,7 @@ PMW3901::start()
info.present = true;
info.enabled = true;
info.ok = true;
info.subsystem_type = subsystem_info_s::SUBSYSTEM_TYPE_RANGEFINDER;
info.subsystem_type = subsystem_info_s::SUBSYSTEM_TYPE_OPTICALFLOW;
static orb_advert_t pub = nullptr;
@@ -712,15 +701,14 @@ PMW3901::cycle_trampoline(void *arg)
void
PMW3901::cycle()
{
measure();
//collect();
collect();
/* schedule a fresh cycle call when the measurement is done */
work_queue(HPWORK,
&_work,
(worker_t)&PMW3901::cycle_trampoline,
this,
USEC2TICK(10000));
USEC2TICK(PMW3901_SAMPLE_RATE));
}
@@ -748,6 +736,7 @@ void test();
void reset();
void info();
/**
* Start the driver.
*/
@@ -763,7 +752,6 @@ start(uint8_t rotation)
/* create the driver */
g_dev = new PMW3901(rotation, PMW3901_BUS);
if (g_dev == nullptr) {
goto fail;
}
@@ -811,6 +799,7 @@ void stop()
exit(0);
}
/**
* Perform some basic functional tests on the driver;
* make sure we can collect data from the sensor in polled
@@ -845,18 +834,15 @@ test()
err(1, "%s open failed (try 'vl53lxx start' if the driver is not running", PMW3901_DEVICE_PATH);
}
// warnx("single read");
// warnx("measurement: %0.2f m", (double)report.current_distance);
// warnx("time: %llu", report.timestamp);
/* start the sensor polling at 2Hz */
if (OK != ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT)) {
if (OK != ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT)) { // change this to read only a few samples
errx(1, "failed to set 2Hz poll rate");
}
errx(0, "PASS");
}
/**
* Reset the driver.
*/
@@ -880,6 +866,7 @@ reset()
exit(0);
}
/**
* Print a little info about the driver.
*/
@@ -896,13 +883,15 @@ info()
exit(0);
}
} // namespace
} // namespace pmw3901
int
pmw3901_main(int argc, char *argv[])
{
int ch;
int myoptind = 1;
const char *myoptarg = nullptr;
uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING;