From d448a02e8e36c2866739eaec38b7b46ff9efb981 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Fri, 17 Jun 2022 16:16:58 -0400 Subject: [PATCH] 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) --- .../optical_flow/paw3902/CMakeLists.txt | 4 +- src/drivers/optical_flow/paw3902/PAW3902.cpp | 981 +++++++++--------- src/drivers/optical_flow/paw3902/PAW3902.hpp | 75 +- ...sters.hpp => PixArt_PAW3902_Registers.hpp} | 38 +- src/drivers/optical_flow/paw3902/parameters.c | 4 +- .../optical_flow/paw3902/paw3902_main.cpp | 8 +- 6 files changed, 553 insertions(+), 557 deletions(-) rename src/drivers/optical_flow/paw3902/{PixArt_PAW3902JF_Registers.hpp => PixArt_PAW3902_Registers.hpp} (71%) diff --git a/src/drivers/optical_flow/paw3902/CMakeLists.txt b/src/drivers/optical_flow/paw3902/CMakeLists.txt index 3626379ed2..2bd4380bf9 100644 --- a/src/drivers/optical_flow/paw3902/CMakeLists.txt +++ b/src/drivers/optical_flow/paw3902/CMakeLists.txt @@ -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 diff --git a/src/drivers/optical_flow/paw3902/PAW3902.cpp b/src/drivers/optical_flow/paw3902/PAW3902.cpp index 3df138a14c..4e41b091aa 100644 --- a/src/drivers/optical_flow/paw3902/PAW3902.cpp +++ b/src/drivers/optical_flow/paw3902/PAW3902.cpp @@ -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 @@ -43,6 +43,10 @@ PAW3902::PAW3902(const I2CSPIDriverConfig &config) : I2CSPIDriver(config), _drdy_gpio(config.drdy_gpio) { + if (_drdy_gpio != 0) { + _no_motion_interrupt_perf = perf_alloc(PC_COUNT, MODULE_NAME": no motion interrupt"); + } + float yaw_rotation_degrees = (float)config.custom1; if (yaw_rotation_degrees >= 0.f) { @@ -52,30 +56,21 @@ PAW3902::PAW3902(const I2CSPIDriverConfig &config) : _rotation = matrix::Dcmf{matrix::Eulerf{0.f, 0.f, math::radians(yaw_rotation_degrees)}}; } else { - // otherwise use the parameter SENS_FLOW_ROT - param_t rot = param_find("SENS_FLOW_ROT"); - int32_t val = 0; - - if (param_get(rot, &val) == PX4_OK) { - _rotation = get_rot_matrix((enum Rotation)val); - - } else { - _rotation.identity(); - } + _rotation.identity(); } } PAW3902::~PAW3902() { // free perf counters - perf_free(_sample_perf); + perf_free(_cycle_perf); perf_free(_interval_perf); - perf_free(_comms_errors); + perf_free(_reset_perf); perf_free(_false_motion_perf); - perf_free(_register_write_fail_perf); perf_free(_mode_change_bright_perf); perf_free(_mode_change_low_light_perf); perf_free(_mode_change_super_low_light_perf); + perf_free(_no_motion_interrupt_perf); } int PAW3902::init() @@ -85,38 +80,37 @@ int PAW3902::init() return PX4_ERROR; } - // force to low light mode (1) initially - ChangeMode(Mode::LowLight, true); - - _previous_collect_timestamp = hrt_absolute_time(); + Configure(); return PX4_OK; } int PAW3902::probe() { - const uint8_t Product_ID = RegisterRead(Register::Product_ID); + for (int retry = 0; retry < 3; retry++) { + const uint8_t Product_ID = RegisterRead(Register::Product_ID); + const uint8_t Revision_ID = RegisterRead(Register::Revision_ID); + const uint8_t Inverse_Product_ID = RegisterRead(Register::Inverse_Product_ID); - if (Product_ID != PRODUCT_ID) { - PX4_ERR("Product_ID: %X", Product_ID); - return PX4_ERROR; + if (Product_ID != PRODUCT_ID) { + PX4_ERR("Product_ID: %X", Product_ID); + break; + } + + if (Revision_ID != REVISION_ID) { + PX4_ERR("Revision_ID: %X", Revision_ID); + break; + } + + if (Inverse_Product_ID != PRODUCT_ID_INVERSE) { + PX4_ERR("Inverse_Product_ID: %X", Inverse_Product_ID); + break; + } + + return PX4_OK; } - const uint8_t Revision_ID = RegisterRead(Register::Revision_ID); - - if (Revision_ID != REVISION_ID) { - PX4_ERR("Revision_ID: %X", Revision_ID); - return PX4_ERROR; - } - - const uint8_t Inverse_Product_ID = RegisterRead(Register::Inverse_Product_ID); - - if (Inverse_Product_ID != PRODUCT_ID_INVERSE) { - PX4_ERR("Inverse_Product_ID: %X", Inverse_Product_ID); - return PX4_ERROR; - } - - return PX4_OK; + return PX4_ERROR; } int PAW3902::DataReadyInterruptCallback(int irq, void *context, void *arg) @@ -127,12 +121,14 @@ int PAW3902::DataReadyInterruptCallback(int irq, void *context, void *arg) void PAW3902::DataReady() { + _drdy_timestamp_sample.store(hrt_absolute_time()); ScheduleNow(); } bool PAW3902::DataReadyInterruptConfigure() { if (_drdy_gpio == 0) { + _data_ready_interrupt_enabled = false; return false; } @@ -148,12 +144,12 @@ bool PAW3902::DataReadyInterruptConfigure() bool PAW3902::DataReadyInterruptDisable() { + _data_ready_interrupt_enabled = false; + if (_drdy_gpio == 0) { return false; } - _data_ready_interrupt_enabled = false; - return px4_arch_gpiosetevent(_drdy_gpio, false, false, false, nullptr, nullptr) == 0; } @@ -163,33 +159,62 @@ void PAW3902::exit_and_cleanup() 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(_mode), static_cast(newMode)); - DataReadyInterruptDisable(); - ScheduleClear(); - // Issue a soft reset - RegisterWrite(Register::Power_Up_Reset, 0x5A); - px4_usleep(1000); - _last_reset = hrt_absolute_time(); + Reset(); switch (newMode) { case Mode::Bright: - ModeBright(); + ConfigureModeBright(); _scheduled_interval_us = SAMPLE_INTERVAL_MODE_0; perf_count(_mode_change_bright_perf); break; case Mode::LowLight: - ModeLowLight(); + ConfigureModeLowLight(); _scheduled_interval_us = SAMPLE_INTERVAL_MODE_1; perf_count(_mode_change_low_light_perf); break; case Mode::SuperLowLight: - ModeSuperLowLight(); + ConfigureModeSuperLowLight(); _scheduled_interval_us = SAMPLE_INTERVAL_MODE_2; perf_count(_mode_change_super_low_light_perf); break; @@ -197,362 +222,354 @@ bool PAW3902::ChangeMode(Mode newMode, bool force) EnableLed(); - _discard_reading = 3; - _valid_count = 0; - if (DataReadyInterruptConfigure()) { - // backup schedule as a watchdog timeout - ScheduleDelayed(_scheduled_interval_us * 2); + // backup schedule + ScheduleDelayed(500_ms); } else { - ScheduleOnInterval(_scheduled_interval_us); + ScheduleOnInterval(_scheduled_interval_us, _scheduled_interval_us); } _mode = newMode; } - _bright_to_low_counter = 0; - _low_to_superlow_counter = 0; - _low_to_bright_counter = 0; - _superlow_to_low_counter = 0; - return true; } -void PAW3902::ModeBright() +void PAW3902::ConfigureModeBright() { // Mode 0: Bright (126 fps) 60 Lux typical // set performance optimization registers - RegisterWriteVerified(0x7F, 0x00); - RegisterWriteVerified(0x55, 0x01); - RegisterWriteVerified(0x50, 0x07); - RegisterWriteVerified(0x7f, 0x0e); - RegisterWriteVerified(0x43, 0x10); + RegisterWrite(0x7F, 0x00); + RegisterWrite(0x55, 0x01); + RegisterWrite(0x50, 0x07); + RegisterWrite(0x7f, 0x0e); + RegisterWrite(0x43, 0x10); - RegisterWriteVerified(0x48, 0x02); - RegisterWriteVerified(0x7F, 0x00); - RegisterWriteVerified(0x51, 0x7b); - RegisterWriteVerified(0x50, 0x00); - RegisterWriteVerified(0x55, 0x00); + RegisterWrite(0x48, 0x02); + RegisterWrite(0x7F, 0x00); + RegisterWrite(0x51, 0x7b); + RegisterWrite(0x50, 0x00); + RegisterWrite(0x55, 0x00); - RegisterWriteVerified(0x7F, 0x00); - RegisterWriteVerified(0x61, 0xAD); - RegisterWriteVerified(0x7F, 0x03); - RegisterWriteVerified(0x40, 0x00); - RegisterWriteVerified(0x7F, 0x05); - RegisterWriteVerified(0x41, 0xB3); - RegisterWriteVerified(0x43, 0xF1); - RegisterWriteVerified(0x45, 0x14); - RegisterWriteVerified(0x5F, 0x34); - RegisterWriteVerified(0x7B, 0x08); - RegisterWriteVerified(0x5e, 0x34); - RegisterWriteVerified(0x5b, 0x32); - RegisterWriteVerified(0x6d, 0x32); - RegisterWriteVerified(0x45, 0x17); - RegisterWriteVerified(0x70, 0xe5); - RegisterWriteVerified(0x71, 0xe5); - RegisterWriteVerified(0x7F, 0x06); - RegisterWriteVerified(0x44, 0x1B); - RegisterWriteVerified(0x40, 0xBF); - RegisterWriteVerified(0x4E, 0x3F); - RegisterWriteVerified(0x7F, 0x08); - RegisterWriteVerified(0x66, 0x44); - RegisterWriteVerified(0x65, 0x20); - RegisterWriteVerified(0x6a, 0x3a); - RegisterWriteVerified(0x61, 0x05); - RegisterWriteVerified(0x62, 0x05); - RegisterWriteVerified(0x7F, 0x09); - RegisterWriteVerified(0x4F, 0xAF); - RegisterWriteVerified(0x48, 0x80); - RegisterWriteVerified(0x49, 0x80); - RegisterWriteVerified(0x57, 0x77); - RegisterWriteVerified(0x5F, 0x40); - RegisterWriteVerified(0x60, 0x78); - RegisterWriteVerified(0x61, 0x78); - RegisterWriteVerified(0x62, 0x08); - RegisterWriteVerified(0x63, 0x50); - RegisterWriteVerified(0x7F, 0x0A); - RegisterWriteVerified(0x45, 0x60); - RegisterWriteVerified(0x7F, 0x00); - RegisterWriteVerified(0x4D, 0x11); - RegisterWriteVerified(0x55, 0x80); - RegisterWriteVerified(0x74, 0x21); - RegisterWriteVerified(0x75, 0x1F); - RegisterWriteVerified(0x4A, 0x78); - RegisterWriteVerified(0x4B, 0x78); - RegisterWriteVerified(0x44, 0x08); - RegisterWriteVerified(0x45, 0x50); - RegisterWriteVerified(0x64, 0xFE); - RegisterWriteVerified(0x65, 0x1F); - RegisterWriteVerified(0x72, 0x0A); - RegisterWriteVerified(0x73, 0x00); - RegisterWriteVerified(0x7F, 0x14); - RegisterWriteVerified(0x44, 0x84); - RegisterWriteVerified(0x65, 0x47); - RegisterWriteVerified(0x66, 0x18); - RegisterWriteVerified(0x63, 0x70); - RegisterWriteVerified(0x6f, 0x2c); - RegisterWriteVerified(0x7F, 0x15); - RegisterWriteVerified(0x48, 0x48); - RegisterWriteVerified(0x7F, 0x07); - RegisterWriteVerified(0x41, 0x0D); - RegisterWriteVerified(0x43, 0x14); - RegisterWriteVerified(0x4B, 0x0E); - RegisterWriteVerified(0x45, 0x0F); - RegisterWriteVerified(0x44, 0x42); - RegisterWriteVerified(0x4C, 0x80); - RegisterWriteVerified(0x7F, 0x10); - RegisterWriteVerified(0x5B, 0x03); - RegisterWriteVerified(0x7F, 0x07); - RegisterWriteVerified(0x40, 0x41); + RegisterWrite(0x7F, 0x00); + RegisterWrite(0x61, 0xAD); + RegisterWrite(0x7F, 0x03); + RegisterWrite(0x40, 0x00); + RegisterWrite(0x7F, 0x05); + RegisterWrite(0x41, 0xB3); + RegisterWrite(0x43, 0xF1); + RegisterWrite(0x45, 0x14); + RegisterWrite(0x5F, 0x34); + RegisterWrite(0x7B, 0x08); + RegisterWrite(0x5e, 0x34); + RegisterWrite(0x5b, 0x32); + RegisterWrite(0x6d, 0x32); + RegisterWrite(0x45, 0x17); + RegisterWrite(0x70, 0xe5); + RegisterWrite(0x71, 0xe5); + RegisterWrite(0x7F, 0x06); + RegisterWrite(0x44, 0x1B); + RegisterWrite(0x40, 0xBF); + RegisterWrite(0x4E, 0x3F); + RegisterWrite(0x7F, 0x08); + RegisterWrite(0x66, 0x44); + RegisterWrite(0x65, 0x20); + RegisterWrite(0x6a, 0x3a); + RegisterWrite(0x61, 0x05); + RegisterWrite(0x62, 0x05); + RegisterWrite(0x7F, 0x09); + RegisterWrite(0x4F, 0xAF); + RegisterWrite(0x48, 0x80); + RegisterWrite(0x49, 0x80); + RegisterWrite(0x57, 0x77); + RegisterWrite(0x5F, 0x40); + RegisterWrite(0x60, 0x78); + RegisterWrite(0x61, 0x78); + RegisterWrite(0x62, 0x08); + RegisterWrite(0x63, 0x50); + RegisterWrite(0x7F, 0x0A); + RegisterWrite(0x45, 0x60); + RegisterWrite(0x7F, 0x00); + RegisterWrite(0x4D, 0x11); + RegisterWrite(0x55, 0x80); + RegisterWrite(0x74, 0x21); + RegisterWrite(0x75, 0x1F); + RegisterWrite(0x4A, 0x78); + RegisterWrite(0x4B, 0x78); + RegisterWrite(0x44, 0x08); + RegisterWrite(0x45, 0x50); + RegisterWrite(0x64, 0xFE); + RegisterWrite(0x65, 0x1F); + RegisterWrite(0x72, 0x0A); + RegisterWrite(0x73, 0x00); + RegisterWrite(0x7F, 0x14); + RegisterWrite(0x44, 0x84); + RegisterWrite(0x65, 0x47); + RegisterWrite(0x66, 0x18); + RegisterWrite(0x63, 0x70); + RegisterWrite(0x6f, 0x2c); + RegisterWrite(0x7F, 0x15); + RegisterWrite(0x48, 0x48); + RegisterWrite(0x7F, 0x07); + RegisterWrite(0x41, 0x0D); + RegisterWrite(0x43, 0x14); + RegisterWrite(0x4B, 0x0E); + RegisterWrite(0x45, 0x0F); + RegisterWrite(0x44, 0x42); + RegisterWrite(0x4C, 0x80); + RegisterWrite(0x7F, 0x10); + RegisterWrite(0x5B, 0x03); + RegisterWrite(0x7F, 0x07); + RegisterWrite(0x40, 0x41); - usleep(10_ms); // delay 10ms + px4_usleep(10'000); // delay 10ms - RegisterWriteVerified(0x7F, 0x00); - RegisterWriteVerified(0x32, 0x00); - RegisterWriteVerified(0x7F, 0x07); - RegisterWriteVerified(0x40, 0x40); - RegisterWriteVerified(0x7F, 0x06); - RegisterWriteVerified(0x68, 0x70); - RegisterWriteVerified(0x69, 0x01); - RegisterWriteVerified(0x7F, 0x0D); - RegisterWriteVerified(0x48, 0xC0); - RegisterWriteVerified(0x6F, 0xD5); - RegisterWriteVerified(0x7F, 0x00); - RegisterWriteVerified(0x5B, 0xA0); - RegisterWriteVerified(0x4E, 0xA8); - RegisterWriteVerified(0x5A, 0x50); - RegisterWriteVerified(0x40, 0x80); - RegisterWriteVerified(0x73, 0x1f); + RegisterWrite(0x7F, 0x00); + RegisterWrite(0x32, 0x00); + RegisterWrite(0x7F, 0x07); + RegisterWrite(0x40, 0x40); + RegisterWrite(0x7F, 0x06); + RegisterWrite(0x68, 0x70); + RegisterWrite(0x69, 0x01); + RegisterWrite(0x7F, 0x0D); + RegisterWrite(0x48, 0xC0); + RegisterWrite(0x6F, 0xD5); + RegisterWrite(0x7F, 0x00); + RegisterWrite(0x5B, 0xA0); + RegisterWrite(0x4E, 0xA8); + RegisterWrite(0x5A, 0x50); + RegisterWrite(0x40, 0x80); + RegisterWrite(0x73, 0x1f); - usleep(10_ms); // delay 10ms + px4_usleep(10'000); // delay 10ms RegisterWrite(0x73, 0x00); } -void PAW3902::ModeLowLight() +void PAW3902::ConfigureModeLowLight() { // Mode 1: Low Light (126 fps) 30 Lux typical // low light and low speed motion tracking // set performance optimization registers - RegisterWriteVerified(0x7F, 0x00); - RegisterWriteVerified(0x55, 0x01); - RegisterWriteVerified(0x50, 0x07); - RegisterWriteVerified(0x7f, 0x0e); - RegisterWriteVerified(0x43, 0x10); + RegisterWrite(0x7F, 0x00); + RegisterWrite(0x55, 0x01); + RegisterWrite(0x50, 0x07); + RegisterWrite(0x7f, 0x0e); + RegisterWrite(0x43, 0x10); - RegisterWriteVerified(0x48, 0x02); - RegisterWriteVerified(0x7F, 0x00); - RegisterWriteVerified(0x51, 0x7b); - RegisterWriteVerified(0x50, 0x00); - RegisterWriteVerified(0x55, 0x00); + RegisterWrite(0x48, 0x02); + RegisterWrite(0x7F, 0x00); + RegisterWrite(0x51, 0x7b); + RegisterWrite(0x50, 0x00); + RegisterWrite(0x55, 0x00); - RegisterWriteVerified(0x7F, 0x00); - RegisterWriteVerified(0x61, 0xAD); - RegisterWriteVerified(0x7F, 0x03); - RegisterWriteVerified(0x40, 0x00); - RegisterWriteVerified(0x7F, 0x05); - RegisterWriteVerified(0x41, 0xB3); - RegisterWriteVerified(0x43, 0xF1); - RegisterWriteVerified(0x45, 0x14); - RegisterWriteVerified(0x5F, 0x34); - RegisterWriteVerified(0x7B, 0x08); - RegisterWriteVerified(0x5e, 0x34); - RegisterWriteVerified(0x5b, 0x65); - RegisterWriteVerified(0x6d, 0x65); - RegisterWriteVerified(0x45, 0x17); - RegisterWriteVerified(0x70, 0xe5); - RegisterWriteVerified(0x71, 0xe5); - RegisterWriteVerified(0x7F, 0x06); - RegisterWriteVerified(0x44, 0x1B); - RegisterWriteVerified(0x40, 0xBF); - RegisterWriteVerified(0x4E, 0x3F); - RegisterWriteVerified(0x7F, 0x08); - RegisterWriteVerified(0x66, 0x44); - RegisterWriteVerified(0x65, 0x20); - RegisterWriteVerified(0x6a, 0x3a); - RegisterWriteVerified(0x61, 0x05); - RegisterWriteVerified(0x62, 0x05); - RegisterWriteVerified(0x7F, 0x09); - RegisterWriteVerified(0x4F, 0xAF); - RegisterWriteVerified(0x48, 0x80); - RegisterWriteVerified(0x49, 0x80); - RegisterWriteVerified(0x57, 0x77); - RegisterWriteVerified(0x5F, 0x40); - RegisterWriteVerified(0x60, 0x78); - RegisterWriteVerified(0x61, 0x78); - RegisterWriteVerified(0x62, 0x08); - RegisterWriteVerified(0x63, 0x50); - RegisterWriteVerified(0x7F, 0x0A); - RegisterWriteVerified(0x45, 0x60); - RegisterWriteVerified(0x7F, 0x00); - RegisterWriteVerified(0x4D, 0x11); - RegisterWriteVerified(0x55, 0x80); - RegisterWriteVerified(0x74, 0x21); - RegisterWriteVerified(0x75, 0x1F); - RegisterWriteVerified(0x4A, 0x78); - RegisterWriteVerified(0x4B, 0x78); - RegisterWriteVerified(0x44, 0x08); - RegisterWriteVerified(0x45, 0x50); - RegisterWriteVerified(0x64, 0xFE); - RegisterWriteVerified(0x65, 0x1F); - RegisterWriteVerified(0x72, 0x0A); - RegisterWriteVerified(0x73, 0x00); - RegisterWriteVerified(0x7F, 0x14); - RegisterWriteVerified(0x44, 0x84); - RegisterWriteVerified(0x65, 0x67); - RegisterWriteVerified(0x66, 0x18); - RegisterWriteVerified(0x63, 0x70); - RegisterWriteVerified(0x6f, 0x2c); - RegisterWriteVerified(0x7F, 0x15); - RegisterWriteVerified(0x48, 0x48); - RegisterWriteVerified(0x7F, 0x07); - RegisterWriteVerified(0x41, 0x0D); - RegisterWriteVerified(0x43, 0x14); - RegisterWriteVerified(0x4B, 0x0E); - RegisterWriteVerified(0x45, 0x0F); - RegisterWriteVerified(0x44, 0x42); - RegisterWriteVerified(0x4C, 0x80); - RegisterWriteVerified(0x7F, 0x10); - RegisterWriteVerified(0x5B, 0x03); - RegisterWriteVerified(0x7F, 0x07); - RegisterWriteVerified(0x40, 0x41); + RegisterWrite(0x7F, 0x00); + RegisterWrite(0x61, 0xAD); + RegisterWrite(0x7F, 0x03); + RegisterWrite(0x40, 0x00); + RegisterWrite(0x7F, 0x05); + RegisterWrite(0x41, 0xB3); + RegisterWrite(0x43, 0xF1); + RegisterWrite(0x45, 0x14); + RegisterWrite(0x5F, 0x34); + RegisterWrite(0x7B, 0x08); + RegisterWrite(0x5e, 0x34); + RegisterWrite(0x5b, 0x65); + RegisterWrite(0x6d, 0x65); + RegisterWrite(0x45, 0x17); + RegisterWrite(0x70, 0xe5); + RegisterWrite(0x71, 0xe5); + RegisterWrite(0x7F, 0x06); + RegisterWrite(0x44, 0x1B); + RegisterWrite(0x40, 0xBF); + RegisterWrite(0x4E, 0x3F); + RegisterWrite(0x7F, 0x08); + RegisterWrite(0x66, 0x44); + RegisterWrite(0x65, 0x20); + RegisterWrite(0x6a, 0x3a); + RegisterWrite(0x61, 0x05); + RegisterWrite(0x62, 0x05); + RegisterWrite(0x7F, 0x09); + RegisterWrite(0x4F, 0xAF); + RegisterWrite(0x48, 0x80); + RegisterWrite(0x49, 0x80); + RegisterWrite(0x57, 0x77); + RegisterWrite(0x5F, 0x40); + RegisterWrite(0x60, 0x78); + RegisterWrite(0x61, 0x78); + RegisterWrite(0x62, 0x08); + RegisterWrite(0x63, 0x50); + RegisterWrite(0x7F, 0x0A); + RegisterWrite(0x45, 0x60); + RegisterWrite(0x7F, 0x00); + RegisterWrite(0x4D, 0x11); + RegisterWrite(0x55, 0x80); + RegisterWrite(0x74, 0x21); + RegisterWrite(0x75, 0x1F); + RegisterWrite(0x4A, 0x78); + RegisterWrite(0x4B, 0x78); + RegisterWrite(0x44, 0x08); + RegisterWrite(0x45, 0x50); + RegisterWrite(0x64, 0xFE); + RegisterWrite(0x65, 0x1F); + RegisterWrite(0x72, 0x0A); + RegisterWrite(0x73, 0x00); + RegisterWrite(0x7F, 0x14); + RegisterWrite(0x44, 0x84); + RegisterWrite(0x65, 0x67); + RegisterWrite(0x66, 0x18); + RegisterWrite(0x63, 0x70); + RegisterWrite(0x6f, 0x2c); + RegisterWrite(0x7F, 0x15); + RegisterWrite(0x48, 0x48); + RegisterWrite(0x7F, 0x07); + RegisterWrite(0x41, 0x0D); + RegisterWrite(0x43, 0x14); + RegisterWrite(0x4B, 0x0E); + RegisterWrite(0x45, 0x0F); + RegisterWrite(0x44, 0x42); + RegisterWrite(0x4C, 0x80); + RegisterWrite(0x7F, 0x10); + RegisterWrite(0x5B, 0x03); + RegisterWrite(0x7F, 0x07); + RegisterWrite(0x40, 0x41); - usleep(10_ms); // delay 10ms + px4_usleep(10'000); // delay 10ms - RegisterWriteVerified(0x7F, 0x00); - RegisterWriteVerified(0x32, 0x00); - RegisterWriteVerified(0x7F, 0x07); - RegisterWriteVerified(0x40, 0x40); - RegisterWriteVerified(0x7F, 0x06); - RegisterWriteVerified(0x68, 0x70); - RegisterWriteVerified(0x69, 0x01); - RegisterWriteVerified(0x7F, 0x0D); - RegisterWriteVerified(0x48, 0xC0); - RegisterWriteVerified(0x6F, 0xD5); - RegisterWriteVerified(0x7F, 0x00); - RegisterWriteVerified(0x5B, 0xA0); - RegisterWriteVerified(0x4E, 0xA8); - RegisterWriteVerified(0x5A, 0x50); - RegisterWriteVerified(0x40, 0x80); - RegisterWriteVerified(0x73, 0x1f); + RegisterWrite(0x7F, 0x00); + RegisterWrite(0x32, 0x00); + RegisterWrite(0x7F, 0x07); + RegisterWrite(0x40, 0x40); + RegisterWrite(0x7F, 0x06); + RegisterWrite(0x68, 0x70); + RegisterWrite(0x69, 0x01); + RegisterWrite(0x7F, 0x0D); + RegisterWrite(0x48, 0xC0); + RegisterWrite(0x6F, 0xD5); + RegisterWrite(0x7F, 0x00); + RegisterWrite(0x5B, 0xA0); + RegisterWrite(0x4E, 0xA8); + RegisterWrite(0x5A, 0x50); + RegisterWrite(0x40, 0x80); + RegisterWrite(0x73, 0x1f); - usleep(10_ms); // delay 10ms + px4_usleep(10'000); // delay 10ms RegisterWrite(0x73, 0x00); } -void PAW3902::ModeSuperLowLight() +void PAW3902::ConfigureModeSuperLowLight() { // Mode 2: Super Low Light (50 fps) 9 Lux typical // super low light and low speed motion tracking // set performance optimization registers - RegisterWriteVerified(0x7F, 0x00); - RegisterWriteVerified(0x55, 0x01); - RegisterWriteVerified(0x50, 0x07); - RegisterWriteVerified(0x7f, 0x0e); - RegisterWriteVerified(0x43, 0x10); + RegisterWrite(0x7F, 0x00); + RegisterWrite(0x55, 0x01); + RegisterWrite(0x50, 0x07); + RegisterWrite(0x7f, 0x0e); + RegisterWrite(0x43, 0x10); - RegisterWriteVerified(0x48, 0x04); - RegisterWriteVerified(0x7F, 0x00); - RegisterWriteVerified(0x51, 0x7b); - RegisterWriteVerified(0x50, 0x00); - RegisterWriteVerified(0x55, 0x00); + RegisterWrite(0x48, 0x04); + RegisterWrite(0x7F, 0x00); + RegisterWrite(0x51, 0x7b); + RegisterWrite(0x50, 0x00); + RegisterWrite(0x55, 0x00); - RegisterWriteVerified(0x7F, 0x00); - RegisterWriteVerified(0x61, 0xAD); - RegisterWriteVerified(0x7F, 0x03); - RegisterWriteVerified(0x40, 0x00); - RegisterWriteVerified(0x7F, 0x05); - RegisterWriteVerified(0x41, 0xB3); - RegisterWriteVerified(0x43, 0xF1); - RegisterWriteVerified(0x45, 0x14); - RegisterWriteVerified(0x5F, 0x34); - RegisterWriteVerified(0x7B, 0x08); - RegisterWriteVerified(0x5E, 0x34); - RegisterWriteVerified(0x5B, 0x32); - RegisterWriteVerified(0x6D, 0x32); - RegisterWriteVerified(0x45, 0x17); - RegisterWriteVerified(0x70, 0xE5); - RegisterWriteVerified(0x71, 0xE5); - RegisterWriteVerified(0x7F, 0x06); - RegisterWriteVerified(0x44, 0x1B); - RegisterWriteVerified(0x40, 0xBF); - RegisterWriteVerified(0x4E, 0x3F); - RegisterWriteVerified(0x7F, 0x08); - RegisterWriteVerified(0x66, 0x44); - RegisterWriteVerified(0x65, 0x20); - RegisterWriteVerified(0x6A, 0x3a); - RegisterWriteVerified(0x61, 0x05); - RegisterWriteVerified(0x62, 0x05); - RegisterWriteVerified(0x7F, 0x09); - RegisterWriteVerified(0x4F, 0xAF); - RegisterWriteVerified(0x48, 0x80); - RegisterWriteVerified(0x49, 0x80); - RegisterWriteVerified(0x57, 0x77); - RegisterWriteVerified(0x5F, 0x40); - RegisterWriteVerified(0x60, 0x78); - RegisterWriteVerified(0x61, 0x78); - RegisterWriteVerified(0x62, 0x08); - RegisterWriteVerified(0x63, 0x50); - RegisterWriteVerified(0x7F, 0x0A); - RegisterWriteVerified(0x45, 0x60); - RegisterWriteVerified(0x7F, 0x00); - RegisterWriteVerified(0x4D, 0x11); - RegisterWriteVerified(0x55, 0x80); - RegisterWriteVerified(0x74, 0x21); - RegisterWriteVerified(0x75, 0x1F); - RegisterWriteVerified(0x4A, 0x78); - RegisterWriteVerified(0x4B, 0x78); - RegisterWriteVerified(0x44, 0x08); - RegisterWriteVerified(0x45, 0x50); - RegisterWriteVerified(0x64, 0xCE); - RegisterWriteVerified(0x65, 0x0B); - RegisterWriteVerified(0x72, 0x0A); - RegisterWriteVerified(0x73, 0x00); - RegisterWriteVerified(0x7F, 0x14); - RegisterWriteVerified(0x44, 0x84); - RegisterWriteVerified(0x65, 0x67); - RegisterWriteVerified(0x66, 0x18); - RegisterWriteVerified(0x63, 0x70); - RegisterWriteVerified(0x6f, 0x2c); - RegisterWriteVerified(0x7F, 0x15); - RegisterWriteVerified(0x48, 0x48); - RegisterWriteVerified(0x7F, 0x07); - RegisterWriteVerified(0x41, 0x0D); - RegisterWriteVerified(0x43, 0x14); - RegisterWriteVerified(0x4B, 0x0E); - RegisterWriteVerified(0x45, 0x0F); - RegisterWriteVerified(0x44, 0x42); - RegisterWriteVerified(0x4C, 0x80); - RegisterWriteVerified(0x7F, 0x10); - RegisterWriteVerified(0x5B, 0x02); - RegisterWriteVerified(0x7F, 0x07); - RegisterWriteVerified(0x40, 0x41); + RegisterWrite(0x7F, 0x00); + RegisterWrite(0x61, 0xAD); + RegisterWrite(0x7F, 0x03); + RegisterWrite(0x40, 0x00); + RegisterWrite(0x7F, 0x05); + RegisterWrite(0x41, 0xB3); + RegisterWrite(0x43, 0xF1); + RegisterWrite(0x45, 0x14); + RegisterWrite(0x5F, 0x34); + RegisterWrite(0x7B, 0x08); + RegisterWrite(0x5E, 0x34); + RegisterWrite(0x5B, 0x32); + RegisterWrite(0x6D, 0x32); + RegisterWrite(0x45, 0x17); + RegisterWrite(0x70, 0xE5); + RegisterWrite(0x71, 0xE5); + RegisterWrite(0x7F, 0x06); + RegisterWrite(0x44, 0x1B); + RegisterWrite(0x40, 0xBF); + RegisterWrite(0x4E, 0x3F); + RegisterWrite(0x7F, 0x08); + RegisterWrite(0x66, 0x44); + RegisterWrite(0x65, 0x20); + RegisterWrite(0x6A, 0x3a); + RegisterWrite(0x61, 0x05); + RegisterWrite(0x62, 0x05); + RegisterWrite(0x7F, 0x09); + RegisterWrite(0x4F, 0xAF); + RegisterWrite(0x48, 0x80); + RegisterWrite(0x49, 0x80); + RegisterWrite(0x57, 0x77); + RegisterWrite(0x5F, 0x40); + RegisterWrite(0x60, 0x78); + RegisterWrite(0x61, 0x78); + RegisterWrite(0x62, 0x08); + RegisterWrite(0x63, 0x50); + RegisterWrite(0x7F, 0x0A); + RegisterWrite(0x45, 0x60); + RegisterWrite(0x7F, 0x00); + RegisterWrite(0x4D, 0x11); + RegisterWrite(0x55, 0x80); + RegisterWrite(0x74, 0x21); + RegisterWrite(0x75, 0x1F); + RegisterWrite(0x4A, 0x78); + RegisterWrite(0x4B, 0x78); + RegisterWrite(0x44, 0x08); + RegisterWrite(0x45, 0x50); + RegisterWrite(0x64, 0xCE); + RegisterWrite(0x65, 0x0B); + RegisterWrite(0x72, 0x0A); + RegisterWrite(0x73, 0x00); + RegisterWrite(0x7F, 0x14); + RegisterWrite(0x44, 0x84); + RegisterWrite(0x65, 0x67); + RegisterWrite(0x66, 0x18); + RegisterWrite(0x63, 0x70); + RegisterWrite(0x6f, 0x2c); + RegisterWrite(0x7F, 0x15); + RegisterWrite(0x48, 0x48); + RegisterWrite(0x7F, 0x07); + RegisterWrite(0x41, 0x0D); + RegisterWrite(0x43, 0x14); + RegisterWrite(0x4B, 0x0E); + RegisterWrite(0x45, 0x0F); + RegisterWrite(0x44, 0x42); + RegisterWrite(0x4C, 0x80); + RegisterWrite(0x7F, 0x10); + RegisterWrite(0x5B, 0x02); + RegisterWrite(0x7F, 0x07); + RegisterWrite(0x40, 0x41); - usleep(25_ms); // delay 25ms + px4_usleep(25'000); // delay 25ms - RegisterWriteVerified(0x7F, 0x00); - RegisterWriteVerified(0x32, 0x44); - RegisterWriteVerified(0x7F, 0x07); - RegisterWriteVerified(0x40, 0x40); - RegisterWriteVerified(0x7F, 0x06); - RegisterWriteVerified(0x68, 0x40); - RegisterWriteVerified(0x69, 0x02); - RegisterWriteVerified(0x7F, 0x0D); - RegisterWriteVerified(0x48, 0xC0); - RegisterWriteVerified(0x6F, 0xD5); - RegisterWriteVerified(0x7F, 0x00); - RegisterWriteVerified(0x5B, 0xA0); - RegisterWriteVerified(0x4E, 0xA8); - RegisterWriteVerified(0x5A, 0x50); - RegisterWriteVerified(0x40, 0x80); - RegisterWriteVerified(0x73, 0x0B); + RegisterWrite(0x7F, 0x00); + RegisterWrite(0x32, 0x44); + RegisterWrite(0x7F, 0x07); + RegisterWrite(0x40, 0x40); + RegisterWrite(0x7F, 0x06); + RegisterWrite(0x68, 0x40); + RegisterWrite(0x69, 0x02); + RegisterWrite(0x7F, 0x0D); + RegisterWrite(0x48, 0xC0); + RegisterWrite(0x6F, 0xD5); + RegisterWrite(0x7F, 0x00); + RegisterWrite(0x5B, 0xA0); + RegisterWrite(0x4E, 0xA8); + RegisterWrite(0x5A, 0x50); + RegisterWrite(0x40, 0x80); + RegisterWrite(0x73, 0x0B); - usleep(25_ms); // delay 25ms + px4_usleep(25'000); // delay 25ms RegisterWrite(0x73, 0x00); } @@ -565,73 +582,78 @@ void PAW3902::EnableLed() RegisterWrite(0x7F, 0x00); } -uint8_t PAW3902::RegisterRead(uint8_t reg, int retries) +uint8_t PAW3902::RegisterRead(uint8_t reg) { - for (int i = 0; i < retries; i++) { - px4_udelay(TIME_us_TSRAD); - uint8_t cmd[2] {reg, 0}; + // tSWR SPI Time Between Write And Read Commands + const hrt_abstime elapsed_last_write = hrt_elapsed_time(&_last_write_time); - if (transfer(&cmd[0], &cmd[0], sizeof(cmd)) == 0) { - return cmd[1]; - } + if (elapsed_last_write < TIME_TSWR_us) { + px4_udelay(TIME_TSWR_us - elapsed_last_write); } - perf_count(_comms_errors); - return 0; + // tSRW/tSRR SPI Time Between Read And Subsequent Commands + const hrt_abstime elapsed_last_read = hrt_elapsed_time(&_last_read_time); + + if (elapsed_last_write < TIME_TSRW_TSRR_us) { + px4_udelay(TIME_TSRW_TSRR_us - elapsed_last_read); + } + + uint8_t cmd[2]; + cmd[0] = DIR_READ(reg); + cmd[1] = 0; + transfer(&cmd[0], &cmd[0], sizeof(cmd)); + hrt_store_absolute_time(&_last_read_time); + + return cmd[1]; } void PAW3902::RegisterWrite(uint8_t reg, uint8_t data) { + // tSWW SPI Time Between Write Commands + const hrt_abstime elapsed_last_write = hrt_elapsed_time(&_last_write_time); + + if (elapsed_last_write < TIME_TSWW_us) { + px4_udelay(TIME_TSWW_us - elapsed_last_write); + } + uint8_t cmd[2]; cmd[0] = DIR_WRITE(reg); cmd[1] = data; - - if (transfer(&cmd[0], nullptr, sizeof(cmd)) != 0) { - perf_count(_comms_errors); - } -} - -bool PAW3902::RegisterWriteVerified(uint8_t reg, uint8_t data, int retries) -{ - for (int i = 0; i < retries; i++) { - uint8_t cmd[2]; - cmd[0] = DIR_WRITE(reg); - cmd[1] = data; - transfer(&cmd[0], nullptr, sizeof(cmd)); - px4_udelay(TIME_us_TSWW); - - // read back to verify - uint8_t data_read = RegisterRead(reg); - - if (data_read == data) { - return true; - } - - PX4_DEBUG("Register write failed 0x%02hhX: 0x%02hhX (actual value 0x%02hhX)", reg, data, data_read); - } - - perf_count(_register_write_fail_perf); - - return false; + transfer(&cmd[0], nullptr, sizeof(cmd)); + hrt_store_absolute_time(&_last_write_time); } void PAW3902::RunImpl() { - // backup schedule - if (_data_ready_interrupt_enabled) { - ScheduleDelayed(_scheduled_interval_us * 2); - } + perf_begin(_cycle_perf); + perf_count(_interval_perf); - // 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 = 5_s; + const hrt_abstime now = hrt_absolute_time(); - if ((hrt_elapsed_time(&_last_good_publish) > RESET_TIMEOUT_US) && (hrt_elapsed_time(&_last_reset) > RESET_TIMEOUT_US)) { - ChangeMode(Mode::LowLight, true); + // 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; } - perf_begin(_sample_perf); - perf_count(_interval_perf); + 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; @@ -639,25 +661,16 @@ void PAW3902::RunImpl() } buf{}; static_assert(sizeof(buf) == (12 + 1)); - const hrt_abstime timestamp_sample = hrt_absolute_time(); - - if (transfer((uint8_t *)&buf, (uint8_t *)&buf, sizeof(buf)) != PX4_OK) { - perf_count(_comms_errors); - perf_end(_sample_perf); + if (transfer((uint8_t *)&buf, (uint8_t *)&buf, sizeof(buf)) != 0) { + perf_end(_cycle_perf); return; } - perf_end(_sample_perf); - - const uint64_t dt_flow = timestamp_sample - _previous_collect_timestamp; - - // update for next iteration - _previous_collect_timestamp = timestamp_sample; + hrt_store_absolute_time(&_last_read_time); if (_discard_reading > 0) { _discard_reading--; - ResetAccumulatedData(); - _valid_count = 0; + perf_end(_cycle_perf); return; } @@ -666,8 +679,17 @@ void PAW3902::RunImpl() // Bright Mode, SQUAL < 0x19, Shutter ≥ 0x1FF0 // Low Light Mode, SQUAL < 0x46, Shutter ≥ 0x1FF0 // Super Low Light Mode, SQUAL < 0x55, Shutter ≥ 0x0BC0 - const uint16_t shutter = (buf.data.Shutter_Upper << 8) | buf.data.Shutter_Lower; + // 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) { @@ -766,99 +788,66 @@ void PAW3902::RunImpl() } if (data_valid) { - 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); + // publish sensor_optical_flow + sensor_optical_flow_s report{}; + report.timestamp_sample = timestamp_sample; + report.device_id = get_device_id(); - _flow_dt_sum_usec += dt_flow; - _flow_sum_x += delta_x_raw; - _flow_sum_y += delta_y_raw; - _flow_sample_counter++; - _flow_quality_sum += buf.data.SQUAL; + report.integration_timespan_us = _scheduled_interval_us; + report.quality = buf.data.SQUAL; - _valid_count++; + // 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 - } else { - _valid_count = 0; - ResetAccumulatedData(); - return; + 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}; + + report.pixel_flow[0] = pixel_flow_rotated(0) / 500.0f; // proportional factor + convert from pixels to radians + report.pixel_flow[1] = pixel_flow_rotated(1) / 500.0f; // proportional factor + convert from pixels to radians + } + + report.timestamp = hrt_absolute_time(); + _sensor_optical_flow_pub.publish(report); + + if (report.quality >= 1) { + _last_good_data = report.timestamp_sample; + } } - // returns if the collect time has not been reached - if (_flow_dt_sum_usec < COLLECT_TIME) { - return; - } - - sensor_optical_flow_s report{}; - report.timestamp_sample = timestamp_sample; - report.device_id = get_device_id(); - - float pixel_flow_x_integral = (float)_flow_sum_x / 500.0f; // proportional factor + convert from pixels to radians - float pixel_flow_y_integral = (float)_flow_sum_y / 500.0f; // proportional factor + convert from pixels to radians - - // rotate measurements in yaw from sensor frame to body frame - const matrix::Vector3f pixel_flow_rotated = _rotation * matrix::Vector3f{pixel_flow_x_integral, pixel_flow_y_integral, 0.f}; - report.pixel_flow[0] = pixel_flow_rotated(0); - report.pixel_flow[1] = pixel_flow_rotated(1); - - report.integration_timespan_us = _flow_dt_sum_usec; // microseconds - - report.quality = _flow_quality_sum / _flow_sample_counter; - - // No gyro on this board - report.delta_angle[0] = NAN; - report.delta_angle[1] = NAN; - report.delta_angle[2] = NAN; - - // set (conservative) 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 = 30.0f; // 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; - } - - report.timestamp = hrt_absolute_time(); - _sensor_optical_flow_pub.publish(report); - - if (report.quality > 10) { - _last_good_publish = report.timestamp; - } - - ResetAccumulatedData(); -} - -void PAW3902::ResetAccumulatedData() -{ - // reset - _flow_dt_sum_usec = 0; - _flow_sum_x = 0; - _flow_sum_y = 0; - _flow_sample_counter = 0; - _flow_quality_sum = 0; + perf_end(_cycle_perf); } void PAW3902::print_status() { I2CSPIDriverBase::print_status(); - perf_print_counter(_sample_perf); + perf_print_counter(_cycle_perf); perf_print_counter(_interval_perf); - perf_print_counter(_comms_errors); + perf_print_counter(_reset_perf); perf_print_counter(_false_motion_perf); - perf_print_counter(_register_write_fail_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); } diff --git a/src/drivers/optical_flow/paw3902/PAW3902.hpp b/src/drivers/optical_flow/paw3902/PAW3902.hpp index d6ea4ff436..11951be383 100644 --- a/src/drivers/optical_flow/paw3902/PAW3902.hpp +++ b/src/drivers/optical_flow/paw3902/PAW3902.hpp @@ -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 #include @@ -48,16 +48,15 @@ #include #include #include -#include #include #include #include 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 { @@ -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_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 _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}; }; diff --git a/src/drivers/optical_flow/paw3902/PixArt_PAW3902JF_Registers.hpp b/src/drivers/optical_flow/paw3902/PixArt_PAW3902_Registers.hpp similarity index 71% rename from src/drivers/optical_flow/paw3902/PixArt_PAW3902JF_Registers.hpp rename to src/drivers/optical_flow/paw3902/PixArt_PAW3902_Registers.hpp index 205f0065d5..eceb490200 100644 --- a/src/drivers/optical_flow/paw3902/PixArt_PAW3902JF_Registers.hpp +++ b/src/drivers/optical_flow/paw3902/PixArt_PAW3902_Registers.hpp @@ -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 + +// 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 { diff --git a/src/drivers/optical_flow/paw3902/parameters.c b/src/drivers/optical_flow/paw3902/parameters.c index 96fa1ce316..e2ce1da533 100644 --- a/src/drivers/optical_flow/paw3902/parameters.c +++ b/src/drivers/optical_flow/paw3902/parameters.c @@ -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 * diff --git a/src/drivers/optical_flow/paw3902/paw3902_main.cpp b/src/drivers/optical_flow/paw3902/paw3902_main.cpp index 68a56ba9ff..f8f383f1ee 100644 --- a/src/drivers/optical_flow/paw3902/paw3902_main.cpp +++ b/src/drivers/optical_flow/paw3902/paw3902_main.cpp @@ -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); }