dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

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 2020-12-16 20:04:46 -05:00 committed by GitHub
parent 43b4cf39bd
commit 74083d6bd1
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
4 changed files with 538 additions and 482 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

893
src/drivers/optical_flow/paw3902/PAW3902.cpp
View File

File diff suppressed because it is too large Load Diff

59
src/drivers/optical_flow/paw3902/PAW3902.hpp
View File

@ -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};
uint64_t _previous_collect_timestamp{0};
uint64_t _flow_dt_sum_usec{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};
};

42
src/drivers/optical_flow/paw3902/PixArt_PAW3902JF_Registers.hpp
View File

@ -36,45 +36,45 @@
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,
Revision_ID = 0x01,
Motion = 0x02,
Delta_X_L = 0x03,
Delta_X_H = 0x04,
Delta_Y_L = 0x05,
Delta_Y_H = 0x06,
Squal = 0x07,
RawData_Sum = 0x08,
Maximum_RawData = 0x09,
Minimum_RawData = 0x0A,
Shutter_Lower = 0x0B,
Shutter_Upper = 0x0C,
Product_ID = 0x00,
Revision_ID = 0x01,
Motion = 0x02,
Delta_X_L = 0x03,
Delta_X_H = 0x04,
Delta_Y_L = 0x05,
Delta_Y_H = 0x06,
Squal = 0x07,
RawData_Sum = 0x08,
Maximum_RawData = 0x09,
Minimum_RawData = 0x0A,
Shutter_Lower = 0x0B,
Shutter_Upper = 0x0C,
Observation = 0x15,
Motion_Burst = 0x16,
Observation = 0x15,
Motion_Burst = 0x16,
Power_Up_Reset = 0x3A,
Power_Up_Reset = 0x3A,
Resolution = 0x4E,
Resolution = 0x4E,
Inverse_Product_ID = 0x5F,
};
enum Product_ID_Bit : uint8_t {
Reset = 0x5A,
};
enum class Mode {
Bright = 0,
LowLight = 1,

26
src/drivers/optical_flow/paw3902/paw3902_main.cpp
View File

@ -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;
}
}