Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-24 07:09:48 +08:00
Code Issues Actions 17 Packages Projects Releases Wiki Activity

crazyflie: optical flow and distace sensor driver fixes

Browse Source
This commit is contained in:
DanielePettenuzzo
2018-05-16 09:49:58 +02:00
committed by Beat Küng
parent 533f42adb8
commit 9c8e97a1ba
4 changed files with 151 additions and 311 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/boards/crazyflie/spi.c
+3 -14
View File
@@ -21,8 +21,6 @@
#include "board_config.h"
#include <systemlib/err.h>
//#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);
src/drivers/distance_sensor/vl53lxx/vl53lxx.cpp
+92 -152
View File
@@ -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 <math.h>
#include <unistd.h>
#include <systemlib/perf_counter.h>
#include <perf/perf_counter.h>
#include <systemlib/err.h>
#include <drivers/drv_hrt.h>
@@ -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;
}
src/drivers/pmw3901/CMakeLists.txt
-2
View File
@@ -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 :
src/drivers/pmw3901/pmw3901.cpp
+56 -143
View File
@@ -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 <conversion/rotation.h>
#include <systemlib/perf_counter.h>
#include <perf/perf_counter.h>
#include <systemlib/err.h>
#include <drivers/drv_hrt.h>
@@ -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;
}