diff --git a/src/drivers/optical_flow/pmw3901/CMakeLists.txt b/src/drivers/optical_flow/pmw3901/CMakeLists.txt index e3dd59ce18..33bb3c4511 100644 --- a/src/drivers/optical_flow/pmw3901/CMakeLists.txt +++ b/src/drivers/optical_flow/pmw3901/CMakeLists.txt @@ -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 ) diff --git a/src/drivers/optical_flow/pmw3901/pmw3901.cpp b/src/drivers/optical_flow/pmw3901/PMW3901.cpp similarity index 66% rename from src/drivers/optical_flow/pmw3901/pmw3901.cpp rename to src/drivers/optical_flow/pmw3901/PMW3901.cpp index efd7447c2a..5c91a4f175 100644 --- a/src/drivers/optical_flow/pmw3901/pmw3901.cpp +++ b/src/drivers/optical_flow/pmw3901/PMW3901.cpp @@ -31,123 +31,7 @@ * ****************************************************************************/ -/** - * @file pmw3901.cpp - * @author Daniele Pettenuzzo - * - * Driver for the pmw3901 optical flow sensor connected via SPI. - */ - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -/* 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_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; } diff --git a/src/drivers/optical_flow/pmw3901/PMW3901.hpp b/src/drivers/optical_flow/pmw3901/PMW3901.hpp new file mode 100644 index 0000000000..fea69cf9bf --- /dev/null +++ b/src/drivers/optical_flow/pmw3901/PMW3901.hpp @@ -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 +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +/* 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_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); +}; diff --git a/src/drivers/optical_flow/pmw3901/pmw3901_main.cpp b/src/drivers/optical_flow/pmw3901/pmw3901_main.cpp new file mode 100644 index 0000000000..3c59adc3b2 --- /dev/null +++ b/src/drivers/optical_flow/pmw3901/pmw3901_main.cpp @@ -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; +}