forked from Archive/PX4-Autopilot
optical_flow/paw3902: minor improvements
- verify register writes - cleanly reset on lighting mode changes as per the datasheet - perf counters for mode switches and other errors - cleanly reset on false motion detection - minimize sleep on reset - allowing setting custom rotation yaw angle directly on command line with -Y (ignoring SENS_FLOW_ROT) - enable LED output
This commit is contained in:
Daniel Agar
GitHub
parent
43b4cf39bd
commit
74083d6bd1
File diff suppressed because it is too large
Load Diff
|
|
@ -32,7 +32,7 @@
|
|||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file paw3902.cpp
|
||||
* @file PAW3902.hpp
|
||||
*
|
||||
* Driver for the Pixart PAW3902 & PAW3903 optical flow sensors connected via SPI.
|
||||
*/
|
||||
|
|
@ -50,27 +50,20 @@
|
|||
#include <lib/perf/perf_counter.h>
|
||||
#include <lib/parameters/param.h>
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <drivers/drv_range_finder.h>
|
||||
#include <uORB/PublicationMulti.hpp>
|
||||
#include <uORB/topics/optical_flow.h>
|
||||
|
||||
/* Configuration Constants */
|
||||
|
||||
#define PAW3902_SPI_BUS_SPEED (2000000L) // 2MHz
|
||||
|
||||
#define DIR_WRITE(a) ((a) | (1 << 7))
|
||||
#define DIR_READ(a) ((a) & 0x7f)
|
||||
|
||||
using namespace time_literals;
|
||||
using namespace PixArt_PAW3902JF;
|
||||
|
||||
// PAW3902JF-TXQT is PixArt Imaging
|
||||
#define DIR_WRITE(a) ((a) | (1 << 7))
|
||||
#define DIR_READ(a) ((a) & 0x7f)
|
||||
|
||||
class PAW3902 : public device::SPI, public I2CSPIDriver<PAW3902>
|
||||
{
|
||||
public:
|
||||
PAW3902(I2CSPIBusOption bus_option, int bus, int devid, enum Rotation yaw_rotation, int bus_frequency,
|
||||
spi_mode_e spi_mode);
|
||||
PAW3902(I2CSPIBusOption bus_option, int bus, int devid, int bus_frequency, spi_mode_e spi_mode,
|
||||
float yaw_rotation_degrees = NAN);
|
||||
virtual ~PAW3902();
|
||||
|
||||
static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator,
|
||||
|
|
@ -83,37 +76,38 @@ public:
|
|||
|
||||
void RunImpl();
|
||||
|
||||
void start();
|
||||
|
||||
protected:
|
||||
private:
|
||||
int probe() override;
|
||||
|
||||
private:
|
||||
uint8_t RegisterRead(uint8_t reg, int retries = 3);
|
||||
void RegisterWrite(uint8_t reg, uint8_t data);
|
||||
bool RegisterWriteVerified(uint8_t reg, uint8_t data, int retries = 5);
|
||||
|
||||
uint8_t registerRead(uint8_t reg);
|
||||
void registerWrite(uint8_t reg, uint8_t data);
|
||||
bool Reset();
|
||||
|
||||
bool reset();
|
||||
bool ModeBright();
|
||||
bool ModeLowLight();
|
||||
bool ModeSuperLowLight();
|
||||
|
||||
bool modeBright();
|
||||
bool modeLowLight();
|
||||
bool modeSuperLowLight();
|
||||
|
||||
bool changeMode(Mode newMode);
|
||||
bool ChangeMode(Mode newMode);
|
||||
|
||||
uORB::PublicationMulti<optical_flow_s> _optical_flow_pub{ORB_ID(optical_flow)};
|
||||
|
||||
perf_counter_t _sample_perf;
|
||||
perf_counter_t _comms_errors;
|
||||
perf_counter_t _dupe_count_perf;
|
||||
perf_counter_t _sample_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": read")};
|
||||
perf_counter_t _interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": interval")};
|
||||
perf_counter_t _comms_errors{perf_alloc(PC_COUNT, MODULE_NAME": com err")};
|
||||
perf_counter_t _false_motion_perf{perf_alloc(PC_COUNT, MODULE_NAME": false motion report")};
|
||||
perf_counter_t _mode_change_perf{perf_alloc(PC_COUNT, MODULE_NAME": mode change")};
|
||||
perf_counter_t _register_write_fail_perf{perf_alloc(PC_COUNT, MODULE_NAME": verified register write failed")};
|
||||
|
||||
static constexpr uint64_t _collect_time{15000}; // 15 milliseconds, optical flow data publish rate
|
||||
static constexpr uint64_t COLLECT_TIME{15000}; // 15 milliseconds, optical flow data publish rate
|
||||
|
||||
uint64_t _previous_collect_timestamp{0};
|
||||
uint64_t _flow_dt_sum_usec{0};
|
||||
unsigned _frame_count_since_last{0};
|
||||
uint8_t _flow_sample_counter{0};
|
||||
uint16_t _flow_quality_sum{0};
|
||||
|
||||
enum Rotation _yaw_rotation {ROTATION_NONE};
|
||||
matrix::Dcmf _rotation;
|
||||
|
||||
int _flow_sum_x{0};
|
||||
int _flow_sum_y{0};
|
||||
|
|
@ -123,5 +117,4 @@ private:
|
|||
uint8_t _low_to_superlow_counter{0};
|
||||
uint8_t _low_to_bright_counter{0};
|
||||
uint8_t _superlow_to_low_counter{0};
|
||||
|
||||
};
|
||||
|
|
|
|||
|
|
@ -36,15 +36,15 @@
|
|||
namespace PixArt_PAW3902JF
|
||||
{
|
||||
|
||||
static constexpr uint8_t PRODUCT_ID = 0x49; // shared with the PMW3901
|
||||
static constexpr uint8_t PRODUCT_ID = 0x49;
|
||||
static constexpr uint8_t REVISION_ID = 0x01;
|
||||
static constexpr uint8_t PRODUCT_ID_INVERSE = 0xB6;
|
||||
|
||||
static constexpr uint32_t SAMPLE_INTERVAL_MODE_0{1000000 / 126}; // 126 fps
|
||||
static constexpr uint32_t SAMPLE_INTERVAL_MODE_1{1000000 / 126}; // 126 fps
|
||||
static constexpr uint32_t SAMPLE_INTERVAL_MODE_2{1000000 / 50}; // 50 fps
|
||||
|
||||
static constexpr uint64_t T_SWW{11}; // 10.5 microseconds
|
||||
static constexpr uint64_t T_SRR{2}; // 1.5 microseconds
|
||||
static constexpr uint32_t SPI_SPEED = 2 * 1000 * 1000; // 2MHz SPI serial interface
|
||||
|
||||
enum Register : uint8_t {
|
||||
Product_ID = 0x00,
|
||||
|
|
@ -68,13 +68,13 @@ enum Register : uint8_t {
|
|||
|
||||
Resolution = 0x4E,
|
||||
|
||||
Inverse_Product_ID = 0x5F,
|
||||
};
|
||||
|
||||
enum Product_ID_Bit : uint8_t {
|
||||
Reset = 0x5A,
|
||||
};
|
||||
|
||||
|
||||
enum class Mode {
|
||||
Bright = 0,
|
||||
LowLight = 1,
|
||||
|
|
|
|||
|
|
@ -1,6 +1,6 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2019 PX4 Development Team. All rights reserved.
|
||||
* Copyright (c) 2019-2020 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
|
||||
|
|
@ -34,23 +34,20 @@
|
|||
#include "PAW3902.hpp"
|
||||
#include <px4_platform_common/module.h>
|
||||
|
||||
extern "C" __EXPORT int paw3902_main(int argc, char *argv[]);
|
||||
|
||||
void
|
||||
PAW3902::print_usage()
|
||||
void PAW3902::print_usage()
|
||||
{
|
||||
PRINT_MODULE_USAGE_NAME("paw3902", "driver");
|
||||
PRINT_MODULE_USAGE_COMMAND("start");
|
||||
PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(false, true);
|
||||
PRINT_MODULE_USAGE_PARAM_INT('R', 0, 0, 35, "Rotation", true);
|
||||
PRINT_MODULE_USAGE_PARAM_INT('Y', 0, 0, 359, "custom yaw rotation (degrees)", true);
|
||||
PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
|
||||
}
|
||||
|
||||
I2CSPIDriverBase *PAW3902::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator,
|
||||
int runtime_instance)
|
||||
{
|
||||
PAW3902 *instance = new PAW3902(iterator.configuredBusOption(), iterator.bus(), iterator.devid(), cli.rotation,
|
||||
cli.bus_frequency, cli.spi_mode);
|
||||
PAW3902 *instance = new PAW3902(iterator.configuredBusOption(), iterator.bus(), iterator.devid(), cli.bus_frequency,
|
||||
cli.spi_mode, cli.custom1);
|
||||
|
||||
if (!instance) {
|
||||
PX4_ERR("alloc failed");
|
||||
|
|
@ -65,19 +62,18 @@ I2CSPIDriverBase *PAW3902::instantiate(const BusCLIArguments &cli, const BusInst
|
|||
return instance;
|
||||
}
|
||||
|
||||
int
|
||||
paw3902_main(int argc, char *argv[])
|
||||
extern "C" __EXPORT int paw3902_main(int argc, char *argv[])
|
||||
{
|
||||
int ch;
|
||||
int ch = 0;
|
||||
using ThisDriver = PAW3902;
|
||||
BusCLIArguments cli{false, true};
|
||||
cli.spi_mode = SPIDEV_MODE0;
|
||||
cli.default_spi_frequency = PAW3902_SPI_BUS_SPEED;
|
||||
cli.default_spi_frequency = SPI_SPEED;
|
||||
|
||||
while ((ch = cli.getopt(argc, argv, "R:")) != EOF) {
|
||||
while ((ch = cli.getopt(argc, argv, "Y:")) != EOF) {
|
||||
switch (ch) {
|
||||
case 'R':
|
||||
cli.rotation = (enum Rotation)atoi(cli.optarg());
|
||||
case 'Y':
|
||||
cli.custom1 = atoi(cli.optarg());
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in New Issue