fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

pmw3901 split out header and main

Browse Source
This commit is contained in:
Daniel Agar 2019-08-17 10:45:55 -04:00
parent 41e544727c
commit ab92f3ab7d
4 changed files with 339 additions and 272 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
src/drivers/optical_flow/pmw3901/CMakeLists.txt
View File

@ -34,8 +34,8 @@
px4_add_module(
MODULE drivers__optical_flow__pmw3901
MAIN pmw3901
COMPILE_FLAGS
-Wno-cast-align # TODO: fix and enable
SRCS
pmw3901.cpp
pmw3901_main.cpp
PMW3901.cpp
PMW3901.hpp
)

270
src/drivers/optical_flow/pmw3901/pmw3901.cpp → src/drivers/optical_flow/pmw3901/PMW3901.cpp
View File

@ -31,123 +31,7 @@
*
****************************************************************************/
/**
* @file pmw3901.cpp
* @author Daniele Pettenuzzo
*
* Driver for the pmw3901 optical flow sensor connected via SPI.
*/
#include <px4_config.h>
#include <px4_defines.h>
#include <px4_getopt.h>
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
#include <drivers/device/spi.h>
#include <conversion/rotation.h>
#include <lib/perf/perf_counter.h>
#include <lib/parameters/param.h>
#include <drivers/drv_hrt.h>
#include <uORB/PublicationMulti.hpp>
#include <uORB/topics/optical_flow.h>
/* Configuration Constants */
#if defined PX4_SPI_BUS_EXPANSION // crazyflie
# define PMW3901_BUS PX4_SPI_BUS_EXPANSION
#elif defined PX4_SPI_BUS_EXTERNAL1 // fmu-v5
# define PMW3901_BUS PX4_SPI_BUS_EXTERNAL1
#elif defined PX4_SPI_BUS_EXTERNAL // fmu-v4 extspi
# define PMW3901_BUS PX4_SPI_BUS_EXTERNAL
#else
# error "add the required spi bus from board_config.h here"
#endif
#if defined PX4_SPIDEV_EXPANSION_2 // crazyflie flow deck
# define PMW3901_SPIDEV PX4_SPIDEV_EXPANSION_2
#elif defined PX4_SPIDEV_EXTERNAL1_1 // fmu-v5 ext CS1
# define PMW3901_SPIDEV PX4_SPIDEV_EXTERNAL1_1
#elif defined PX4_SPIDEV_EXTERNAL // fmu-v4 extspi
# define PMW3901_SPIDEV PX4_SPIDEV_EXTERNAL
#else
# error "add the required spi dev from board_config.h here"
#endif
#define PMW3901_SPI_BUS_SPEED (2000000L) // 2MHz
#define DIR_WRITE(a) ((a) | (1 << 7))
#define DIR_READ(a) ((a) & 0x7f)
#define PMW3901_DEVICE_PATH "/dev/pmw3901"
/* PMW3901 Registers addresses */
#define PMW3901_US 1000 /* 1 ms */
#define PMW3901_SAMPLE_INTERVAL 10000 /* 10 ms */
class PMW3901 : public device::SPI, public px4::ScheduledWorkItem
{
public:
PMW3901(int bus = PMW3901_BUS, enum Rotation yaw_rotation = (enum Rotation)0);
virtual ~PMW3901();
virtual int init();
/**
* Diagnostics - print some basic information about the driver.
*/
void print_info();
protected:
virtual int probe();
private:
const uint64_t _collect_time{15000}; // usecs, optical flow data publish rate
uORB::PublicationMulti<optical_flow_s> _optical_flow_pub{ORB_ID(optical_flow)};
perf_counter_t _sample_perf;
perf_counter_t _comms_errors;
uint64_t _previous_collect_timestamp{0};
enum Rotation _yaw_rotation;
int _flow_sum_x{0};
int _flow_sum_y{0};
uint64_t _flow_dt_sum_usec{0};
/**
* 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();
/**
* Stop the automatic measurement state machine.
*/
void stop();
/**
* Perform a poll cycle; collect from the previous measurement
* and start a new one.
*/
void Run() override;
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);
};
/*
* Driver 'main' command.
*/
extern "C" __EXPORT int pmw3901_main(int argc, char *argv[]);
#include "PMW3901.hpp"
PMW3901::PMW3901(int bus, enum Rotation yaw_rotation) :
SPI("PMW3901", PMW3901_DEVICE_PATH, bus, PMW3901_SPIDEV, SPIDEV_MODE0, PMW3901_SPI_BUS_SPEED),
@ -520,156 +404,4 @@ PMW3901::print_info()
{
perf_print_counter(_sample_perf);
perf_print_counter(_comms_errors);
printf("poll interval: %u\n", _measure_interval);
}
/**
* Local functions in support of the shell command.
*/
namespace pmw3901
{
PMW3901 *g_dev;
void start(int spi_bus);
void stop();
void test();
void reset();
void info();
void usage();
/**
* Start the driver.
*/
void
start(int spi_bus)
{
if (g_dev != nullptr) {
errx(1, "already started");
}
/* create the driver */
g_dev = new PMW3901(spi_bus, (enum Rotation)0);
if (g_dev == nullptr) {
goto fail;
}
if (OK != g_dev->init()) {
goto fail;
}
exit(0);
fail:
if (g_dev != nullptr) {
delete g_dev;
g_dev = nullptr;
}
errx(1, "driver start failed");
}
/**
* Stop the driver
*/
void stop()
{
if (g_dev != nullptr) {
delete g_dev;
g_dev = nullptr;
} else {
errx(1, "driver not running");
}
exit(0);
}
/**
* Print a little info about the driver.
*/
void
info()
{
if (g_dev == nullptr) {
errx(1, "driver not running");
}
printf("state @ %p\n", g_dev);
g_dev->print_info();
exit(0);
}
/**
* Print a little info about how to start/stop/use the driver
*/
void usage()
{
PX4_INFO("usage: pmw3901 {start|test|reset|info'}");
PX4_INFO(" [-b SPI_BUS]");
}
} // namespace pmw3901
int
pmw3901_main(int argc, char *argv[])
{
if (argc < 2) {
pmw3901::usage();
return PX4_ERROR;
}
// don't exit from getopt loop to leave getopt global variables in consistent state,
// set error flag instead
bool err_flag = false;
int ch;
int myoptind = 1;
const char *myoptarg = nullptr;
int spi_bus = PMW3901_BUS;
while ((ch = px4_getopt(argc, argv, "b:", &myoptind, &myoptarg)) != EOF) {
switch (ch) {
case 'b':
spi_bus = (uint8_t)atoi(myoptarg);
break;
default:
err_flag = true;
break;
}
}
if (err_flag) {
pmw3901::usage();
return PX4_ERROR;
}
/*
* Start/load the driver.
*/
if (!strcmp(argv[myoptind], "start")) {
pmw3901::start(spi_bus);
}
/*
* Stop the driver
*/
if (!strcmp(argv[myoptind], "stop")) {
pmw3901::stop();
}
/*
* Print driver information.
*/
if (!strcmp(argv[myoptind], "info") || !strcmp(argv[myoptind], "status")) {
pmw3901::info();
}
pmw3901::usage();
return PX4_ERROR;
}

145
src/drivers/optical_flow/pmw3901/PMW3901.hpp Normal file
View File

@ -0,0 +1,145 @@
/****************************************************************************
*
* Copyright (c) 2018-2019 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
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file PMW3901.hpp
* @author Daniele Pettenuzzo
*
* Driver for the pmw3901 optical flow sensor connected via SPI.
*/
#include <px4_config.h>
#include <px4_defines.h>
#include <px4_getopt.h>
#include <px4_work_queue/ScheduledWorkItem.hpp>
#include <drivers/device/spi.h>
#include <conversion/rotation.h>
#include <lib/perf/perf_counter.h>
#include <lib/parameters/param.h>
#include <drivers/drv_hrt.h>
#include <uORB/PublicationMulti.hpp>
#include <uORB/topics/optical_flow.h>
/* Configuration Constants */
#if defined PX4_SPI_BUS_EXPANSION // crazyflie
# define PMW3901_BUS PX4_SPI_BUS_EXPANSION
#elif defined PX4_SPI_BUS_EXTERNAL1 // fmu-v5
# define PMW3901_BUS PX4_SPI_BUS_EXTERNAL1
#elif defined PX4_SPI_BUS_EXTERNAL // fmu-v4 extspi
# define PMW3901_BUS PX4_SPI_BUS_EXTERNAL
#else
# error "add the required spi bus from board_config.h here"
#endif
#if defined PX4_SPIDEV_EXPANSION_2 // crazyflie flow deck
# define PMW3901_SPIDEV PX4_SPIDEV_EXPANSION_2
#elif defined PX4_SPIDEV_EXTERNAL1_1 // fmu-v5 ext CS1
# define PMW3901_SPIDEV PX4_SPIDEV_EXTERNAL1_1
#elif defined PX4_SPIDEV_EXTERNAL // fmu-v4 extspi
# define PMW3901_SPIDEV PX4_SPIDEV_EXTERNAL
#else
# error "add the required spi dev from board_config.h here"
#endif
#define PMW3901_SPI_BUS_SPEED (2000000L) // 2MHz
#define DIR_WRITE(a) ((a) | (1 << 7))
#define DIR_READ(a) ((a) & 0x7f)
#define PMW3901_DEVICE_PATH "/dev/pmw3901"
/* PMW3901 Registers addresses */
#define PMW3901_US 1000 /* 1 ms */
#define PMW3901_SAMPLE_INTERVAL 10000 /* 10 ms */
class PMW3901 : public device::SPI, public px4::ScheduledWorkItem
{
public:
PMW3901(int bus = PMW3901_BUS, enum Rotation yaw_rotation = (enum Rotation)0);
virtual ~PMW3901();
virtual int init();
/**
* Diagnostics - print some basic information about the driver.
*/
void print_info();
protected:
virtual int probe();
private:
const uint64_t _collect_time{15000}; // usecs, optical flow data publish rate
uORB::PublicationMulti<optical_flow_s> _optical_flow_pub{ORB_ID(optical_flow)};
perf_counter_t _sample_perf;
perf_counter_t _comms_errors;
uint64_t _previous_collect_timestamp{0};
enum Rotation _yaw_rotation;
int _flow_sum_x{0};
int _flow_sum_y{0};
uint64_t _flow_dt_sum_usec{0};
/**
* 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();
/**
* Stop the automatic measurement state machine.
*/
void stop();
/**
* Perform a poll cycle; collect from the previous measurement
* and start a new one.
*/
void Run() override;
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);
};

190
src/drivers/optical_flow/pmw3901/pmw3901_main.cpp Normal file
View File

@ -0,0 +1,190 @@
/****************************************************************************
*
* Copyright (c) 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
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#include "PMW3901.hpp"
/*
* Driver 'main' command.
*/
extern "C" __EXPORT int pmw3901_main(int argc, char *argv[]);
/**
* Local functions in support of the shell command.
*/
namespace pmw3901
{
PMW3901 *g_dev;
void start(int spi_bus);
void stop();
void test();
void reset();
void info();
void usage();
/**
* Start the driver.
*/
void
start(int spi_bus)
{
if (g_dev != nullptr) {
errx(1, "already started");
}
/* create the driver */
g_dev = new PMW3901(spi_bus, (enum Rotation)0);
if (g_dev == nullptr) {
goto fail;
}
if (OK != g_dev->init()) {
goto fail;
}
exit(0);
fail:
if (g_dev != nullptr) {
delete g_dev;
g_dev = nullptr;
}
errx(1, "driver start failed");
}
/**
* Stop the driver
*/
void stop()
{
if (g_dev != nullptr) {
delete g_dev;
g_dev = nullptr;
} else {
errx(1, "driver not running");
}
exit(0);
}
/**
* Print a little info about the driver.
*/
void
info()
{
if (g_dev == nullptr) {
errx(1, "driver not running");
}
printf("state @ %p\n", g_dev);
g_dev->print_info();
exit(0);
}
/**
* Print a little info about how to start/stop/use the driver
*/
void usage()
{
PX4_INFO("usage: pmw3901 {start|test|reset|info'}");
PX4_INFO(" [-b SPI_BUS]");
}
} // namespace pmw3901
int
pmw3901_main(int argc, char *argv[])
{
if (argc < 2) {
pmw3901::usage();
return PX4_ERROR;
}
// don't exit from getopt loop to leave getopt global variables in consistent state,
// set error flag instead
bool err_flag = false;
int ch;
int myoptind = 1;
const char *myoptarg = nullptr;
int spi_bus = PMW3901_BUS;
while ((ch = px4_getopt(argc, argv, "b:", &myoptind, &myoptarg)) != EOF) {
switch (ch) {
case 'b':
spi_bus = (uint8_t)atoi(myoptarg);
break;
default:
err_flag = true;
break;
}
}
if (err_flag) {
pmw3901::usage();
return PX4_ERROR;
}
/*
* Start/load the driver.
*/
if (!strcmp(argv[myoptind], "start")) {
pmw3901::start(spi_bus);
}
/*
* Stop the driver
*/
if (!strcmp(argv[myoptind], "stop")) {
pmw3901::stop();
}
/*
* Print driver information.
*/
if (!strcmp(argv[myoptind], "info") || !strcmp(argv[myoptind], "status")) {
pmw3901::info();
}
pmw3901::usage();
return PX4_ERROR;
}