From 6ca341f84ec146f29c599b89f0942883cb251b13 Mon Sep 17 00:00:00 2001 From: DanielePettenuzzo Date: Fri, 9 Mar 2018 09:26:31 +0100 Subject: [PATCH] pmw3901 driver: cleanup for pull request --- src/drivers/pmw3901/pmw3901.cpp | 715 ++++++++++++++++---------------- 1 file changed, 352 insertions(+), 363 deletions(-) diff --git a/src/drivers/pmw3901/pmw3901.cpp b/src/drivers/pmw3901/pmw3901.cpp index 18e57ef595..2f775aec30 100644 --- a/src/drivers/pmw3901/pmw3901.cpp +++ b/src/drivers/pmw3901/pmw3901.cpp @@ -58,6 +58,8 @@ #include #include +#include + #include #include @@ -68,8 +70,8 @@ #include #include -#include #include +#include #include @@ -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(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(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(x_integral); /// 10000.0f; //convert to radians + report.pixel_flow_y_integral = static_cast(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;