Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-23 22:58:10 +08:00
Code Issues Actions 15 Packages Projects Releases Wiki Activity

drivers/optical_flow: paw3902/paa3905 refactor into simple state machine for reset/configure/read

Browse Source
This commit is contained in:
Daniel Agar
2022-07-05 22:18:18 -04:00
parent d5f7763817
commit a96187cb18
14 changed files with 834 additions and 751 deletions
Download Patch File Download Diff File Expand all files Collapse all files
boards/nxp/fmuk66-v3/default.px4board
+2 -5
View File
@@ -23,10 +23,7 @@ CONFIG_DRIVERS_LIGHTS_RGBLED=y
CONFIG_DRIVERS_LIGHTS_RGBLED_NCP5623C=y
CONFIG_DRIVERS_LIGHTS_RGBLED_PWM=y
CONFIG_COMMON_MAGNETOMETER=y
CONFIG_DRIVERS_OPTICAL_FLOW_PAW3902=y
CONFIG_DRIVERS_OPTICAL_FLOW_PMW3901=y
CONFIG_DRIVERS_OPTICAL_FLOW_PX4FLOW=y
CONFIG_DRIVERS_OPTICAL_FLOW_THONEFLOW=y
CONFIG_COMMON_OPTICAL_FLOW=y
CONFIG_DRIVERS_OSD=y
CONFIG_DRIVERS_PCA9685=y
CONFIG_DRIVERS_PCA9685_PWM_OUT=y
@@ -55,6 +52,7 @@ CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
CONFIG_MODULES_FW_ATT_CONTROL=y
CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
CONFIG_MODULES_FW_POS_CONTROL_L1=y
CONFIG_MODULES_GIMBAL=y
CONFIG_MODULES_GYRO_CALIBRATION=y
CONFIG_MODULES_GYRO_FFT=y
CONFIG_MODULES_LAND_DETECTOR=y
@@ -80,7 +78,6 @@ CONFIG_MODULES_SIMULATION_SENSOR_GPS_SIM=y
CONFIG_MODULES_SIMULATION_SENSOR_MAG_SIM=y
CONFIG_MODULES_SIMULATION_SIMULATOR_SIH=y
CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
CONFIG_MODULES_GIMBAL=y
CONFIG_MODULES_VTOL_ATT_CONTROL=y
CONFIG_SYSTEMCMDS_DUMPFILE=y
CONFIG_SYSTEMCMDS_I2CDETECT=y
src/drivers/optical_flow/Kconfig
+1 -1
View File
@@ -8,6 +8,6 @@ menu "Optical flow"
select DRIVERS_OPTICAL_FLOW_PX4FLOW
select DRIVERS_OPTICAL_FLOW_THONEFLOW
---help---
Enable default set of magnetometer drivers
Enable default set of optical flow drivers
rsource "*/Kconfig"
endmenu
src/drivers/optical_flow/paa3905/CMakeLists.txt
+2
View File
@@ -34,6 +34,8 @@
px4_add_module(
MODULE drivers__optical_flow__paa3905
MAIN paa3905
COMPILE_FLAGS
#-DDEBUG_BUILD
SRCS
paa3905_main.cpp
PAA3905.cpp
src/drivers/optical_flow/paa3905/Kconfig
+1 -1
View File
@@ -2,4 +2,4 @@ menuconfig DRIVERS_OPTICAL_FLOW_PAA3905
bool "paa3905"
default n
---help---
Enable support for paa3905
Enable support for PixArt paa3905
src/drivers/optical_flow/paa3905/PAA3905.cpp
+367 -314
View File
File diff suppressed because it is too large Load Diff
src/drivers/optical_flow/paa3905/PAA3905.hpp
+30 -38
View File
@@ -41,14 +41,12 @@
#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 <drivers/drv_hrt.h>
#include <drivers/device/spi.h>
#include <lib/conversion/rotation.h>
#include <lib/perf/perf_counter.h>
#include <px4_platform_common/atomic.h>
#include <px4_platform_common/i2c_spi_buses.h>
#include <uORB/PublicationMulti.hpp>
#include <uORB/topics/sensor_optical_flow.h>
@@ -66,18 +64,18 @@ public:
static void print_usage();
int init() override;
void print_status() override;
void RunImpl();
int init() override;
void print_status() override;
private:
void exit_and_cleanup() override;
int probe() override;
void Reset();
bool Reset();
bool Configure();
static int DataReadyInterruptCallback(int irq, void *context, void *arg);
void DataReady();
@@ -87,25 +85,26 @@ private:
uint8_t RegisterRead(uint8_t reg);
void RegisterWrite(uint8_t reg, uint8_t data);
void Configure();
void ConfigureAutomaticModeSwitching();
void ConfigureModeBright();
void ConfigureModeLowLight();
void ConfigureModeSuperLowLight();
void ConfigureStandardDetectionSetting();
void ConfigureEnhancedDetectionMode();
void EnableLed();
bool UpdateMode(const uint8_t observation);
enum class STATE : uint8_t {
RESET,
WAIT_FOR_RESET,
CONFIGURE,
READ,
} _state{STATE::RESET};
uORB::PublicationMulti<sensor_optical_flow_s> _sensor_optical_flow_pub{ORB_ID(sensor_optical_flow)};
perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")};
perf_counter_t _interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": interval")};
const spi_drdy_gpio_t _drdy_gpio;
matrix::Dcmf _rotation;
perf_counter_t _bad_register_perf{perf_alloc(PC_COUNT, MODULE_NAME": bad register")};
perf_counter_t _bad_transfer_perf{perf_alloc(PC_COUNT, MODULE_NAME": bad transfer")};
perf_counter_t _reset_perf{perf_alloc(PC_COUNT, MODULE_NAME": reset")};
perf_counter_t _false_motion_perf{perf_alloc(PC_COUNT, MODULE_NAME": false motion report")};
perf_counter_t _mode_change_bright_perf{perf_alloc(PC_COUNT, MODULE_NAME": mode change bright (0)")};
@@ -113,25 +112,18 @@ private:
perf_counter_t _mode_change_super_low_light_perf{perf_alloc(PC_COUNT, MODULE_NAME": mode change super low light (2)")};
perf_counter_t _no_motion_interrupt_perf{nullptr};
const spi_drdy_gpio_t _drdy_gpio;
matrix::Dcmf _rotation;
int _discard_reading{3};
Mode _mode{Mode::LowLight};
uint32_t _scheduled_interval_us{SAMPLE_INTERVAL_MODE_0};
hrt_abstime _reset_timestamp{0};
hrt_abstime _last_publish{0};
int _failure_count{0};
int _discard_reading{0};
px4::atomic<hrt_abstime> _drdy_timestamp_sample{0};
bool _data_ready_interrupt_enabled{false};
uint32_t _scheduled_interval_us{SAMPLE_INTERVAL_MODE_0 / 2};
Mode _mode{Mode::LowLight};
hrt_abstime _last_write_time{0};
hrt_abstime _last_read_time{0};
// force reset if there hasn't been valid data for an extended period (sensor could be in a bad state)
static constexpr hrt_abstime RESET_TIMEOUT_US = 3_s;
hrt_abstime _last_good_data{0};
hrt_abstime _last_reset{0};
};
src/drivers/optical_flow/paa3905/PixArt_PAA3905_Registers.hpp
+1 -2
View File
@@ -86,14 +86,13 @@ enum Register : uint8_t {
Power_Up_Reset = 0x3A,
Shutdown = 0x3B,
Resolution = 0x4E,
Resolution = 0x4E,
Inverse_Product_ID = 0x5F,
};
enum Motion_Bit : uint8_t {
MotionOccurred = Bit7, // Motion since last report
ChallengingSurface = Bit0, // Challenging surface is detected
};
src/drivers/optical_flow/paa3905/paa3905_main.cpp
+2
View File
@@ -32,6 +32,8 @@
****************************************************************************/
#include "PAA3905.hpp"
#include <px4_platform_common/getopt.h>
#include <px4_platform_common/module.h>
void PAA3905::print_usage()
src/drivers/optical_flow/paw3902/CMakeLists.txt
+2
View File
@@ -34,6 +34,8 @@
px4_add_module(
MODULE drivers__optical_flow__paw3902
MAIN paw3902
COMPILE_FLAGS
#-DDEBUG_BUILD
SRCS
paw3902_main.cpp
PAW3902.cpp
src/drivers/optical_flow/paw3902/Kconfig
+1 -1
View File
@@ -2,4 +2,4 @@ menuconfig DRIVERS_OPTICAL_FLOW_PAW3902
bool "paw3902"
default n
---help---
Enable support for paw3902
Enable support for PixArt paw3902
src/drivers/optical_flow/paw3902/PAW3902.cpp
+391 -354
View File
File diff suppressed because it is too large Load Diff
src/drivers/optical_flow/paw3902/PAW3902.hpp
+30 -33
View File
@@ -41,14 +41,12 @@
#include "PixArt_PAW3902_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 <drivers/drv_hrt.h>
#include <drivers/device/spi.h>
#include <lib/conversion/rotation.h>
#include <lib/perf/perf_counter.h>
#include <px4_platform_common/atomic.h>
#include <px4_platform_common/i2c_spi_buses.h>
#include <uORB/PublicationMulti.hpp>
#include <uORB/topics/sensor_optical_flow.h>
@@ -66,18 +64,18 @@ public:
static void print_usage();
int init() override;
void print_status() override;
void RunImpl();
int init() override;
void print_status() override;
private:
void exit_and_cleanup() override;
int probe() override;
void Reset();
bool Reset();
bool Configure();
static int DataReadyInterruptCallback(int irq, void *context, void *arg);
void DataReady();
@@ -87,20 +85,26 @@ private:
uint8_t RegisterRead(uint8_t reg);
void RegisterWrite(uint8_t reg, uint8_t data);
void Configure();
bool ChangeMode(Mode newMode, bool force = false);
void ConfigureModeBright();
void ConfigureModeLowLight();
void ConfigureModeSuperLowLight();
void EnableLed();
enum class STATE : uint8_t {
RESET,
WAIT_FOR_RESET,
CONFIGURE,
READ,
} _state{STATE::RESET};
uORB::PublicationMulti<sensor_optical_flow_s> _sensor_optical_flow_pub{ORB_ID(sensor_optical_flow)};
perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")};
perf_counter_t _interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": interval")};
const spi_drdy_gpio_t _drdy_gpio;
matrix::Dcmf _rotation;
perf_counter_t _bad_register_perf{perf_alloc(PC_COUNT, MODULE_NAME": bad register")};
perf_counter_t _bad_transfer_perf{perf_alloc(PC_COUNT, MODULE_NAME": bad transfer")};
perf_counter_t _reset_perf{perf_alloc(PC_COUNT, MODULE_NAME": reset")};
perf_counter_t _false_motion_perf{perf_alloc(PC_COUNT, MODULE_NAME": false motion report")};
perf_counter_t _mode_change_bright_perf{perf_alloc(PC_COUNT, MODULE_NAME": mode change bright (0)")};
@@ -108,30 +112,23 @@ private:
perf_counter_t _mode_change_super_low_light_perf{perf_alloc(PC_COUNT, MODULE_NAME": mode change super low light (2)")};
perf_counter_t _no_motion_interrupt_perf{nullptr};
const spi_drdy_gpio_t _drdy_gpio;
hrt_abstime _reset_timestamp{0};
hrt_abstime _last_publish{0};
int _failure_count{0};
int _discard_reading{0};
matrix::Dcmf _rotation;
px4::atomic<hrt_abstime> _drdy_timestamp_sample{0};
bool _data_ready_interrupt_enabled{false};
int _discard_reading{3};
uint32_t _scheduled_interval_us{SAMPLE_INTERVAL_MODE_0 / 2};
Mode _mode{Mode::LowLight};
uint32_t _scheduled_interval_us{SAMPLE_INTERVAL_MODE_0};
int _bright_to_low_counter{0};
int _low_to_superlow_counter{0};
int _low_to_bright_counter{0};
int _superlow_to_low_counter{0};
px4::atomic<hrt_abstime> _drdy_timestamp_sample{0};
bool _data_ready_interrupt_enabled{false};
hrt_abstime _last_write_time{0};
hrt_abstime _last_read_time{0};
// force reset if there hasn't been valid data for an extended period (sensor could be in a bad state)
static constexpr hrt_abstime RESET_TIMEOUT_US = 3_s;
hrt_abstime _last_good_data{0};
hrt_abstime _last_reset{0};
};
src/drivers/optical_flow/paw3902/PixArt_PAW3902_Registers.hpp
+1 -1
View File
@@ -84,7 +84,7 @@ enum Register : uint8_t {
Power_Up_Reset = 0x3A,
Resolution = 0x4E,
Resolution = 0x4E,
Inverse_Product_ID = 0x5F,
};
src/drivers/optical_flow/paw3902/paw3902_main.cpp
+3 -1
View File
@@ -32,6 +32,8 @@
****************************************************************************/
#include "PAW3902.hpp"
#include <px4_platform_common/getopt.h>
#include <px4_platform_common/module.h>
void PAW3902::print_usage()
@@ -49,7 +51,7 @@ extern "C" __EXPORT int paw3902_main(int argc, char *argv[])
using ThisDriver = PAW3902;
BusCLIArguments cli{false, true};
cli.custom1 = -1;
cli.spi_mode = SPIDEV_MODE0;
cli.spi_mode = SPIDEV_MODE3;
cli.default_spi_frequency = SPI_SPEED;
while ((ch = cli.getOpt(argc, argv, "Y:")) != EOF) {