forked from Archive/PX4-Autopilot
drivers/optical_flow/paw3902: cleanup/overhaul
- 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
parent
d5839e2dd5
commit
1fbe3c4ab3
|
|
@ -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
|
||||
# modification, are permitted provided that the following conditions
|
||||
|
|
@ -38,7 +38,7 @@ px4_add_module(
|
|||
paw3902_main.cpp
|
||||
PAW3902.cpp
|
||||
PAW3902.hpp
|
||||
PixArt_PAW3902JF_Registers.hpp
|
||||
PixArt_PAW3902_Registers.hpp
|
||||
DEPENDS
|
||||
conversion
|
||||
drivers__device
|
||||
|
|
|
|||
File diff suppressed because it is too large
Load Diff
|
|
@ -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
|
||||
* modification, are permitted provided that the following conditions
|
||||
|
|
@ -34,12 +34,12 @@
|
|||
/**
|
||||
* @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
|
||||
|
||||
#include "PixArt_PAW3902JF_Registers.hpp"
|
||||
#include "PixArt_PAW3902_Registers.hpp"
|
||||
|
||||
#include <px4_platform_common/px4_config.h>
|
||||
#include <px4_platform_common/defines.h>
|
||||
|
|
@ -48,16 +48,15 @@
|
|||
#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/sensor_optical_flow.h>
|
||||
|
||||
using namespace time_literals;
|
||||
using namespace PixArt_PAW3902JF;
|
||||
using namespace PixArt_PAW3902;
|
||||
|
||||
#define DIR_WRITE(a) ((a) | (1 << 7))
|
||||
#define DIR_READ(a) ((a) & 0x7f)
|
||||
#define DIR_WRITE(a) ((a) | Bit7)
|
||||
#define DIR_READ(a) ((a) & 0x7F)
|
||||
|
||||
class PAW3902 : public device::SPI, public I2CSPIDriver<PAW3902>
|
||||
{
|
||||
|
|
@ -78,65 +77,61 @@ private:
|
|||
|
||||
int probe() override;
|
||||
|
||||
void Reset();
|
||||
|
||||
static int DataReadyInterruptCallback(int irq, void *context, void *arg);
|
||||
void DataReady();
|
||||
bool DataReadyInterruptConfigure();
|
||||
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);
|
||||
bool RegisterWriteVerified(uint8_t reg, uint8_t data, int retries = 1);
|
||||
|
||||
void EnableLed();
|
||||
|
||||
void ModeBright();
|
||||
void ModeLowLight();
|
||||
void ModeSuperLowLight();
|
||||
void Configure();
|
||||
|
||||
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)};
|
||||
|
||||
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")};
|
||||
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_super_low_light_perf{perf_alloc(PC_COUNT, MODULE_NAME": mode change super low light (2)")};
|
||||
|
||||
static constexpr uint64_t COLLECT_TIME{15000}; // 15 milliseconds, optical flow data publish rate
|
||||
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 _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)")};
|
||||
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 _no_motion_interrupt_perf{nullptr};
|
||||
|
||||
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;
|
||||
|
||||
matrix::Dcmf _rotation;
|
||||
int _discard_reading{3};
|
||||
|
||||
int _discard_reading{3};
|
||||
Mode _mode{Mode::LowLight};
|
||||
|
||||
int _flow_sum_x{0};
|
||||
int _flow_sum_y{0};
|
||||
|
||||
Mode _mode{Mode::LowLight};
|
||||
|
||||
uint32_t _scheduled_interval_us{SAMPLE_INTERVAL_MODE_1};
|
||||
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};
|
||||
|
||||
int _valid_count{0};
|
||||
|
||||
px4::atomic<hrt_abstime> _drdy_timestamp_sample{0};
|
||||
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};
|
||||
};
|
||||
|
|
|
|||
|
|
@ -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
|
||||
* modification, are permitted provided that the following conditions
|
||||
|
|
@ -33,22 +33,36 @@
|
|||
|
||||
#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 REVISION_ID = 0x01;
|
||||
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 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 PAW3902
|
||||
static constexpr uint32_t TIME_us_TSWW = 11; // actually 10.5us
|
||||
static constexpr uint32_t TIME_us_TSRAD = 2;
|
||||
// Various time delays
|
||||
static constexpr uint32_t TIME_TSWW_us = 11; // SPI Time Between Write Commands (actually 10.5us)
|
||||
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 {
|
||||
Product_ID = 0x00,
|
||||
|
|
@ -75,8 +89,8 @@ enum Register : uint8_t {
|
|||
Inverse_Product_ID = 0x5F,
|
||||
};
|
||||
|
||||
enum Product_ID_Bit : uint8_t {
|
||||
Reset = 0x5A,
|
||||
enum Motion_Bit : uint8_t {
|
||||
MOT = Bit7, // Motion since last report
|
||||
};
|
||||
|
||||
enum class Mode {
|
||||
|
|
@ -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
|
||||
* modification, are permitted provided that the following conditions
|
||||
|
|
@ -32,7 +32,7 @@
|
|||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* PAW3902 & PAW3903 Optical Flow
|
||||
* PAW3902/PAW3903 Optical Flow
|
||||
*
|
||||
* @reboot_required true
|
||||
*
|
||||
|
|
|
|||
|
|
@ -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
|
||||
* 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")) {
|
||||
return ThisDriver::module_start(cli, iterator);
|
||||
}
|
||||
|
||||
if (!strcmp(verb, "stop")) {
|
||||
} else if (!strcmp(verb, "stop")) {
|
||||
return ThisDriver::module_stop(iterator);
|
||||
}
|
||||
|
||||
if (!strcmp(verb, "status")) {
|
||||
} else if (!strcmp(verb, "status")) {
|
||||
return ThisDriver::module_status(iterator);
|
||||
}
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue