diff --git a/src/drivers/boards/crazyflie/spi.c b/src/drivers/boards/crazyflie/spi.c index f51383210f..40df3620be 100644 --- a/src/drivers/boards/crazyflie/spi.c +++ b/src/drivers/boards/crazyflie/spi.c @@ -21,8 +21,6 @@ #include "board_config.h" #include -//#include "stm32.h" - /**************************************************************************** * Pre-Processor Definitions ****************************************************************************/ @@ -76,15 +74,15 @@ __EXPORT int stm32_spi_bus_initialize(void) return -ENODEV; } - //#ifdef CONFIG_MMCSD +#ifdef CONFIG_MMCSD int ret = mmcsd_spislotinitialize(CONFIG_NSH_MMCSDMINOR, CONFIG_NSH_MMCSDSLOTNO, spi_expansion); if (ret != OK) { - //message("[boot] FAILED to bind SPI port 1 to the MMCSD driver\n"); + PX4_ERR("[boot] FAILED to bind SPI port 1 to the MMCSD driver\n"); return -ENODEV; } - //#endif +#endif return OK; @@ -104,7 +102,6 @@ __EXPORT void stm32_spi1select(FAR struct spi_dev_s *dev, uint32_t devid, bool s /* SPI select is active low, so write !selected to select the device */ int sel = (int) devid; - //ASSERT(PX4_SPI_BUS_ID(sel) == PX4_SPI_BUS_EXPANSION); /* Making sure the other peripherals are not selected */ @@ -148,18 +145,10 @@ __EXPORT void board_spi_reset(int ms) stm32_configgpio(GPIO_SPI1_MISO_OFF); stm32_configgpio(GPIO_SPI1_MOSI_OFF); - /* set the sensor rail off */ - //stm32_gpiowrite(GPIO_VDD_3V3_SENSORS_EN, 0); - /* wait for the sensor rail to reach GND */ usleep(ms * 1000); warnx("reset done, %d ms", ms); - /* re-enable power */ - - /* switch the sensor rail back on */ - //stm32_gpiowrite(GPIO_VDD_3V3_SENSORS_EN, 1); - /* wait a bit before starting SPI, different times didn't influence results */ usleep(100); diff --git a/src/drivers/distance_sensor/vl53lxx/vl53lxx.cpp b/src/drivers/distance_sensor/vl53lxx/vl53lxx.cpp index a4c5b1eee3..6c0be20e49 100644 --- a/src/drivers/distance_sensor/vl53lxx/vl53lxx.cpp +++ b/src/drivers/distance_sensor/vl53lxx/vl53lxx.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013-2017 PX4 Development Team. All rights reserved. + * Copyright (c) 2013-2018 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -58,7 +58,7 @@ #include #include -#include +#include #include #include @@ -74,41 +74,40 @@ /* Configuration Constants */ #ifdef PX4_I2C_BUS_EXPANSION -#define VL53LXX_BUS PX4_I2C_BUS_EXPANSION +#define VL53LXX_BUS PX4_I2C_BUS_EXPANSION #else -#define VL53LXX_BUS 0 +#define VL53LXX_BUS 0 #endif -#define VL53LXX_BASEADDR 0b0101001 // 7-bit address -#define VL53LXX_DEVICE_PATH "/dev/vl53lxx" +#define VL53LXX_BASEADDR 0b0101001 // 7-bit address +#define VL53LXX_DEVICE_PATH "/dev/vl53lxx" /* VL53LXX Registers addresses */ -#define VHV_CONFIG_PAD_SCL_SDA_EXTSUP_HW_REG 0x89 -#define MSRC_CONFIG_CONTROL_REG 0x60 -#define FINAL_RANGE_CONFIG_MIN_COUNT_RATE_RTN_LIMIT_REG 0x44 -#define SYSTEM_SEQUENCE_CONFIG_REG 0x01 -#define DYNAMIC_SPAD_REF_EN_START_OFFSET_REG 0x4F -#define DYNAMIC_SPAD_NUM_REQUESTED_REF_SPAD_REG 0x4E -#define GLOBAL_CONFIG_REF_EN_START_SELECT_REG 0xB6 -#define GLOBAL_CONFIG_SPAD_ENABLES_REF_0_REG 0xB0 -#define SYSTEM_INTERRUPT_CONFIG_GPIO_REG 0x0A -#define SYSTEM_SEQUENCE_CONFIG_REG 0x01 -#define SYSRANGE_START_REG 0x00 -#define RESULT_INTERRUPT_STATUS_REG 0x13 -#define SYSTEM_INTERRUPT_CLEAR_REG 0x0B -#define GLOBAL_CONFIG_SPAD_ENABLES_REF_0_REG 0xB0 -#define GPIO_HV_MUX_ACTIVE_HIGH_REG 0x84 -#define SYSTEM_INTERRUPT_CLEAR_REG 0x0B -#define RESULT_RANGE_STATUS_REG 0x14 +#define VHV_CONFIG_PAD_SCL_SDA_EXTSUP_HW_REG 0x89 +#define MSRC_CONFIG_CONTROL_REG 0x60 +#define FINAL_RANGE_CONFIG_MIN_COUNT_RATE_RTN_LIMIT_REG 0x44 +#define SYSTEM_SEQUENCE_CONFIG_REG 0x01 +#define DYNAMIC_SPAD_REF_EN_START_OFFSET_REG 0x4F +#define DYNAMIC_SPAD_NUM_REQUESTED_REF_SPAD_REG 0x4E +#define GLOBAL_CONFIG_REF_EN_START_SELECT_REG 0xB6 +#define GLOBAL_CONFIG_SPAD_ENABLES_REF_0_REG 0xB0 +#define SYSTEM_INTERRUPT_CONFIG_GPIO_REG 0x0A +#define SYSTEM_SEQUENCE_CONFIG_REG 0x01 +#define SYSRANGE_START_REG 0x00 +#define RESULT_INTERRUPT_STATUS_REG 0x13 +#define SYSTEM_INTERRUPT_CLEAR_REG 0x0B +#define GLOBAL_CONFIG_SPAD_ENABLES_REF_0_REG 0xB0 +#define GPIO_HV_MUX_ACTIVE_HIGH_REG 0x84 +#define SYSTEM_INTERRUPT_CLEAR_REG 0x0B +#define RESULT_RANGE_STATUS_REG 0x14 +#define VL53LXX_RA_IDENTIFICATION_MODEL_ID 0xC0 +#define VL53LXX_IDENTIFICATION_MODEL_ID 0xEEAA -#define VL53LXX_CONVERSION_INTERVAL 1000 /* 1ms */ -#define VL53LXX_SAMPLE_RATE 50000 /* 50ms */ +#define VL53LXX_MS 1000 /* 1ms */ +#define VL53LXX_SAMPLE_RATE 50000 /* 50ms */ -#define VL53LXX_MAX_RANGING_DISTANCE 2.0f -#define VL53LXX_MIN_RANGING_DISTANCE 0.0f - -#define VL53LXX_RA_IDENTIFICATION_MODEL_ID 0xC0 -#define VL53LXX_IDENTIFICATION_MODEL_ID 0xEEAA +#define VL53LXX_MAX_RANGING_DISTANCE 2.0f +#define VL53LXX_MIN_RANGING_DISTANCE 0.0f #ifndef CONFIG_SCHED_WORKQUEUE # error This requires CONFIG_SCHED_WORKQUEUE. @@ -122,72 +121,69 @@ public: virtual ~VL53LXX(); - virtual int init(); + virtual int init(); - virtual ssize_t read(device::file_t *filp, char *buffer, size_t buflen); // NOT TESTED YET + 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); + virtual int ioctl(device::file_t *filp, int cmd, unsigned long arg); /** * Diagnostics - print some basic information about the driver. */ - void print_info(); + void print_info(); protected: - virtual int probe(); + virtual int probe(); private: - uint8_t _rotation; - work_s _work; - ringbuffer::RingBuffer *_reports; - bool _sensor_ok; - int _measure_ticks; - bool _collect_phase; - bool _new_measurement; - bool _measurement_started; + uint8_t _rotation; + work_s _work; + ringbuffer::RingBuffer *_reports; + bool _sensor_ok; + int _measure_ticks; + bool _collect_phase; + bool _new_measurement; + bool _measurement_started; - int _class_instance; - int _orb_class_instance; + int _class_instance; + int _orb_class_instance; - orb_advert_t _distance_sensor_topic; + orb_advert_t _distance_sensor_topic; - perf_counter_t _sample_perf; - perf_counter_t _comms_errors; + perf_counter_t _sample_perf; + perf_counter_t _comms_errors; - uint8_t stop_variable_; + uint8_t _stop_variable; /** * Initialise the automatic measurement state machine and start it. - * - * @note This function is called at open and error time. It might make sense - * to make it more aggressive about resetting the bus in case of errors. */ - void start(); + void start(); /** * Stop the automatic measurement state machine. */ - void stop(); + void stop(); /** * Perform a poll cycle; collect from the previous measurement * and start a new one. */ - void cycle(); - int measure(); - int collect(); + void cycle(); + int measure(); + int collect(); - int readRegister(uint8_t reg_address, uint8_t &value); - int writeRegister(uint8_t reg_address, uint8_t value); + int readRegister(uint8_t reg_address, uint8_t &value); + int writeRegister(uint8_t reg_address, uint8_t value); - int writeRegisterMulti(uint8_t reg_address, uint8_t *value, uint8_t length); - int readRegisterMulti(uint8_t reg_address, uint8_t *value, uint8_t length); + int writeRegisterMulti(uint8_t reg_address, uint8_t *value, uint8_t length); + int readRegisterMulti(uint8_t reg_address, uint8_t *value, uint8_t length); - int sensorInit(); - bool spadCalculations(); - bool sensorTuning(); - bool singleRefCalibration(uint8_t byte); + int sensorInit(); + bool spadCalculations(); + bool sensorTuning(); + bool singleRefCalibration(uint8_t byte); /** * Static trampoline from the workq context; because we don't have a @@ -195,8 +191,7 @@ private: * * @param arg Instance pointer for the driver that is polling. */ - static void cycle_trampoline(void *arg); - + static void cycle_trampoline(void *arg); }; @@ -222,7 +217,7 @@ VL53LXX::VL53LXX(uint8_t rotation, int bus, int address) : _comms_errors(perf_alloc(PC_COUNT, "vl53lxx_com_err")) { // up the retries since the device misses the first measure attempts - I2C::_retries = 2; + I2C::_retries = 3; // enable debug() calls _debug_enabled = false; @@ -289,7 +284,7 @@ VL53LXX::sensorInit() return ret; } - stop_variable_ = val; + _stop_variable = val; // disable SIGNAL_RATE_MSRC (bit 1) and SIGNAL_RATE_PRE_RANGE (bit 4) limit checks readRegister(MSRC_CONFIG_CONTROL_REG, val); @@ -305,8 +300,6 @@ VL53LXX::sensorInit() spadCalculations(); - ret = OK; - return ret; } @@ -314,12 +307,13 @@ VL53LXX::sensorInit() int VL53LXX::init() { - int ret = PX4_ERROR; + int ret = OK; set_device_address(VL53LXX_BASEADDR); /* do I2C init (and probe) first */ if (I2C::init() != OK) { + ret = PX4_ERROR; goto out; } @@ -327,26 +321,12 @@ VL53LXX::init() _reports = new ringbuffer::RingBuffer(2, sizeof(distance_sensor_s)); if (_reports == nullptr) { + ret = PX4_ERROR; goto out; } _class_instance = register_class_devname(RANGE_FINDER_BASE_DEVICE_PATH); - if (_class_instance == CLASS_DEVICE_PRIMARY) { - /* get a publish handle on the range finder topic */ - struct distance_sensor_s ds_report; - - _reports->get(&ds_report); - - _distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report, - &_orb_class_instance, ORB_PRIO_LOW); - - if (_distance_sensor_topic == nullptr) { - DEVICE_LOG("failed to create distance_sensor object. Did you start uOrb?"); - } - } - - ret = OK; /* sensor is ok, but we don't really know if it is within range */ _sensor_ok = true; @@ -381,7 +361,7 @@ VL53LXX::ioctl(device::file_t *filp, int cmd, unsigned long arg) bool want_start = (_measure_ticks == 0); /* set interval for next measurement to minimum legal value */ - _measure_ticks = USEC2TICK(VL53LXX_CONVERSION_INTERVAL); + _measure_ticks = USEC2TICK(VL53LXX_SAMPLE_RATE); /* if we need to start the poll state machine, do it */ if (want_start) { @@ -469,6 +449,8 @@ VL53LXX::read(device::file_t *filp, char *buffer, size_t buflen) break; } + while (!_collect_phase); + /* run the collection phase */ if (OK != collect()) { ret = -EIO; @@ -601,7 +583,7 @@ VL53LXX::writeRegisterMulti(uint8_t reg_address, uint8_t *value, int VL53LXX::measure() { - int ret; + int ret = OK; uint8_t wait_for_measurement; uint8_t system_start; @@ -617,7 +599,7 @@ VL53LXX::measure() writeRegister(0x80, 0x01); writeRegister(0xFF, 0x01); writeRegister(0x00, 0x00); - writeRegister(0x91, stop_variable_); + writeRegister(0x91, _stop_variable); writeRegister(0x00, 0x01); writeRegister(0xFF, 0x00); writeRegister(0x80, 0x00); @@ -628,7 +610,7 @@ VL53LXX::measure() if ((system_start & 0x01) == 1) { work_queue(LPWORK, &_work, (worker_t)&VL53LXX::cycle_trampoline, this, - 1000); // reschedule every 1 ms until measurement is ready + USEC2TICK(VL53LXX_MS)); // reschedule every 1 ms until measurement is ready ret = OK; return ret; @@ -644,7 +626,7 @@ VL53LXX::measure() if ((system_start & 0x01) == 1) { work_queue(LPWORK, &_work, (worker_t)&VL53LXX::cycle_trampoline, this, - 1000); // reschedule every 1 ms until measurement is ready + USEC2TICK(VL53LXX_MS)); // reschedule every 1 ms until measurement is ready ret = OK; return ret; @@ -657,7 +639,7 @@ VL53LXX::measure() if ((wait_for_measurement & 0x07) == 0) { work_queue(LPWORK, &_work, (worker_t)&VL53LXX::cycle_trampoline, this, - 1000); // reschedule every 1 ms until measurement is ready + USEC2TICK(VL53LXX_MS)); // reschedule every 1 ms until measurement is ready ret = OK; return ret; } @@ -672,8 +654,6 @@ VL53LXX::measure() return ret; } - ret = OK; - return ret; } @@ -717,8 +697,9 @@ VL53LXX::collect() 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); + if (_class_instance == CLASS_DEVICE_PRIMARY) { + orb_publish_auto(ORB_ID(distance_sensor), &_distance_sensor_topic, &report, &_orb_class_instance, + ORB_PRIO_DEFAULT); } _reports->force(&report); @@ -741,7 +722,7 @@ VL53LXX::start() _reports->flush(); /* schedule a cycle to start things */ - work_queue(LPWORK, &_work, (worker_t)&VL53LXX::cycle_trampoline, this, 1000); + work_queue(LPWORK, &_work, (worker_t)&VL53LXX::cycle_trampoline, this, USEC2TICK(VL53LXX_MS)); /* notify about state change */ struct subsystem_info_s info = {}; @@ -753,11 +734,9 @@ VL53LXX::start() static orb_advert_t pub = nullptr; if (pub != nullptr) { - orb_publish(ORB_ID(subsystem_info), pub, &info); } else { - pub = orb_advertise(ORB_ID(subsystem_info), &info); } } @@ -795,7 +774,7 @@ VL53LXX::cycle() &_work, (worker_t)&VL53LXX::cycle_trampoline, this, - USEC2TICK(VL53LXX_SAMPLE_RATE)); + _measure_ticks); } } @@ -1024,7 +1003,6 @@ VL53LXX *g_dev; void start(uint8_t rotation); void stop(); void test(); -void reset(); void info(); /** @@ -1098,45 +1076,28 @@ void stop() void test() { - //struct distance_sensor_s report; + struct distance_sensor_s report; + ssize_t sz; int fd = open(VL53LXX_DEVICE_PATH, O_RDONLY); if (fd < 0) { - err(1, "%s open failed (try 'vl53lxx start' if the driver is not running", VL53LXX_DEVICE_PATH); + err(1, "%s open failed (try 'vl53lxx start' if the driver is not running)", VL53LXX_DEVICE_PATH); } + /* do a simple demand read */ + sz = read(fd, &report, sizeof(report)); - /* start the sensor polling at 2Hz */ - if (OK != ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT)) { - errx(1, "failed to set 2Hz poll rate"); + if (sz != sizeof(report)) { + PX4_ERR("ret: %d, expected: %d", sz, sizeof(report)); + err(1, "immediate acc read failed"); } + print_message(report); + errx(0, "PASS"); } -/** - * Reset the driver. - */ -void -reset() -{ - int fd = open(VL53LXX_DEVICE_PATH, O_RDONLY); - - if (fd < 0) { - err(1, "failed "); - } - - if (ioctl(fd, SENSORIOCRESET, 0) < 0) { - err(1, "driver reset failed"); - } - - if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { - err(1, "driver poll restart failed"); - } - - exit(0); -} /** * Print a little info about the driver. @@ -1160,27 +1121,13 @@ info() int vl53lxx_main(int argc, char *argv[]) { - int ch; int myoptind = 1; - const char *myoptarg = nullptr; uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING; if (argc < 2) { goto out; } - while ((ch = px4_getopt(argc, argv, "R:", &myoptind, &myoptarg)) != EOF) { - switch (ch) { - case 'R': - rotation = (uint8_t)atoi(myoptarg); - PX4_INFO("Setting sensor orientation to %d", (int)rotation); - break; - - default: - PX4_WARN("Unknown option!"); - } - } - /* * Start/load the driver. */ @@ -1202,22 +1149,15 @@ vl53lxx_main(int argc, char *argv[]) vl53lxx::test(); } - /* - * Reset the driver. - */ - if (!strcmp(argv[myoptind], "reset")) { - vl53lxx::reset(); - } - /* * Print driver information. */ - if (!strcmp(argv[myoptind], "info") || !strcmp(argv[1], "status")) { + if (!strcmp(argv[myoptind], "info") || !strcmp(argv[myoptind], "status")) { vl53lxx::info(); } out: - PX4_ERR("unrecognized command, try 'start', 'test', 'reset' or 'info'"); + PX4_ERR("unrecognized command, try 'start', 'test', or 'info'"); return PX4_ERROR; } diff --git a/src/drivers/pmw3901/CMakeLists.txt b/src/drivers/pmw3901/CMakeLists.txt index 61d078993d..e6c65ab87c 100644 --- a/src/drivers/pmw3901/CMakeLists.txt +++ b/src/drivers/pmw3901/CMakeLists.txt @@ -5,7 +5,5 @@ px4_add_module( -Wno-sign-compare SRCS pmw3901.cpp - DEPENDS - platforms__common ) # vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/drivers/pmw3901/pmw3901.cpp b/src/drivers/pmw3901/pmw3901.cpp index 8bfd9c30f4..a2eae1eb97 100644 --- a/src/drivers/pmw3901/pmw3901.cpp +++ b/src/drivers/pmw3901/pmw3901.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013-2017 PX4 Development Team. All rights reserved. + * Copyright (c) 2013-2018 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -60,7 +60,7 @@ #include -#include +#include #include #include @@ -78,27 +78,27 @@ /* Configuration Constants */ #ifdef PX4_SPI_BUS_EXPANSION -#define PMW3901_BUS PX4_SPI_BUS_EXPANSION +#define PMW3901_BUS PX4_SPI_BUS_EXPANSION #else -#define PMW3901_BUS 0 +#define PMW3901_BUS 0 #endif #ifdef PX4_SPIDEV_EXPANSION_2 -#define PMW3901_SPIDEV PX4_SPIDEV_EXPANSION_2 +#define PMW3901_SPIDEV PX4_SPIDEV_EXPANSION_2 #else -#define PMW3901_SPIDEV 0 +#define PMW3901_SPIDEV 0 #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" /* PMW3901 Registers addresses */ -#define PMW3901_CONVERSION_INTERVAL 1000 /* 1 ms */ -#define PMW3901_SAMPLE_RATE 10000 /* 10 ms */ +#define PMW3901_MS 1000 /* 1 ms */ +#define PMW3901_SAMPLE_RATE 10000 /* 10 ms */ #ifndef CONFIG_SCHED_WORKQUEUE @@ -112,37 +112,34 @@ public: virtual ~PMW3901(); - virtual int init(); + 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); + virtual int ioctl(device::file_t *filp, int cmd, unsigned long arg); /** * Diagnostics - print some basic information about the driver. */ - void print_info(); - - int collect(); + void print_info(); private: uint8_t _rotation; - work_s _work; - ringbuffer::RingBuffer *_reports; - bool _sensor_ok; - int _measure_ticks; - int _class_instance; - int _orb_class_instance; + work_s _work; + ringbuffer::RingBuffer *_reports; + bool _sensor_ok; + int _measure_ticks; + int _class_instance; + int _orb_class_instance; - orb_advert_t _optical_flow_pub; + orb_advert_t _optical_flow_pub; - perf_counter_t _sample_perf; - perf_counter_t _comms_errors; + perf_counter_t _sample_perf; + perf_counter_t _comms_errors; - uint64_t _previous_collect_timestamp; - - enum Rotation _sensor_rotation; + uint64_t _previous_collect_timestamp; + enum Rotation _sensor_rotation; /** @@ -151,25 +148,25 @@ private: * @note This function is called at open and error time. It might make sense * to make it more aggressive about resetting the bus in case of errors. */ - void start(); + void start(); /** * Stop the automatic measurement state machine. */ - void stop(); + void stop(); /** * Perform a poll cycle; collect from the previous measurement * and start a new one. */ - void cycle(); - // int collect(); + void cycle(); + int collect(); - int readRegister(unsigned reg, uint8_t *data, unsigned count); - int writeRegister(unsigned reg, uint8_t data); + int readRegister(unsigned reg, uint8_t *data, unsigned count); + int writeRegister(unsigned reg, uint8_t data); - int sensorInit(); - int readMotionCount(int16_t &deltaX, int16_t &deltaY); + int sensorInit(); + int readMotionCount(int16_t &deltaX, int16_t &deltaY); /** * Static trampoline from the workq context; because we don't have a @@ -177,14 +174,13 @@ private: * * @param arg Instance pointer for the driver that is polling. */ - static void cycle_trampoline(void *arg); + static void cycle_trampoline(void *arg); }; - /* * Driver 'main' command. */ @@ -199,8 +195,8 @@ PMW3901::PMW3901(uint8_t rotation, int bus) : _class_instance(-1), _orb_class_instance(-1), _optical_flow_pub(nullptr), - _sample_perf(perf_alloc(PC_ELAPSED, "tr1_read")), - _comms_errors(perf_alloc(PC_COUNT, "tr1_com_err")), + _sample_perf(perf_alloc(PC_ELAPSED, "pmw3901_read")), + _comms_errors(perf_alloc(PC_COUNT, "pmw3901_com_err")), _previous_collect_timestamp(0), _sensor_rotation((enum Rotation)rotation) { @@ -222,10 +218,6 @@ PMW3901::~PMW3901() delete _reports; } - // if (_class_instance != -1) { - // unregister_class_devname(PMW3901_DEVICE_PATH, _class_instance); - // } - // free perf counters perf_free(_sample_perf); perf_free(_comms_errors); @@ -354,7 +346,7 @@ PMW3901::init() /* For devices competing with NuttX SPI drivers on a bus (Crazyflie SD Card expansion board) */ SPI::set_lockmode(LOCK_THREADS); - /* do I2C init (and probe) first */ + /* do SPI init (and probe) first */ if (SPI::init() != OK) { goto out; } @@ -370,23 +362,6 @@ PMW3901::init() goto out; } - //_class_instance = register_class_devname(PMW3901_DEVICE_PATH); - - //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; - - // _reports->get(&ds_report); - - // _distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report, - // &_orb_class_instance, ORB_PRIO_LOW); - - // if (_distance_sensor_topic == nullptr) { - // DEVICE_LOG("failed to create distance_sensor object. Did you start uOrb?"); - // } - - //} - ret = OK; _sensor_ok = true; _previous_collect_timestamp = hrt_absolute_time(); @@ -414,7 +389,7 @@ PMW3901::ioctl(device::file_t *filp, int cmd, unsigned long arg) bool want_start = (_measure_ticks == 0); /* set interval for next measurement to minimum legal value */ - _measure_ticks = USEC2TICK(PMW3901_CONVERSION_INTERVAL); + _measure_ticks = USEC2TICK(PMW3901_SAMPLE_RATE); /* if we need to start the poll state machine, do it */ if (want_start) { @@ -440,7 +415,7 @@ PMW3901::ioctl(device::file_t *filp, int cmd, unsigned long arg) unsigned ticks = USEC2TICK(1000000 / arg); /* check against maximum rate */ - if (ticks < USEC2TICK(PMW3901_CONVERSION_INTERVAL)) { + if (ticks < USEC2TICK(PMW3901_SAMPLE_RATE)) { return -EINVAL; } @@ -567,7 +542,7 @@ PMW3901::writeRegister(unsigned reg, uint8_t data) int PMW3901::collect() { - int ret; + int ret = OK; int16_t delta_x_raw, delta_y_raw; float delta_x, delta_y; @@ -610,9 +585,8 @@ PMW3901::collect() /* notify anyone waiting for data */ poll_notify(POLLIN); - ret = OK; - perf_end(_sample_perf); + return ret; } @@ -652,7 +626,7 @@ PMW3901::start() _reports->flush(); /* schedule a cycle to start things */ - work_queue(LPWORK, &_work, (worker_t)&PMW3901::cycle_trampoline, this, 1000); + work_queue(LPWORK, &_work, (worker_t)&PMW3901::cycle_trampoline, this, USEC2TICK(PMW3901_MS)); /* notify about state change */ struct subsystem_info_s info = {}; @@ -796,68 +770,29 @@ void test() { - if (g_dev != nullptr) { - errx(1, "already started"); - } - - /* create the driver */ - g_dev = new PMW3901(0, PMW3901_BUS); - - if (g_dev == nullptr) { - delete g_dev; - g_dev = nullptr; - } - - if (OK != g_dev->init()) { - delete g_dev; - g_dev = nullptr; - - } + struct optical_flow_s report; + ssize_t sz; int fd = open(PMW3901_DEVICE_PATH, O_RDONLY); if (fd < 0) { - err(1, "%s open failed (try 'vl53lxx start' if the driver is not running", PMW3901_DEVICE_PATH); + err(1, "%s open failed (try 'pmw3901 start' if the driver is not running)", PMW3901_DEVICE_PATH); } - /* start the sensor polling at 2Hz */ - // if (OK != ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT)) { - // errx(1, "failed to set 2Hz poll rate"); - // } + /* do a simple demand read */ + sz = read(fd, &report, sizeof(report)); - for (int i = 0; i < 10000; i++) { - g_dev->collect(); - usleep(10000); + if (sz != sizeof(report)) { + PX4_ERR("ret: %d, expected: %d", sz, sizeof(report)); + err(1, "immediate acc read failed"); } + print_message(report); + errx(0, "PASS"); } -/** - * Reset the driver. - */ -void -reset() -{ - int fd = open(PMW3901_DEVICE_PATH, O_RDONLY); - - if (fd < 0) { - err(1, "failed "); - } - - if (ioctl(fd, SENSORIOCRESET, 0) < 0) { - err(1, "driver reset failed"); - } - - if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { - err(1, "driver poll restart failed"); - } - - exit(0); -} - - /** * Print a little info about the driver. */ @@ -880,28 +815,13 @@ info() 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; if (argc < 2) { goto out; } - while ((ch = px4_getopt(argc, argv, "R:", &myoptind, &myoptarg)) != EOF) { - switch (ch) { - case 'R': - rotation = (uint8_t)atoi(myoptarg); - PX4_INFO("Setting sensor orientation to %d", (int)rotation); - break; - - default: - PX4_WARN("Unknown option!"); - } - } - /* * Start/load the driver. */ @@ -923,22 +843,15 @@ pmw3901_main(int argc, char *argv[]) pmw3901::test(); } - /* - * Reset the driver. - */ - if (!strcmp(argv[myoptind], "reset")) { - pmw3901::reset(); - } - /* * Print driver information. */ - if (!strcmp(argv[myoptind], "info") || !strcmp(argv[1], "status")) { + if (!strcmp(argv[myoptind], "info") || !strcmp(argv[myoptind], "status")) { pmw3901::info(); } out: - PX4_ERR("unrecognized command, try 'start', 'test', 'reset' or 'info'"); + PX4_ERR("unrecognized command, try 'start', 'test', or 'info'"); return PX4_ERROR; }