fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-03 14:08:52 +08:00
Code Issues 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-09-20 18:59:00 -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
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
@@ -62,9 +62,8 @@ PAA3905::PAA3905(const I2CSPIDriverConfig &config) :
PAA3905::~PAA3905()
{
// free perf counters
perf_free(_cycle_perf);
perf_free(_interval_perf);
perf_free(_bad_register_perf);
perf_free(_bad_transfer_perf);
perf_free(_reset_perf);
perf_free(_false_motion_perf);
perf_free(_mode_change_bright_perf);
@@ -75,14 +74,44 @@ PAA3905::~PAA3905()
int PAA3905::init()
{
/* do SPI init (and probe) first */
if (SPI::init() != OK) {
return PX4_ERROR;
int ret = SPI::init();
if (ret != PX4_OK) {
DEVICE_DEBUG("SPI::init failed (%i)", ret);
return ret;
}
Configure();
return Reset() ? 0 : -1;
}
return PX4_OK;
bool PAA3905::Reset()
{
_state = STATE::RESET;
DataReadyInterruptDisable();
_drdy_timestamp_sample.store(0);
ScheduleClear();
ScheduleNow();
return true;
}
void PAA3905::exit_and_cleanup()
{
DataReadyInterruptDisable();
I2CSPIDriverBase::exit_and_cleanup();
}
void PAA3905::print_status()
{
I2CSPIDriverBase::print_status();
perf_print_counter(_bad_register_perf);
perf_print_counter(_bad_transfer_perf);
perf_print_counter(_reset_perf);
perf_print_counter(_false_motion_perf);
perf_print_counter(_mode_change_bright_perf);
perf_print_counter(_mode_change_low_light_perf);
perf_print_counter(_mode_change_super_low_light_perf);
perf_print_counter(_no_motion_interrupt_perf);
}
int PAA3905::probe()
@@ -93,17 +122,17 @@ int PAA3905::probe()
const uint8_t Inverse_Product_ID = RegisterRead(Register::Inverse_Product_ID);
if (Product_ID != PRODUCT_ID) {
PX4_ERR("Product_ID: %X", Product_ID);
DEVICE_DEBUG("unexpected Product_ID 0x%02x", Product_ID);
break;
}
if (Revision_ID != REVISION_ID) {
PX4_ERR("Revision_ID: %X", Revision_ID);
DEVICE_DEBUG("unexpected Revision_ID 0x%02x", Revision_ID);
break;
}
if (Inverse_Product_ID != PRODUCT_ID_INVERSE) {
PX4_ERR("Inverse_Product_ID: %X", Inverse_Product_ID);
DEVICE_DEBUG("unexpected Inverse_Product_ID 0x%02x", Inverse_Product_ID);
break;
}
@@ -113,95 +142,322 @@ int PAA3905::probe()
return PX4_ERROR;
}
int PAA3905::DataReadyInterruptCallback(int irq, void *context, void *arg)
void PAA3905::RunImpl()
{
static_cast<PAA3905 *>(arg)->DataReady();
return 0;
}
const hrt_abstime now = hrt_absolute_time();
void PAA3905::DataReady()
{
_drdy_timestamp_sample.store(hrt_absolute_time());
ScheduleNow();
}
switch (_state) {
case STATE::RESET:
// Issue a soft reset
RegisterWrite(Register::Power_Up_Reset, 0x5A);
bool PAA3905::DataReadyInterruptConfigure()
{
if (_drdy_gpio == 0) {
_data_ready_interrupt_enabled = false;
return false;
_reset_timestamp = now;
_failure_count = 0;
_state = STATE::WAIT_FOR_RESET;
perf_count(_reset_perf);
ScheduleDelayed(1_ms);
break;
case STATE::WAIT_FOR_RESET:
if (RegisterRead(Register::Product_ID) == PRODUCT_ID) {
// Read from registers 0x02, 0x03, 0x04, 0x05 and 0x06 one time regardless of the motion pin state.
RegisterRead(0x02);
RegisterRead(0x03);
RegisterRead(0x04);
RegisterRead(0x05);
RegisterRead(0x06);
_discard_reading = 3;
// if reset succeeded then configure
_state = STATE::CONFIGURE;
ScheduleNow();
} else {
// RESET not complete
if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) {
PX4_DEBUG("Reset failed, retrying");
_state = STATE::RESET;
ScheduleDelayed(100_ms);
} else {
PX4_DEBUG("Reset not complete, check again in 100 ms");
ScheduleDelayed(100_ms);
}
}
break;
case STATE::CONFIGURE:
if (Configure()) {
// if configure succeeded then start measurement cycle
_state = STATE::READ;
if (DataReadyInterruptConfigure()) {
_data_ready_interrupt_enabled = true;
// backup schedule as a watchdog timeout
ScheduleDelayed(1_s);
} else {
_data_ready_interrupt_enabled = false;
ScheduleOnInterval(_scheduled_interval_us, _scheduled_interval_us);
}
} else {
// CONFIGURE not complete
if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) {
PX4_DEBUG("Configure failed, resetting");
_state = STATE::RESET;
} else {
PX4_DEBUG("Configure failed, retrying");
}
ScheduleDelayed(100_ms);
}
break;
case STATE::READ: {
hrt_abstime timestamp_sample = now;
if (_data_ready_interrupt_enabled) {
// scheduled from interrupt if _drdy_timestamp_sample was set as expected
const hrt_abstime drdy_timestamp_sample = _drdy_timestamp_sample.fetch_and(0);
if (now < drdy_timestamp_sample + _scheduled_interval_us) {
timestamp_sample = drdy_timestamp_sample;
} else {
perf_count(_no_motion_interrupt_perf);
}
// push backup schedule back
ScheduleDelayed(1_s);
}
struct TransferBuffer {
uint8_t cmd = Register::Motion_Burst;
BURST_TRANSFER data{};
} buffer{};
static_assert(sizeof(buffer) == (14 + 1));
bool success = false;
if (transfer((uint8_t *)&buffer, (uint8_t *)&buffer, sizeof(buffer)) == 0) {
hrt_store_absolute_time(&_last_read_time);
if (_discard_reading > 0) {
_discard_reading--;
}
if (buffer.data.RawData_Sum > 0x98) {
perf_count(_bad_register_perf);
PX4_ERR("invalid RawData_Sum > 0x98");
}
// Number of Features = SQUAL * 4
// RawData_Sum maximum register value is 0x98
bool data_valid = (buffer.data.SQUAL > 0)
&& (buffer.data.RawData_Sum <= 0x98)
&& (_discard_reading == 0);
// Bit [5:0] check if chip is working correctly
// 0x3F: chip is working correctly
if ((buffer.data.Observation & 0x3F) != 0x3F) {
// Other value: recommend to issue a software reset
perf_count(_bad_register_perf);
PX4_ERR("Observation not equal to 0x3F, resetting");
Reset();
return;
} else {
// Observation: check mode
const Mode prev_mode = _mode;
// Bit [7:6] AMS mode
const uint8_t ams_mode = (buffer.data.Observation & (Bit7 | Bit6)) >> 6;
if (ams_mode == 0x0) {
// Mode 0 (Bright)
if (_mode != Mode::Bright) {
_mode = Mode::Bright;
_scheduled_interval_us = SAMPLE_INTERVAL_MODE_0 / 2;
perf_count(_mode_change_bright_perf);
}
} else if (ams_mode == 0x1) {
// Mode 1 (LowLight)
if (_mode != Mode::LowLight) {
_mode = Mode::LowLight;
_scheduled_interval_us = SAMPLE_INTERVAL_MODE_1 / 2;
perf_count(_mode_change_low_light_perf);
}
} else if (ams_mode == 0x2) {
// Mode 2 (SuperLowLight)
if (_mode != Mode::SuperLowLight) {
_mode = Mode::SuperLowLight;
_scheduled_interval_us = SAMPLE_INTERVAL_MODE_2 / 2;
perf_count(_mode_change_super_low_light_perf);
}
} else {
perf_count(_bad_register_perf);
PX4_ERR("invalid mode (%d) Observation: %X", ams_mode, buffer.data.Observation);
Reset();
return;
}
if (prev_mode != _mode) {
// update scheduling on mode change
if (!_data_ready_interrupt_enabled) {
ScheduleOnInterval(_scheduled_interval_us, _scheduled_interval_us);
}
}
}
if (buffer.data.Motion & Motion_Bit::ChallengingSurface) {
PX4_WARN("challenging surface detected");
}
// publish sensor_optical_flow
sensor_optical_flow_s sensor_optical_flow{};
sensor_optical_flow.timestamp_sample = timestamp_sample;
sensor_optical_flow.device_id = get_device_id();
sensor_optical_flow.error_count = perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf);
// set specs according to datasheet
sensor_optical_flow.max_flow_rate = 7.4f; // Datasheet: 7.4 rad/s
sensor_optical_flow.min_ground_distance = 0.08f; // Datasheet: 80mm
sensor_optical_flow.max_ground_distance = INFINITY; // Datasheet: infinity
// check SQUAL & Shutter values
// To suppress false motion reports, discard Delta X and Delta Y values if the SQUAL and Shutter values meet the condition
// Bright Mode, SQUAL < 0x19, Shutter ≥ 0x00FF80
// Low Light Mode, SQUAL < 0x46, Shutter ≥ 0x00FF80
// Super Low Light Mode, SQUAL < 0x55, Shutter ≥ 0x025998
// 23-bit Shutter register
const uint8_t shutter_lower = buffer.data.Shutter_Lower;
const uint8_t shutter_middle = buffer.data.Shutter_Middle;
const uint8_t shutter_upper = buffer.data.Shutter_Upper & (Bit6 | Bit5 | Bit4 | Bit3 | Bit2 | Bit1 | Bit0);
const uint32_t shutter = (shutter_upper << 16) | (shutter_middle << 8) | shutter_lower;
switch (_mode) {
case Mode::Bright:
sensor_optical_flow.integration_timespan_us = SAMPLE_INTERVAL_MODE_0;
sensor_optical_flow.mode = sensor_optical_flow_s::MODE_BRIGHT;
// quality < 25 (0x19) and shutter >= 0x00FF80
if ((buffer.data.SQUAL < 0x19) && (shutter >= 0x00FF80)) {
// false motion report, discarding
data_valid = false;
perf_count(_false_motion_perf);
}
break;
case Mode::LowLight:
sensor_optical_flow.integration_timespan_us = SAMPLE_INTERVAL_MODE_1;
sensor_optical_flow.mode = sensor_optical_flow_s::MODE_LOWLIGHT;
// quality < 70 (0x46) and shutter >= 0x00FF80
if ((buffer.data.SQUAL < 0x46) && (shutter >= 0x00FF80)) {
// false motion report, discarding
data_valid = false;
perf_count(_false_motion_perf);
}
break;
case Mode::SuperLowLight:
sensor_optical_flow.integration_timespan_us = SAMPLE_INTERVAL_MODE_2;
sensor_optical_flow.mode = sensor_optical_flow_s::MODE_SUPER_LOWLIGHT;
// quality < 85 (0x55) and shutter >= 0x025998
if ((buffer.data.SQUAL < 0x55) && (shutter >= 0x025998)) {
// false motion report, discarding
data_valid = false;
perf_count(_false_motion_perf);
}
break;
}
// motion in burst transfer
const bool motion_reported = (buffer.data.Motion & Motion_Bit::MotionOccurred);
if (data_valid) {
if (motion_reported) {
// only populate flow if data valid (motion and quality > 0)
const int16_t delta_x_raw = combine(buffer.data.Delta_X_H, buffer.data.Delta_X_L);
const int16_t delta_y_raw = combine(buffer.data.Delta_Y_H, buffer.data.Delta_Y_L);
// rotate measurements in yaw from sensor frame to body frame
const matrix::Vector3f pixel_flow_rotated = _rotation * matrix::Vector3f{(float)delta_x_raw, (float)delta_y_raw, 0.f};
// datasheet provides 11.914 CPI (count per inch) scaling per meter of height
static constexpr float PIXART_RESOLUTION = 11.914f; // counts per inch (CPI) per meter (from surface)
static constexpr float INCHES_PER_METER = 39.3701f;
// CPI/m -> radians
static constexpr float SCALE = 1.f / (PIXART_RESOLUTION * INCHES_PER_METER);
sensor_optical_flow.pixel_flow[0] = pixel_flow_rotated(0) * SCALE;
sensor_optical_flow.pixel_flow[1] = pixel_flow_rotated(1) * SCALE;
sensor_optical_flow.quality = buffer.data.SQUAL;
}
// only publish when there's motion or at least every second
if (motion_reported || (hrt_elapsed_time(&_last_publish) >= 1_s)) {
sensor_optical_flow.timestamp = hrt_absolute_time();
_sensor_optical_flow_pub.publish(sensor_optical_flow);
_last_publish = sensor_optical_flow.timestamp;
}
success = true;
if (_failure_count > 0) {
_failure_count--;
}
}
} else {
perf_count(_bad_transfer_perf);
}
if (!success) {
_failure_count++;
// full reset if things are failing consistently
if (_failure_count > 10) {
Reset();
}
}
}
break;
}
// Setup data ready on falling edge
if (px4_arch_gpiosetevent(_drdy_gpio, false, true, true, &DataReadyInterruptCallback, this) == 0) {
_data_ready_interrupt_enabled = true;
return true;
}
_data_ready_interrupt_enabled = false;
return false;
}
bool PAA3905::DataReadyInterruptDisable()
bool PAA3905::Configure()
{
_data_ready_interrupt_enabled = false;
if (_drdy_gpio == 0) {
return false;
}
return px4_arch_gpiosetevent(_drdy_gpio, false, false, false, nullptr, nullptr) == 0;
}
void PAA3905::exit_and_cleanup()
{
DataReadyInterruptDisable();
I2CSPIDriverBase::exit_and_cleanup();
}
void PAA3905::Reset()
{
perf_count(_reset_perf);
DataReadyInterruptDisable();
ScheduleClear();
// Issue a soft reset
RegisterWrite(Register::Power_Up_Reset, 0x5A);
px4_usleep(1000);
_last_reset = hrt_absolute_time();
_discard_reading = 3;
// Read from registers 0x02, 0x03, 0x04, 0x05 and 0x06 one time regardless of the motion pin state.
RegisterRead(0x02);
RegisterRead(0x03);
RegisterRead(0x04);
RegisterRead(0x05);
RegisterRead(0x06);
}
void PAA3905::Configure()
{
Reset();
ConfigureStandardDetectionSetting();
// ConfigureEnhancedDetectionMode();
ConfigureAutomaticModeSwitching();
EnableLed();
// Read Register 0x15. Check Bit [7:6] for AMS mode
const uint8_t Observation = RegisterRead(Register::Observation);
UpdateMode(Observation);
if (DataReadyInterruptConfigure()) {
// backup schedule
ScheduleDelayed(500_ms);
} else {
ScheduleOnInterval(_scheduled_interval_us, _scheduled_interval_us);
}
return true;
}
void PAA3905::ConfigureStandardDetectionSetting()
@@ -350,7 +606,6 @@ void PAA3905::ConfigureAutomaticModeSwitching()
RegisterWrite(0x68, 0x02);
RegisterWrite(0x7F, 0x00);
// TODO: for mode 0 and 1 only
// Automatic switching between Mode 0 and 1 only:
// RegisterWrite(0x7F, 0x08);
// RegisterWrite(0x68, 0x01); // different than mode 0,1,2
@@ -365,45 +620,35 @@ void PAA3905::EnableLed()
RegisterWrite(0x7F, 0x00);
}
bool PAA3905::UpdateMode(const uint8_t observation)
int PAA3905::DataReadyInterruptCallback(int irq, void *context, void *arg)
{
bool mode_changed = false;
static_cast<PAA3905 *>(arg)->DataReady();
return 0;
}
// Bit [7:6] AMS mode
const uint8_t ams_mode = (Observation & (Bit7 | Bit6)) >> 5;
void PAA3905::DataReady()
{
_drdy_timestamp_sample.store(hrt_absolute_time());
ScheduleNow();
}
if (ams_mode == 0x0) {
// Mode 0 (Bright)
if (_mode != Mode::Bright) {
mode_changed = true;
perf_count(_mode_change_bright_perf);
}
_mode = Mode::Bright;
_scheduled_interval_us = SAMPLE_INTERVAL_MODE_0;
} else if (ams_mode == 0x1) {
// Mode 1 (LowLight)
if (_mode != Mode::LowLight) {
mode_changed = true;
perf_count(_mode_change_low_light_perf);
}
_mode = Mode::LowLight;
_scheduled_interval_us = SAMPLE_INTERVAL_MODE_1;
} else if (ams_mode == 0x2) {
// Mode 2 (SuperLowLight)
if (_mode != Mode::SuperLowLight) {
mode_changed = true;
perf_count(_mode_change_super_low_light_perf);
}
_mode = Mode::SuperLowLight;
_scheduled_interval_us = SAMPLE_INTERVAL_MODE_2;
bool PAA3905::DataReadyInterruptConfigure()
{
if (_drdy_gpio == 0) {
return false;
}
return mode_changed;
// Setup data ready on falling edge
return (px4_arch_gpiosetevent(_drdy_gpio, false, true, false, &DataReadyInterruptCallback, this) == 0);
}
bool PAA3905::DataReadyInterruptDisable()
{
if (_drdy_gpio == 0) {
return false;
}
return (px4_arch_gpiosetevent(_drdy_gpio, false, false, false, nullptr, nullptr) == 0);
}
uint8_t PAA3905::RegisterRead(uint8_t reg)
@@ -446,195 +691,3 @@ void PAA3905::RegisterWrite(uint8_t reg, uint8_t data)
transfer(&cmd[0], nullptr, sizeof(cmd));
hrt_store_absolute_time(&_last_write_time);
}
void PAA3905::RunImpl()
{
perf_begin(_cycle_perf);
perf_count(_interval_perf);
const hrt_abstime now = hrt_absolute_time();
// force reconfigure if we haven't received valid data for quite some time
if ((now > _last_good_data + RESET_TIMEOUT_US) && (now > _last_reset + RESET_TIMEOUT_US)) {
Configure();
perf_end(_cycle_perf);
return;
}
hrt_abstime timestamp_sample = now;
if (_data_ready_interrupt_enabled) {
// scheduled from interrupt if _drdy_timestamp_sample was set as expected
const hrt_abstime drdy_timestamp_sample = _drdy_timestamp_sample.fetch_and(0);
if (now < drdy_timestamp_sample + _scheduled_interval_us) {
timestamp_sample = drdy_timestamp_sample;
} else {
perf_count(_no_motion_interrupt_perf);
}
// push backup schedule back
ScheduleDelayed(500_ms);
}
struct TransferBuffer {
uint8_t cmd = Register::Motion_Burst;
BURST_TRANSFER data{};
} buf{};
static_assert(sizeof(buf) == (14 + 1));
if (transfer((uint8_t *)&buf, (uint8_t *)&buf, sizeof(buf)) != 0) {
perf_end(_cycle_perf);
return;
}
hrt_store_absolute_time(&_last_read_time);
if (_discard_reading > 0) {
_discard_reading--;
perf_end(_cycle_perf);
return;
}
// Bit [5:0] check if chip is working correctly
// 0x3F: chip is working correctly
if ((buf.data.Observation & (Bit5 | Bit4 | Bit3 | Bit2 | Bit1 | Bit0)) != 0x3F) {
// Other value: recommend to issue a software reset
Configure();
perf_end(_cycle_perf);
return;
}
if (UpdateMode(buf.data.Observation)) {
// update scheduling if mode changed
if (!_data_ready_interrupt_enabled) {
ScheduleOnInterval(_scheduled_interval_us, _scheduled_interval_us);
}
}
// check SQUAL & Shutter values
// To suppress false motion reports, discard Delta X and Delta Y values if the SQUAL and Shutter values meet the condition
// Bright Mode, SQUAL < 0x19, Shutter ≥ 0x00FF80
// Low Light Mode, SQUAL < 0x46, Shutter ≥ 0x00FF80
// Super Low Light Mode, SQUAL < 0x55, Shutter ≥ 0x025998
// 23-bit Shutter register
const uint8_t Shutter_Lower = buf.data.Shutter_Lower;
const uint8_t Shutter_Middle = buf.data.Shutter_Middle;
const uint8_t Shutter_Upper = buf.data.Shutter_Upper & (Bit6 | Bit5 | Bit4 | Bit3 | Bit2 | Bit1 | Bit0);
const uint32_t shutter = (Shutter_Upper << 16) | (Shutter_Middle << 8) | Shutter_Lower;
// Motion since last report and Surface quality non-zero
const bool motion_detected = buf.data.Motion & Motion_Bit::MotionOccurred;
// Number of Features = SQUAL * 4
bool data_valid = (buf.data.SQUAL > 0);
switch (_mode) {
case Mode::Bright:
// quality < 25 (0x19) and shutter >= 0x00FF80
if ((buf.data.SQUAL < 0x19) && (shutter >= 0x00FF80)) {
// false motion report, discarding
perf_count(_false_motion_perf);
data_valid = false;
}
break;
case Mode::LowLight:
// quality < 70 (0x46) and shutter >= 0x00FF80
if ((buf.data.SQUAL < 0x46) && (shutter >= 0x00FF80)) {
// false motion report, discarding
perf_count(_false_motion_perf);
data_valid = false;
}
break;
case Mode::SuperLowLight:
// quality < 85 (0x55) and shutter >= 0x025998
if ((buf.data.SQUAL < 0x55) && (shutter >= 0x025998)) {
// false motion report, discarding
perf_count(_false_motion_perf);
data_valid = false;
}
break;
}
if (data_valid) {
// publish sensor_optical_flow
sensor_optical_flow_s report{};
report.timestamp_sample = timestamp_sample;
report.device_id = get_device_id();
report.integration_timespan_us = _scheduled_interval_us;
report.quality = buf.data.SQUAL;
// set specs according to datasheet
report.max_flow_rate = 7.4f; // Datasheet: 7.4 rad/s
report.min_ground_distance = 0.08f; // Datasheet: 80mm
report.max_ground_distance = INFINITY; // Datasheet: infinity
switch (_mode) {
case Mode::Bright:
report.mode = sensor_optical_flow_s::MODE_BRIGHT;
break;
case Mode::LowLight:
report.mode = sensor_optical_flow_s::MODE_LOWLIGHT;
break;
case Mode::SuperLowLight:
report.mode = sensor_optical_flow_s::MODE_SUPER_LOWLIGHT;
break;
}
if (motion_detected) {
// only populate flow if data valid (motion and quality > 0)
const int16_t delta_x_raw = combine(buf.data.Delta_X_H, buf.data.Delta_X_L);
const int16_t delta_y_raw = combine(buf.data.Delta_Y_H, buf.data.Delta_Y_L);
// rotate measurements in yaw from sensor frame to body frame
const matrix::Vector3f pixel_flow_rotated = _rotation * matrix::Vector3f{(float)delta_x_raw, (float)delta_y_raw, 0.f};
// datasheet provides 11.914 CPI (count per inch) scaling per meter of height
static constexpr float PIXART_RESOLUTION = 11.914f; // counts per inch (CPI) per meter (from surface)
static constexpr float INCHES_PER_METER = 39.3701f;
// CPI/m -> radians
static constexpr float SCALE = 1.f / (PIXART_RESOLUTION * INCHES_PER_METER);
report.pixel_flow[0] = pixel_flow_rotated(0) * SCALE;
report.pixel_flow[1] = pixel_flow_rotated(1) * SCALE;
}
report.timestamp = hrt_absolute_time();
_sensor_optical_flow_pub.publish(report);
if (report.quality >= 1) {
_last_good_data = report.timestamp_sample;
}
}
perf_end(_cycle_perf);
}
void PAA3905::print_status()
{
I2CSPIDriverBase::print_status();
perf_print_counter(_cycle_perf);
perf_print_counter(_interval_perf);
perf_print_counter(_reset_perf);
perf_print_counter(_false_motion_perf);
perf_print_counter(_mode_change_bright_perf);
perf_print_counter(_mode_change_low_light_perf);
perf_print_counter(_mode_change_super_low_light_perf);
perf_print_counter(_no_motion_interrupt_perf);
}
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
@@ -62,9 +62,8 @@ PAW3902::PAW3902(const I2CSPIDriverConfig &config) :
PAW3902::~PAW3902()
{
// free perf counters
perf_free(_cycle_perf);
perf_free(_interval_perf);
perf_free(_bad_register_perf);
perf_free(_bad_transfer_perf);
perf_free(_reset_perf);
perf_free(_false_motion_perf);
perf_free(_mode_change_bright_perf);
@@ -75,14 +74,44 @@ PAW3902::~PAW3902()
int PAW3902::init()
{
/* do SPI init (and probe) first */
if (SPI::init() != OK) {
return PX4_ERROR;
int ret = SPI::init();
if (ret != PX4_OK) {
DEVICE_DEBUG("SPI::init failed (%i)", ret);
return ret;
}
Configure();
return Reset() ? 0 : -1;
}
return PX4_OK;
bool PAW3902::Reset()
{
_state = STATE::RESET;
DataReadyInterruptDisable();
_drdy_timestamp_sample.store(0);
ScheduleClear();
ScheduleNow();
return true;
}
void PAW3902::exit_and_cleanup()
{
DataReadyInterruptDisable();
I2CSPIDriverBase::exit_and_cleanup();
}
void PAW3902::print_status()
{
I2CSPIDriverBase::print_status();
perf_print_counter(_bad_register_perf);
perf_print_counter(_bad_transfer_perf);
perf_print_counter(_reset_perf);
perf_print_counter(_false_motion_perf);
perf_print_counter(_mode_change_bright_perf);
perf_print_counter(_mode_change_low_light_perf);
perf_print_counter(_mode_change_super_low_light_perf);
perf_print_counter(_no_motion_interrupt_perf);
}
int PAW3902::probe()
@@ -93,17 +122,17 @@ int PAW3902::probe()
const uint8_t Inverse_Product_ID = RegisterRead(Register::Inverse_Product_ID);
if (Product_ID != PRODUCT_ID) {
PX4_ERR("Product_ID: %X", Product_ID);
DEVICE_DEBUG("unexpected Product_ID 0x%02x", Product_ID);
break;
}
if (Revision_ID != REVISION_ID) {
PX4_ERR("Revision_ID: %X", Revision_ID);
DEVICE_DEBUG("unexpected Revision_ID 0x%02x", Revision_ID);
break;
}
if (Inverse_Product_ID != PRODUCT_ID_INVERSE) {
PX4_ERR("Inverse_Product_ID: %X", Inverse_Product_ID);
DEVICE_DEBUG("unexpected Inverse_Product_ID 0x%02x", Inverse_Product_ID);
break;
}
@@ -113,125 +142,338 @@ int PAW3902::probe()
return PX4_ERROR;
}
int PAW3902::DataReadyInterruptCallback(int irq, void *context, void *arg)
void PAW3902::RunImpl()
{
static_cast<PAW3902 *>(arg)->DataReady();
return 0;
}
const hrt_abstime now = hrt_absolute_time();
void PAW3902::DataReady()
{
_drdy_timestamp_sample.store(hrt_absolute_time());
ScheduleNow();
}
switch (_state) {
case STATE::RESET:
// Issue a soft reset
RegisterWrite(Register::Power_Up_Reset, 0x5A);
bool PAW3902::DataReadyInterruptConfigure()
{
if (_drdy_gpio == 0) {
_data_ready_interrupt_enabled = false;
return false;
}
_bright_to_low_counter = 0;
_low_to_superlow_counter = 0;
_low_to_bright_counter = 0;
_superlow_to_low_counter = 0;
// Setup data ready on falling edge
if (px4_arch_gpiosetevent(_drdy_gpio, false, true, true, &DataReadyInterruptCallback, this) == 0) {
_data_ready_interrupt_enabled = true;
return true;
}
_reset_timestamp = now;
_failure_count = 0;
_state = STATE::WAIT_FOR_RESET;
perf_count(_reset_perf);
ScheduleDelayed(1_ms);
break;
_data_ready_interrupt_enabled = false;
return false;
}
case STATE::WAIT_FOR_RESET:
if (RegisterRead(Register::Product_ID) == PRODUCT_ID) {
// Read from registers 0x02, 0x03, 0x04, 0x05 and 0x06 one time regardless of the motion pin state.
RegisterRead(0x02);
RegisterRead(0x03);
RegisterRead(0x04);
RegisterRead(0x05);
RegisterRead(0x06);
bool PAW3902::DataReadyInterruptDisable()
{
_data_ready_interrupt_enabled = false;
_discard_reading = 3;
if (_drdy_gpio == 0) {
return false;
}
return px4_arch_gpiosetevent(_drdy_gpio, false, false, false, nullptr, nullptr) == 0;
}
void PAW3902::exit_and_cleanup()
{
DataReadyInterruptDisable();
I2CSPIDriverBase::exit_and_cleanup();
}
void PAW3902::Reset()
{
perf_count(_reset_perf);
DataReadyInterruptDisable();
ScheduleClear();
// Issue a soft reset
RegisterWrite(Register::Power_Up_Reset, 0x5A);
px4_usleep(1000);
_last_reset = hrt_absolute_time();
_discard_reading = 3;
_bright_to_low_counter = 0;
_low_to_superlow_counter = 0;
_low_to_bright_counter = 0;
_superlow_to_low_counter = 0;
// Read from registers 0x02, 0x03, 0x04, 0x05 and 0x06 one time regardless of the motion pin state.
RegisterRead(0x02);
RegisterRead(0x03);
RegisterRead(0x04);
RegisterRead(0x05);
RegisterRead(0x06);
}
void PAW3902::Configure()
{
Reset();
ChangeMode(Mode::LowLight, true);
}
bool PAW3902::ChangeMode(Mode newMode, bool force)
{
if (newMode != _mode || force) {
PX4_DEBUG("changing from mode %d -> %d", static_cast<int>(_mode), static_cast<int>(newMode));
Reset();
switch (newMode) {
case Mode::Bright:
ConfigureModeBright();
_scheduled_interval_us = SAMPLE_INTERVAL_MODE_0;
perf_count(_mode_change_bright_perf);
break;
case Mode::LowLight:
ConfigureModeLowLight();
_scheduled_interval_us = SAMPLE_INTERVAL_MODE_1;
perf_count(_mode_change_low_light_perf);
break;
case Mode::SuperLowLight:
ConfigureModeSuperLowLight();
_scheduled_interval_us = SAMPLE_INTERVAL_MODE_2;
perf_count(_mode_change_super_low_light_perf);
break;
}
EnableLed();
if (DataReadyInterruptConfigure()) {
// backup schedule
ScheduleDelayed(500_ms);
// if reset succeeded then configure
_state = STATE::CONFIGURE;
ScheduleNow();
} else {
ScheduleOnInterval(_scheduled_interval_us, _scheduled_interval_us);
// RESET not complete
if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) {
PX4_DEBUG("Reset failed, retrying");
_state = STATE::RESET;
ScheduleDelayed(100_ms);
} else {
PX4_DEBUG("Reset not complete, check again in 100 ms");
ScheduleDelayed(100_ms);
}
}
_mode = newMode;
break;
case STATE::CONFIGURE:
if (Configure()) {
// if configure succeeded then start measurement cycle
_state = STATE::READ;
if (DataReadyInterruptConfigure()) {
_data_ready_interrupt_enabled = true;
// backup schedule as a watchdog timeout
ScheduleDelayed(1_s);
} else {
_data_ready_interrupt_enabled = false;
ScheduleOnInterval(_scheduled_interval_us, _scheduled_interval_us);
}
} else {
// CONFIGURE not complete
if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) {
PX4_DEBUG("Configure failed, resetting");
_state = STATE::RESET;
} else {
PX4_DEBUG("Configure failed, retrying");
}
ScheduleDelayed(100_ms);
}
break;
case STATE::READ: {
hrt_abstime timestamp_sample = now;
if (_data_ready_interrupt_enabled) {
// scheduled from interrupt if _drdy_timestamp_sample was set as expected
const hrt_abstime drdy_timestamp_sample = _drdy_timestamp_sample.fetch_and(0);
if (now < drdy_timestamp_sample + _scheduled_interval_us) {
timestamp_sample = drdy_timestamp_sample;
} else {
perf_count(_no_motion_interrupt_perf);
}
// push backup schedule back
ScheduleDelayed(1_s);
}
struct TransferBuffer {
uint8_t cmd = Register::Motion_Burst;
BURST_TRANSFER data{};
} buffer{};
static_assert(sizeof(buffer) == (12 + 1));
bool success = false;
if (transfer((uint8_t *)&buffer, (uint8_t *)&buffer, sizeof(buffer)) == 0) {
hrt_store_absolute_time(&_last_read_time);
if (_discard_reading > 0) {
_discard_reading--;
}
if (buffer.data.RawData_Sum > 0x98) {
perf_count(_bad_register_perf);
PX4_ERR("invalid RawData_Sum > 0x98");
}
// Number of Features = SQUAL * 4
// RawData_Sum maximum register value is 0x98
bool data_valid = (buffer.data.SQUAL > 0)
&& (buffer.data.RawData_Sum <= 0x98)
&& (_discard_reading == 0);
// publish sensor_optical_flow
sensor_optical_flow_s sensor_optical_flow{};
sensor_optical_flow.timestamp_sample = timestamp_sample;
sensor_optical_flow.device_id = get_device_id();
sensor_optical_flow.error_count = perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf);
// set specs according to datasheet
sensor_optical_flow.max_flow_rate = 7.4f; // Datasheet: 7.4 rad/s
sensor_optical_flow.min_ground_distance = 0.08f; // Datasheet: 80mm
sensor_optical_flow.max_ground_distance = INFINITY; // Datasheet: infinity
// check SQUAL & Shutter values
// To suppress false motion reports, discard Delta X and Delta Y values if the SQUAL and Shutter values meet the condition
// Bright Mode, SQUAL < 0x19, Shutter ≥ 0x1FF0
// Low Light Mode, SQUAL < 0x46, Shutter ≥ 0x1FF0
// Super Low Light Mode, SQUAL < 0x55, Shutter ≥ 0x0BC0
// 13-bit Shutter register
const uint8_t shutter_upper = buffer.data.Shutter_Upper & (Bit4 | Bit3 | Bit2 | Bit1 | Bit0);
const uint8_t shutter_lower = buffer.data.Shutter_Lower;
const uint16_t shutter = (shutter_upper << 8) | shutter_lower;
switch (_mode) {
case Mode::Bright:
sensor_optical_flow.integration_timespan_us = SAMPLE_INTERVAL_MODE_0;
sensor_optical_flow.mode = sensor_optical_flow_s::MODE_BRIGHT;
// quality < 25 (0x19) and shutter >= 8176 (0x1FF0)
if ((buffer.data.SQUAL < 0x19) && (shutter >= 0x1FF0)) {
// false motion report, discarding
data_valid = false;
perf_count(_false_motion_perf);
}
// shutter >= 8190 (0x1FFE), raw data sum < 60 (0x3C)
if ((shutter >= 0x1FFE) && (buffer.data.RawData_Sum < 0x3C)) {
// Bright -> LowLight
_bright_to_low_counter++;
if (_bright_to_low_counter >= 10) {
_mode = Mode::LowLight;
Reset();
}
} else {
_bright_to_low_counter = 0;
}
break;
case Mode::LowLight:
sensor_optical_flow.integration_timespan_us = SAMPLE_INTERVAL_MODE_1;
sensor_optical_flow.mode = sensor_optical_flow_s::MODE_LOWLIGHT;
// quality < 70 (0x46) and shutter >= 8176 (0x1FF0)
if ((buffer.data.SQUAL < 0x46) && (shutter >= 0x1FF0)) {
// false motion report, discarding
data_valid = false;
perf_count(_false_motion_perf);
}
// shutter >= 8190 (0x1FFE) and raw data sum < 90 (0x5A)
if ((shutter >= 0x1FFE) && (buffer.data.RawData_Sum < 0x5A)) {
// LowLight -> SuperLowLight
_low_to_bright_counter = 0;
_low_to_superlow_counter++;
if (_low_to_superlow_counter >= 10) {
_mode = Mode::SuperLowLight;
Reset();
}
} else if (shutter < 0x0BB8) {
// LowLight -> Bright
// shutter < 0x0BB8 (3000)
_low_to_bright_counter++;
_low_to_superlow_counter = 0;
if (_low_to_bright_counter >= 10) {
_mode = Mode::Bright;
Reset();
}
} else {
_low_to_bright_counter = 0;
_low_to_superlow_counter = 0;
}
break;
case Mode::SuperLowLight:
sensor_optical_flow.integration_timespan_us = SAMPLE_INTERVAL_MODE_2;
sensor_optical_flow.mode = sensor_optical_flow_s::MODE_SUPER_LOWLIGHT;
// quality < 85 (0x55) and shutter >= 3008 (0x0BC0)
if ((buffer.data.SQUAL < 0x55) && (shutter >= 0x0BC0)) {
// false motion report, discarding
data_valid = false;
perf_count(_false_motion_perf);
}
// shutter < 500 (0x01F4)
if (shutter < 0x01F4) {
// should not operate with Shutter < 0x01F4 in Mode 2
_superlow_to_low_counter++;
data_valid = false;
} else if (shutter < 0x03E8) {
// SuperLowLight -> LowLight
// shutter < 1000 (0x03E8)
_superlow_to_low_counter++;
}
if (_superlow_to_low_counter >= 10) {
_mode = Mode::LowLight;
Reset();
}
break;
}
// motion in burst transfer
const bool motion_reported = (buffer.data.Motion & Motion_Bit::MOT);
if (data_valid) {
if (motion_reported) {
// only populate flow if data valid (motion and quality > 0)
const int16_t delta_x_raw = combine(buffer.data.Delta_X_H, buffer.data.Delta_X_L);
const int16_t delta_y_raw = combine(buffer.data.Delta_Y_H, buffer.data.Delta_Y_L);
// rotate measurements in yaw from sensor frame to body frame
const matrix::Vector3f pixel_flow_rotated = _rotation * matrix::Vector3f{(float)delta_x_raw, (float)delta_y_raw, 0.f};
// datasheet provides 11.914 CPI (count per inch) scaling per meter of height
static constexpr float PIXART_RESOLUTION = 11.914f; // counts per inch (CPI) per meter (from surface)
static constexpr float INCHES_PER_METER = 39.3701f;
// CPI/m -> radians
static constexpr float SCALE = 1.f / (PIXART_RESOLUTION * INCHES_PER_METER);
sensor_optical_flow.pixel_flow[0] = pixel_flow_rotated(0) * SCALE;
sensor_optical_flow.pixel_flow[1] = pixel_flow_rotated(1) * SCALE;
sensor_optical_flow.quality = buffer.data.SQUAL;
}
// only publish when there's motion or at least every second
if (motion_reported || (hrt_elapsed_time(&_last_publish) >= 1_s)) {
sensor_optical_flow.timestamp = hrt_absolute_time();
_sensor_optical_flow_pub.publish(sensor_optical_flow);
_last_publish = sensor_optical_flow.timestamp;
}
success = true;
if (_failure_count > 0) {
_failure_count--;
}
}
} else {
perf_count(_bad_transfer_perf);
}
if (!success) {
_failure_count++;
// full reset if things are failing consistently
if (_failure_count > 10) {
Reset();
}
}
}
break;
}
}
bool PAW3902::Configure()
{
switch (_mode) {
case Mode::Bright:
ConfigureModeBright();
_scheduled_interval_us = SAMPLE_INTERVAL_MODE_0 / 2;
perf_count(_mode_change_bright_perf);
break;
case Mode::LowLight:
ConfigureModeLowLight();
_scheduled_interval_us = SAMPLE_INTERVAL_MODE_1 / 2;
perf_count(_mode_change_low_light_perf);
break;
case Mode::SuperLowLight:
ConfigureModeSuperLowLight();
_scheduled_interval_us = SAMPLE_INTERVAL_MODE_2 / 2;
perf_count(_mode_change_super_low_light_perf);
break;
}
EnableLed();
return true;
}
@@ -582,6 +824,37 @@ void PAW3902::EnableLed()
RegisterWrite(0x7F, 0x00);
}
int PAW3902::DataReadyInterruptCallback(int irq, void *context, void *arg)
{
static_cast<PAW3902 *>(arg)->DataReady();
return 0;
}
void PAW3902::DataReady()
{
_drdy_timestamp_sample.store(hrt_absolute_time());
ScheduleNow();
}
bool PAW3902::DataReadyInterruptConfigure()
{
if (_drdy_gpio == 0) {
return false;
}
// Setup data ready on falling edge
return (px4_arch_gpiosetevent(_drdy_gpio, false, true, false, &DataReadyInterruptCallback, this) == 0);
}
bool PAW3902::DataReadyInterruptDisable()
{
if (_drdy_gpio == 0) {
return false;
}
return (px4_arch_gpiosetevent(_drdy_gpio, false, false, false, nullptr, nullptr) == 0);
}
uint8_t PAW3902::RegisterRead(uint8_t reg)
{
// tSWR SPI Time Between Write And Read Commands
@@ -622,239 +895,3 @@ void PAW3902::RegisterWrite(uint8_t reg, uint8_t data)
transfer(&cmd[0], nullptr, sizeof(cmd));
hrt_store_absolute_time(&_last_write_time);
}
void PAW3902::RunImpl()
{
perf_begin(_cycle_perf);
perf_count(_interval_perf);
const hrt_abstime now = hrt_absolute_time();
// force reconfigure if we haven't received valid data for quite some time
if ((now > _last_good_data + RESET_TIMEOUT_US) && (now > _last_reset + RESET_TIMEOUT_US)) {
PX4_ERR("no valid data for %.3f seconds, resetting", 1e-6 * (now - _last_good_data));
Configure();
perf_end(_cycle_perf);
return;
}
hrt_abstime timestamp_sample = now;
if (_data_ready_interrupt_enabled) {
// scheduled from interrupt if _drdy_timestamp_sample was set as expected
const hrt_abstime drdy_timestamp_sample = _drdy_timestamp_sample.fetch_and(0);
if (now < drdy_timestamp_sample + _scheduled_interval_us) {
timestamp_sample = drdy_timestamp_sample;
} else {
perf_count(_no_motion_interrupt_perf);
}
// push backup schedule back
ScheduleDelayed(500_ms);
}
struct TransferBuffer {
uint8_t cmd = Register::Motion_Burst;
BURST_TRANSFER data{};
} buf{};
static_assert(sizeof(buf) == (12 + 1));
if (transfer((uint8_t *)&buf, (uint8_t *)&buf, sizeof(buf)) != 0) {
perf_end(_cycle_perf);
return;
}
hrt_store_absolute_time(&_last_read_time);
if (_discard_reading > 0) {
_discard_reading--;
perf_end(_cycle_perf);
return;
}
// check SQUAL & Shutter values
// To suppress false motion reports, discard Delta X and Delta Y values if the SQUAL and Shutter values meet the condition
// Bright Mode, SQUAL < 0x19, Shutter ≥ 0x1FF0
// Low Light Mode, SQUAL < 0x46, Shutter ≥ 0x1FF0
// Super Low Light Mode, SQUAL < 0x55, Shutter ≥ 0x0BC0
// 13-bit Shutter register
const uint8_t Shutter_Upper = buf.data.Shutter_Upper & (Bit4 | Bit3 | Bit2 | Bit1 | Bit0);
const uint8_t Shutter_Lower = buf.data.Shutter_Lower;
const uint16_t shutter = (Shutter_Upper << 8) | Shutter_Lower;
// Motion since last report and Surface quality non-zero
const bool motion_detected = buf.data.Motion & Motion_Bit::MOT;
// Number of Features = SQUAL * 4
bool data_valid = (buf.data.SQUAL > 0);
switch (_mode) {
case Mode::Bright:
// quality < 25 (0x19) and shutter >= 8176 (0x1FF0)
if ((buf.data.SQUAL < 0x19) && (shutter >= 0x1FF0)) {
// false motion report, discarding
perf_count(_false_motion_perf);
data_valid = false;
}
// shutter >= 8190 (0x1FFE), raw data sum < 60 (0x3C)
if ((shutter >= 0x1FFE) && (buf.data.RawData_Sum < 0x3C)) {
// Bright -> LowLight
_bright_to_low_counter++;
if (_bright_to_low_counter >= 10) {
ChangeMode(Mode::LowLight);
}
} else {
_bright_to_low_counter = 0;
}
break;
case Mode::LowLight:
// quality < 70 (0x46) and shutter >= 8176 (0x1FF0)
if ((buf.data.SQUAL < 0x46) && (shutter >= 0x1FF0)) {
// false motion report, discarding
perf_count(_false_motion_perf);
data_valid = false;
}
// shutter >= 8190 (0x1FFE) and raw data sum < 90 (0x5A)
if ((shutter >= 0x1FFE) && (buf.data.RawData_Sum < 0x5A)) {
// LowLight -> SuperLowLight
_low_to_bright_counter = 0;
_low_to_superlow_counter++;
if (_low_to_superlow_counter >= 10) {
ChangeMode(Mode::SuperLowLight);
}
} else if (shutter < 0x0BB8) {
// LowLight -> Bright
// shutter < 0x0BB8 (3000)
_low_to_bright_counter++;
_low_to_superlow_counter = 0;
if (_low_to_bright_counter >= 10) {
ChangeMode(Mode::Bright);
}
} else {
_low_to_bright_counter = 0;
_low_to_superlow_counter = 0;
}
break;
case Mode::SuperLowLight:
// quality < 85 (0x55) and shutter >= 3008 (0x0BC0)
if ((buf.data.SQUAL < 0x55) && (shutter >= 0x0BC0)) {
// false motion report, discarding
perf_count(_false_motion_perf);
data_valid = false;
}
// shutter < 500 (0x01F4)
if (shutter < 0x01F4) {
// should not operate with Shutter < 0x01F4 in Mode 2
_superlow_to_low_counter++;
if (_superlow_to_low_counter >= 10) {
ChangeMode(Mode::LowLight);
}
} else if (shutter < 0x03E8) {
// SuperLowLight -> LowLight
// shutter < 1000 (0x03E8)
_superlow_to_low_counter++;
if (_superlow_to_low_counter >= 10) {
ChangeMode(Mode::LowLight);
}
} else {
_superlow_to_low_counter = 0;
}
break;
}
if (data_valid) {
// publish sensor_optical_flow
sensor_optical_flow_s report{};
report.timestamp_sample = timestamp_sample;
report.device_id = get_device_id();
report.integration_timespan_us = _scheduled_interval_us;
report.quality = buf.data.SQUAL;
// set specs according to datasheet
report.max_flow_rate = 7.4f; // Datasheet: 7.4 rad/s
report.min_ground_distance = 0.08f; // Datasheet: 80mm
report.max_ground_distance = INFINITY; // Datasheet: infinity
switch (_mode) {
case Mode::Bright:
report.mode = sensor_optical_flow_s::MODE_BRIGHT;
break;
case Mode::LowLight:
report.mode = sensor_optical_flow_s::MODE_LOWLIGHT;
break;
case Mode::SuperLowLight:
report.mode = sensor_optical_flow_s::MODE_SUPER_LOWLIGHT;
break;
}
if (motion_detected) {
// only populate flow if data valid (motion and quality > 0)
const int16_t delta_x_raw = combine(buf.data.Delta_X_H, buf.data.Delta_X_L);
const int16_t delta_y_raw = combine(buf.data.Delta_Y_H, buf.data.Delta_Y_L);
// rotate measurements in yaw from sensor frame to body frame
const matrix::Vector3f pixel_flow_rotated = _rotation * matrix::Vector3f{(float)delta_x_raw, (float)delta_y_raw, 0.f};
// datasheet provides 11.914 CPI (count per inch) scaling per meter of height
static constexpr float PIXART_RESOLUTION = 11.914f; // counts per inch (CPI) per meter (from surface)
static constexpr float INCHES_PER_METER = 39.3701f;
// CPI/m -> radians
static constexpr float SCALE = 1.f / (PIXART_RESOLUTION * INCHES_PER_METER);
report.pixel_flow[0] = pixel_flow_rotated(0) * SCALE;
report.pixel_flow[1] = pixel_flow_rotated(1) * SCALE;
}
report.timestamp = hrt_absolute_time();
_sensor_optical_flow_pub.publish(report);
if (report.quality >= 1) {
_last_good_data = report.timestamp_sample;
}
}
perf_end(_cycle_perf);
}
void PAW3902::print_status()
{
I2CSPIDriverBase::print_status();
perf_print_counter(_cycle_perf);
perf_print_counter(_interval_perf);
perf_print_counter(_reset_perf);
perf_print_counter(_false_motion_perf);
perf_print_counter(_mode_change_bright_perf);
perf_print_counter(_mode_change_low_light_perf);
perf_print_counter(_mode_change_super_low_light_perf);
perf_print_counter(_no_motion_interrupt_perf);
}
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) {