Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-27 10:17:45 +08:00
Code Issues Actions 21 Packages Projects Releases Wiki Activity

drivers/optical_flow/paw3902: cleanup/overhaul

Browse Source 
- remove internal accumulation and publish every valid raw sample synchronized with sensor
 - store timestamp_sample from motion interrupt
 - improve timing requirements from datasheet (minimum delays after register read/write)
This commit is contained in:
Daniel Agar
2022-06-17 16:16:58 -04:00
parent d5839e2dd5
commit 1fbe3c4ab3
6 changed files with 553 additions and 557 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/optical_flow/paw3902/CMakeLists.txt
+2 -2
View File
@@ -1,6 +1,6 @@
############################################################################ ############################################################################
# #
# Copyright (c) 2019-2021 PX4 Development Team. All rights reserved. # Copyright (c) 2019-2022 PX4 Development Team. All rights reserved.
# #
# Redistribution and use in source and binary forms, with or without # Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions # modification, are permitted provided that the following conditions
@@ -38,7 +38,7 @@ px4_add_module(
paw3902_main.cpp paw3902_main.cpp
PAW3902.cpp PAW3902.cpp
PAW3902.hpp PAW3902.hpp
PixArt_PAW3902JF_Registers.hpp PixArt_PAW3902_Registers.hpp
DEPENDS DEPENDS
conversion conversion
drivers__device drivers__device
src/drivers/optical_flow/paw3902/PAW3902.cpp
+485 -496
View File
File diff suppressed because it is too large Load Diff
src/drivers/optical_flow/paw3902/PAW3902.hpp
+35 -40
View File
@@ -1,6 +1,6 @@
/**************************************************************************** /****************************************************************************
* *
* Copyright (c) 2019 PX4 Development Team. All rights reserved. * Copyright (c) 2019-2022 PX4 Development Team. All rights reserved.
* *
* Redistribution and use in source and binary forms, with or without * Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions * modification, are permitted provided that the following conditions
@@ -34,12 +34,12 @@
/** /**
* @file PAW3902.hpp * @file PAW3902.hpp
* *
* Driver for the Pixart PAW3902 & PAW3903 optical flow sensors connected via SPI. * Driver for the PAW3902JF-TXQT: Optical Motion Tracking Chip
*/ */
#pragma once #pragma once
#include "PixArt_PAW3902JF_Registers.hpp" #include "PixArt_PAW3902_Registers.hpp"
#include <px4_platform_common/px4_config.h> #include <px4_platform_common/px4_config.h>
#include <px4_platform_common/defines.h> #include <px4_platform_common/defines.h>
@@ -48,16 +48,15 @@
#include <drivers/device/spi.h> #include <drivers/device/spi.h>
#include <conversion/rotation.h> #include <conversion/rotation.h>
#include <lib/perf/perf_counter.h> #include <lib/perf/perf_counter.h>
#include <lib/parameters/param.h>
#include <drivers/drv_hrt.h> #include <drivers/drv_hrt.h>
#include <uORB/PublicationMulti.hpp> #include <uORB/PublicationMulti.hpp>
#include <uORB/topics/sensor_optical_flow.h> #include <uORB/topics/sensor_optical_flow.h>
using namespace time_literals; using namespace time_literals;
using namespace PixArt_PAW3902JF; using namespace PixArt_PAW3902;
#define DIR_WRITE(a) ((a) | (1 << 7)) #define DIR_WRITE(a) ((a) | Bit7)
#define DIR_READ(a) ((a) & 0x7f) #define DIR_READ(a) ((a) & 0x7F)
class PAW3902 : public device::SPI, public I2CSPIDriver<PAW3902> class PAW3902 : public device::SPI, public I2CSPIDriver<PAW3902>
{ {
@@ -78,65 +77,61 @@ private:
int probe() override; int probe() override;
void Reset();
static int DataReadyInterruptCallback(int irq, void *context, void *arg); static int DataReadyInterruptCallback(int irq, void *context, void *arg);
void DataReady(); void DataReady();
bool DataReadyInterruptConfigure(); bool DataReadyInterruptConfigure();
bool DataReadyInterruptDisable(); bool DataReadyInterruptDisable();
uint8_t RegisterRead(uint8_t reg, int retries = 2); uint8_t RegisterRead(uint8_t reg);
void RegisterWrite(uint8_t reg, uint8_t data); void RegisterWrite(uint8_t reg, uint8_t data);
bool RegisterWriteVerified(uint8_t reg, uint8_t data, int retries = 1);
void EnableLed(); void Configure();
void ModeBright();
void ModeLowLight();
void ModeSuperLowLight();
bool ChangeMode(Mode newMode, bool force = false); bool ChangeMode(Mode newMode, bool force = false);
void ResetAccumulatedData(); void ConfigureModeBright();
void ConfigureModeLowLight();
void ConfigureModeSuperLowLight();
void EnableLed();
uORB::PublicationMulti<sensor_optical_flow_s> _sensor_optical_flow_pub{ORB_ID(sensor_optical_flow)}; uORB::PublicationMulti<sensor_optical_flow_s> _sensor_optical_flow_pub{ORB_ID(sensor_optical_flow)};
perf_counter_t _sample_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": read")}; perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")};
perf_counter_t _interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": interval")}; 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 _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 _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")}; perf_counter_t _mode_change_bright_perf{perf_alloc(PC_COUNT, MODULE_NAME": mode change bright (0)")};
perf_counter_t _mode_change_bright_perf{perf_alloc(PC_COUNT, MODULE_NAME": mode change bright (0)")}; perf_counter_t _mode_change_low_light_perf{perf_alloc(PC_COUNT, MODULE_NAME": mode change low light (1)")};
perf_counter_t _mode_change_low_light_perf{perf_alloc(PC_COUNT, MODULE_NAME": mode change low light (1)")}; perf_counter_t _mode_change_super_low_light_perf{perf_alloc(PC_COUNT, MODULE_NAME": mode change super low light (2)")};
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};
static constexpr uint64_t COLLECT_TIME{15000}; // 15 milliseconds, optical flow data publish rate
const spi_drdy_gpio_t _drdy_gpio; const spi_drdy_gpio_t _drdy_gpio;
uint64_t _previous_collect_timestamp{0}; matrix::Dcmf _rotation;
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 _discard_reading{3}; Mode _mode{Mode::LowLight};
int _flow_sum_x{0}; uint32_t _scheduled_interval_us{SAMPLE_INTERVAL_MODE_0};
int _flow_sum_y{0};
Mode _mode{Mode::LowLight};
uint32_t _scheduled_interval_us{SAMPLE_INTERVAL_MODE_1};
int _bright_to_low_counter{0}; int _bright_to_low_counter{0};
int _low_to_superlow_counter{0}; int _low_to_superlow_counter{0};
int _low_to_bright_counter{0}; int _low_to_bright_counter{0};
int _superlow_to_low_counter{0}; int _superlow_to_low_counter{0};
int _valid_count{0}; px4::atomic<hrt_abstime> _drdy_timestamp_sample{0};
bool _data_ready_interrupt_enabled{false}; bool _data_ready_interrupt_enabled{false};
hrt_abstime _last_good_publish{0}; 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}; hrt_abstime _last_reset{0};
}; };
src/drivers/optical_flow/paw3902/PixArt_PAW3902JF_Registers.hpp → src/drivers/optical_flow/paw3902/PixArt_PAW3902_Registers.hpp
+26 -12
View File
@@ -1,6 +1,6 @@
/**************************************************************************** /****************************************************************************
* *
* Copyright (c) 2019-2021 PX4 Development Team. All rights reserved. * Copyright (c) 2019-2022 PX4 Development Team. All rights reserved.
* *
* Redistribution and use in source and binary forms, with or without * Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions * modification, are permitted provided that the following conditions
@@ -33,22 +33,36 @@
#pragma once #pragma once
namespace PixArt_PAW3902JF #include <cstdint>
// 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);
namespace PixArt_PAW3902
{ {
static constexpr uint8_t PRODUCT_ID = 0x49; static constexpr uint8_t PRODUCT_ID = 0x49;
static constexpr uint8_t REVISION_ID = 0x01; static constexpr uint8_t REVISION_ID = 0x01;
static constexpr uint8_t PRODUCT_ID_INVERSE = 0xB6; 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_0{1000000 / 126}; // 126 fps
static constexpr uint32_t SAMPLE_INTERVAL_MODE_1{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 SAMPLE_INTERVAL_MODE_2{1000000 / 50}; // 50 fps
static constexpr uint32_t SPI_SPEED = 2 * 1000 * 1000; // 2MHz SPI serial interface static constexpr uint32_t SPI_SPEED = 2 * 1000 * 1000; // 2MHz SPI serial interface
// Various time delay needed for PAW3902 // Various time delays
static constexpr uint32_t TIME_us_TSWW = 11; // actually 10.5us static constexpr uint32_t TIME_TSWW_us = 11; // SPI Time Between Write Commands (actually 10.5us)
static constexpr uint32_t TIME_us_TSRAD = 2; static constexpr uint32_t TIME_TSWR_us = 6; // SPI Time Between Write and Read Commands
static constexpr uint32_t TIME_TSRW_TSRR_us = 2; // SPI Time Between Read And Subsequent Commands (actually 1.5us)
static constexpr uint32_t TIME_TSRAD_us = 2; // SPI Read Address-Data Delay
enum Register : uint8_t { enum Register : uint8_t {
Product_ID = 0x00, Product_ID = 0x00,
@@ -75,8 +89,8 @@ enum Register : uint8_t {
Inverse_Product_ID = 0x5F, Inverse_Product_ID = 0x5F,
}; };
enum Product_ID_Bit : uint8_t { enum Motion_Bit : uint8_t {
Reset = 0x5A, MOT = Bit7, // Motion since last report
}; };
enum class Mode { enum class Mode {
src/drivers/optical_flow/paw3902/parameters.c
+2 -2
View File
@@ -1,6 +1,6 @@
/**************************************************************************** /****************************************************************************
* *
* Copyright (c) 2019 PX4 Development Team. All rights reserved. * Copyright (c) 2019-2022 PX4 Development Team. All rights reserved.
* *
* Redistribution and use in source and binary forms, with or without * Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions * modification, are permitted provided that the following conditions
@@ -32,7 +32,7 @@
****************************************************************************/ ****************************************************************************/
/** /**
* PAW3902 & PAW3903 Optical Flow * PAW3902/PAW3903 Optical Flow
* *
* @reboot_required true * @reboot_required true
* *
src/drivers/optical_flow/paw3902/paw3902_main.cpp
+3 -5
View File
@@ -1,6 +1,6 @@
/**************************************************************************** /****************************************************************************
* *
* Copyright (c) 2019-2021 PX4 Development Team. All rights reserved. * Copyright (c) 2019-2022 PX4 Development Team. All rights reserved.
* *
* Redistribution and use in source and binary forms, with or without * Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions * modification, are permitted provided that the following conditions
@@ -71,13 +71,11 @@ extern "C" __EXPORT int paw3902_main(int argc, char *argv[])
if (!strcmp(verb, "start")) { if (!strcmp(verb, "start")) {
return ThisDriver::module_start(cli, iterator); return ThisDriver::module_start(cli, iterator);
}
if (!strcmp(verb, "stop")) { } else if (!strcmp(verb, "stop")) {
return ThisDriver::module_stop(iterator); return ThisDriver::module_stop(iterator);
}
if (!strcmp(verb, "status")) { } else if (!strcmp(verb, "status")) {
return ThisDriver::module_status(iterator); return ThisDriver::module_status(iterator);
} }