drivers/optical_flow: new PixArt PAA3905 optical flow driver

This commit is contained in:
alexklimaj
2022-04-08 13:55:52 -06:00
committed by Daniel Agar
parent 9a9aad98a1
commit 6e0ac66c3c
10 changed files with 1053 additions and 5 deletions
+10 -4
View File
@@ -83,18 +83,24 @@ then
teraranger start -X
fi
# Possible external pmw3901 optical flow sensor
if param greater -s SENS_EN_PMW3901 0
# paa3905 optical flow sensor (external SPI)
if param greater -s SENS_EN_PAA3905 0
then
pmw3901 -S start
paa3905 -S start
fi
# Possible external paw3902 optical flow sensor
# paw3902 optical flow sensor (external SPI)
if param greater -s SENS_EN_PAW3902 0
then
paw3902 -S start
fi
# pmw3901 optical flow sensor (external SPI)
if param greater -s SENS_EN_PMW3901 0
then
pmw3901 -S start
fi
# Check for px4flow sensor
if param compare -s SENS_EN_PX4FLOW 1
then
+3 -1
View File
@@ -130,12 +130,14 @@
#define DRV_GYR_DEVTYPE_BMI088 0x66
#define DRV_BARO_DEVTYPE_BMP388 0x67
#define DRV_BARO_DEVTYPE_DPS310 0x68
#define DRV_PWM_DEVTYPE_PCA9685 0x69
#define DRV_ACC_DEVTYPE_BMI088 0x6a
#define DRV_OSD_DEVTYPE_ATXXXX 0x6b
#define DRV_FLOW_DEVTYPE_PMW3901 0x6c
#define DRV_FLOW_DEVTYPE_PAW3902 0x6d
#define DRV_FLOW_DEVTYPE_PX4FLOW 0x6e
#define DRV_PWM_DEVTYPE_PCA9685 0x6f
#define DRV_FLOW_DEVTYPE_PAA3905 0x6f
#define DRV_DIST_DEVTYPE_LL40LS 0x70
#define DRV_DIST_DEVTYPE_MAPPYDOT 0x71
+1
View File
@@ -2,6 +2,7 @@ menu "Optical flow"
menuconfig COMMON_OPTICAL_FLOW
bool "Common Optical flow"
default n
select DRIVERS_OPTICAL_FLOW_PAA3905
select DRIVERS_OPTICAL_FLOW_PAW3902
select DRIVERS_OPTICAL_FLOW_PMW3901
select DRIVERS_OPTICAL_FLOW_PX4FLOW
@@ -0,0 +1,46 @@
############################################################################
#
# Copyright (c) 2022 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.
#
############################################################################
px4_add_module(
MODULE drivers__optical_flow__paa3905
MAIN paa3905
SRCS
paa3905_main.cpp
PAA3905.cpp
PAA3905.hpp
PixArt_PAA3905_Registers.hpp
DEPENDS
conversion
drivers__device
px4_work_queue
)
+5
View File
@@ -0,0 +1,5 @@
menuconfig DRIVERS_OPTICAL_FLOW_PAA3905
bool "paa3905"
default n
---help---
Enable support for paa3905
File diff suppressed because it is too large Load Diff
@@ -0,0 +1,136 @@
/****************************************************************************
*
* Copyright (c) 2022 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 paa3905.hpp
*
* Driver for the Pixart paa3905 optical flow sensors connected via SPI.
*/
#pragma once
#include "PixArt_PAA3905_Registers.hpp"
#include <px4_platform_common/px4_config.h>
#include <px4_platform_common/defines.h>
#include <px4_platform_common/getopt.h>
#include <px4_platform_common/i2c_spi_buses.h>
#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>
using namespace time_literals;
using namespace PixArt_PAA3905;
#define DIR_WRITE(a) ((a) | (1 << 7))
#define DIR_READ(a) ((a) & 0x7f)
class PAA3905 : public device::SPI, public I2CSPIDriver<PAA3905>
{
public:
PAA3905(const I2CSPIDriverConfig &config);
virtual ~PAA3905();
static void print_usage();
int init() override;
void print_status() override;
void RunImpl();
private:
void exit_and_cleanup() override;
int probe() override;
static int DataReadyInterruptCallback(int irq, void *context, void *arg);
void DataReady();
bool DataReadyInterruptConfigure();
bool DataReadyInterruptDisable();
uint8_t RegisterRead(uint8_t reg, int retries = 2);
void RegisterWrite(uint8_t reg, uint8_t data);
bool RegisterWriteVerified(uint8_t reg, uint8_t data, int retries = 1);
void EnableLed();
void StandardDetectionSetting();
void EnhancedDetectionMode();
void ModeAuto012();
void CheckMode();
void Configure();
void ResetAccumulatedData();
uORB::PublicationMulti<optical_flow_s> _optical_flow_pub{ORB_ID(optical_flow)};
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 _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
const spi_drdy_gpio_t _drdy_gpio;
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};
matrix::Dcmf _rotation;
int _discard_reading{3};
int _flow_sum_x{0};
int _flow_sum_y{0};
Mode _mode{Mode::LowLight};
uint32_t _scheduled_interval_us{SAMPLE_INTERVAL_MODE_1};
int _valid_count{0};
bool _data_ready_interrupt_enabled{false};
hrt_abstime _last_good_publish{0};
hrt_abstime _last_reset{0};
};
@@ -0,0 +1,117 @@
/****************************************************************************
*
* Copyright (c) 2022 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.
*
****************************************************************************/
#pragma once
namespace PixArt_PAA3905
{
// TODO: move to a central header
static constexpr uint8_t Bit0 = (1 << 0);
static constexpr uint8_t Bit1 = (1 << 1);
static constexpr uint8_t Bit2 = (1 << 2);
static constexpr uint8_t Bit3 = (1 << 3);
static constexpr uint8_t Bit4 = (1 << 4);
static constexpr uint8_t Bit5 = (1 << 5);
static constexpr uint8_t Bit6 = (1 << 6);
static constexpr uint8_t Bit7 = (1 << 7);
static constexpr uint8_t PRODUCT_ID = 0xA2;
static constexpr uint8_t REVISION_ID = 0x00;
static constexpr uint8_t PRODUCT_ID_INVERSE = 0x5D;
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 uint32_t SPI_SPEED = 2 * 1000 * 1000; // 2MHz SPI serial interface
// Various time delay needed for paa3905
static constexpr uint32_t TIME_us_TSWW = 11; // actually 10.5us
static constexpr uint32_t TIME_us_TSRAD = 2;
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_Middle = 0x0C,
Shutter_Upper = 0x0D,
Observation = 0x15,
Motion_Burst = 0x16,
Power_Up_Reset = 0x3A,
Shutdown = 0x3B,
Resolution = 0x4E,
Inverse_Product_ID = 0x5F,
};
// Observation
enum Observation_Bit : uint8_t {
Reset = 0x5A,
};
enum class Mode {
Bright = 0,
LowLight = 1,
SuperLowLight = 2,
};
struct BURST_TRANSFER {
uint8_t Motion;
uint8_t Observation;
uint8_t Delta_X_L;
uint8_t Delta_X_H;
uint8_t Delta_Y_L;
uint8_t Delta_Y_H;
uint8_t Reserved;
uint8_t SQUAL;
uint8_t RawData_Sum;
uint8_t Maximum_RawData;
uint8_t Minimum_RawData;
uint8_t Shutter_Upper;
uint8_t Shutter_Middle;
uint8_t Shutter_Lower;
};
}
@@ -0,0 +1,84 @@
/****************************************************************************
*
* Copyright (c) 2022 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 "PAA3905.hpp"
#include <px4_platform_common/module.h>
void PAA3905::print_usage()
{
PRINT_MODULE_USAGE_NAME("paa3905", "driver");
PRINT_MODULE_USAGE_COMMAND("start");
PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(false, true);
PRINT_MODULE_USAGE_PARAM_INT('Y', 0, 0, 359, "custom yaw rotation (degrees)", true);
PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
}
extern "C" __EXPORT int paa3905_main(int argc, char *argv[])
{
int ch = 0;
using ThisDriver = PAA3905;
BusCLIArguments cli{false, true};
cli.custom1 = -1;
cli.spi_mode = SPIDEV_MODE0;
cli.default_spi_frequency = SPI_SPEED;
while ((ch = cli.getOpt(argc, argv, "Y:")) != EOF) {
switch (ch) {
case 'Y':
cli.custom1 = atoi(cli.optArg());
break;
}
}
const char *verb = cli.optArg();
if (!verb) {
ThisDriver::print_usage();
return -1;
}
BusInstanceIterator iterator(MODULE_NAME, cli, DRV_FLOW_DEVTYPE_PAA3905);
if (!strcmp(verb, "start")) {
return ThisDriver::module_start(cli, iterator);
} else if (!strcmp(verb, "stop")) {
return ThisDriver::module_stop(iterator);
} else if (!strcmp(verb, "status")) {
return ThisDriver::module_status(iterator);
}
ThisDriver::print_usage();
return -1;
}
@@ -0,0 +1,42 @@
/****************************************************************************
*
* Copyright (c) 2022 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.
*
****************************************************************************/
/**
* paa3905 Optical Flow
*
* @reboot_required true
*
* @boolean
* @group Sensors
*/
PARAM_DEFINE_INT32(SENS_EN_PAA3905, 0);