diff --git a/msg/optical_flow.msg b/msg/optical_flow.msg index 68404f7667..4b3796ff9f 100644 --- a/msg/optical_flow.msg +++ b/msg/optical_flow.msg @@ -19,3 +19,11 @@ uint8 quality # Average of quality of accumulated frames, 0: bad quality, 255: m float32 max_flow_rate # Magnitude of maximum angular which the optical flow sensor can measure reliably float32 min_ground_distance # Minimum distance from ground at which the optical flow sensor operates reliably float32 max_ground_distance # Maximum distance from ground at which the optical flow sensor operates reliably + + +uint8 MODE_UNKNOWN = 0 +uint8 MODE_BRIGHT = 1 +uint8 MODE_LOWLIGHT = 2 +uint8 MODE_SUPER_LOWLIGHT = 3 + +uint8 mode diff --git a/src/drivers/optical_flow/paw3902/PAW3902.cpp b/src/drivers/optical_flow/paw3902/PAW3902.cpp index faf34ab645..d581b9945d 100644 --- a/src/drivers/optical_flow/paw3902/PAW3902.cpp +++ b/src/drivers/optical_flow/paw3902/PAW3902.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2019-2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2019-2021 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,10 +33,16 @@ #include "PAW3902.hpp" +static constexpr int16_t combine(uint8_t msb, uint8_t lsb) +{ + return (msb << 8u) | lsb; +} + PAW3902::PAW3902(I2CSPIBusOption bus_option, int bus, int devid, int bus_frequency, spi_mode_e spi_mode, - float yaw_rotation_degrees) : + spi_drdy_gpio_t drdy_gpio, float yaw_rotation_degrees) : SPI(DRV_FLOW_DEVTYPE_PAW3902, MODULE_NAME, bus, devid, spi_mode, bus_frequency), - I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus) + I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus), + _drdy_gpio(drdy_gpio) { if (PX4_ISFINITE(yaw_rotation_degrees)) { PX4_INFO("using yaw rotation %.3f degrees (%.3f radians)", @@ -65,8 +71,10 @@ PAW3902::~PAW3902() perf_free(_interval_perf); perf_free(_comms_errors); perf_free(_false_motion_perf); - perf_free(_mode_change_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); } int PAW3902::init() @@ -78,14 +86,11 @@ int PAW3902::init() Reset(); - // default to low light mode (1) - ModeLowLight(); + // force to low light mode (1) initially + ChangeMode(Mode::LowLight, true); _previous_collect_timestamp = hrt_absolute_time(); - // schedule a cycle to start things - ScheduleOnInterval(SAMPLE_INTERVAL_MODE_1, SAMPLE_INTERVAL_MODE_1); - return PX4_OK; } @@ -115,8 +120,40 @@ int PAW3902::probe() return PX4_OK; } +int PAW3902::DataReadyInterruptCallback(int irq, void *context, void *arg) +{ + static_cast(arg)->DataReady(); + return 0; +} + +void PAW3902::DataReady() +{ + ScheduleNow(); +} + +bool PAW3902::DataReadyInterruptConfigure() +{ + if (_drdy_gpio == 0) { + return false; + } + + // Setup data ready on falling edge + return px4_arch_gpiosetevent(_drdy_gpio, false, true, true, &DataReadyInterruptCallback, this) == 0; +} + +bool PAW3902::DataReadyInterruptDisable() +{ + if (_drdy_gpio == 0) { + return false; + } + + return px4_arch_gpiosetevent(_drdy_gpio, false, false, false, nullptr, nullptr) == 0; +} + bool PAW3902::Reset() { + DataReadyInterruptDisable(); + // Power on reset RegisterWrite(Register::Power_Up_Reset, 0x5A); usleep(1000); @@ -131,32 +168,52 @@ bool PAW3902::Reset() return true; } -bool PAW3902::ChangeMode(Mode newMode) +void PAW3902::exit_and_cleanup() { - if (newMode != _mode) { + DataReadyInterruptDisable(); + I2CSPIDriverBase::exit_and_cleanup(); +} + +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); + uint32_t interval_us = 0; + switch (newMode) { case Mode::Bright: ModeBright(); - ScheduleOnInterval(SAMPLE_INTERVAL_MODE_0); + interval_us = SAMPLE_INTERVAL_MODE_0; + perf_count(_mode_change_bright_perf); break; case Mode::LowLight: ModeLowLight(); - ScheduleOnInterval(SAMPLE_INTERVAL_MODE_1); + interval_us = SAMPLE_INTERVAL_MODE_1; + perf_count(_mode_change_low_light_perf); break; case Mode::SuperLowLight: ModeSuperLowLight(); - ScheduleOnInterval(SAMPLE_INTERVAL_MODE_2); + interval_us = SAMPLE_INTERVAL_MODE_2; + perf_count(_mode_change_super_low_light_perf); break; } + if (DataReadyInterruptConfigure()) { + // backup schedule as a watchdog timeout + ScheduleDelayed(500_ms); + + } else { + ScheduleOnInterval(interval_us); + } + // Discard the first three motion data. for (int i = 0; i < 3; i++) { RegisterRead(Register::Motion); @@ -166,9 +223,9 @@ bool PAW3902::ChangeMode(Mode newMode) RegisterRead(Register::Delta_Y_H); } - _mode = newMode; + EnableLed(); - perf_count(_mode_change_perf); + _mode = newMode; } _bright_to_low_counter = 0; @@ -184,7 +241,7 @@ bool PAW3902::ChangeMode(Mode newMode) return true; } -bool PAW3902::ModeBright() +void PAW3902::ModeBright() { // Mode 0: Bright (126 fps) 60 Lux typical @@ -293,17 +350,10 @@ bool PAW3902::ModeBright() usleep(10_ms); // delay 10ms - RegisterWriteVerified(0x73, 0x00); - - // Enable LED_N controls - RegisterWriteVerified(0x7F, 0x14); - RegisterWriteVerified(0x6F, 0x1c); - RegisterWriteVerified(0x7F, 0x00); - - return true; + RegisterWrite(0x73, 0x00); } -bool PAW3902::ModeLowLight() +void PAW3902::ModeLowLight() { // Mode 1: Low Light (126 fps) 30 Lux typical // low light and low speed motion tracking @@ -413,17 +463,10 @@ bool PAW3902::ModeLowLight() usleep(10_ms); // delay 10ms - RegisterWriteVerified(0x73, 0x00); - - // Enable LED_N controls - RegisterWriteVerified(0x7F, 0x14); - RegisterWriteVerified(0x6F, 0x1c); - RegisterWriteVerified(0x7F, 0x00); - - return true; + RegisterWrite(0x73, 0x00); } -bool PAW3902::ModeSuperLowLight() +void PAW3902::ModeSuperLowLight() { // Mode 2: Super Low Light (50 fps) 9 Lux typical // super low light and low speed motion tracking @@ -533,19 +576,21 @@ bool PAW3902::ModeSuperLowLight() usleep(25_ms); // delay 25ms - RegisterWriteVerified(0x73, 0x00); + RegisterWrite(0x73, 0x00); +} +void PAW3902::EnableLed() +{ // Enable LED_N controls RegisterWriteVerified(0x7F, 0x14); RegisterWriteVerified(0x6F, 0x1c); RegisterWriteVerified(0x7F, 0x00); - - return true; } uint8_t PAW3902::RegisterRead(uint8_t reg, int retries) { for (int i = 0; i < retries; i++) { + px4_udelay(TIME_us_TSRAD); uint8_t cmd[2] {reg, 0}; if (transfer(&cmd[0], &cmd[0], sizeof(cmd)) == 0) { @@ -575,11 +620,16 @@ bool PAW3902::RegisterWriteVerified(uint8_t reg, uint8_t data, int retries) cmd[0] = DIR_WRITE(reg); cmd[1] = data; transfer(&cmd[0], nullptr, sizeof(cmd)); + px4_udelay(TIME_us_TSWW); // read back to verify - if (RegisterRead(reg) == data) { + 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); @@ -613,7 +663,6 @@ void PAW3902::RunImpl() // update for next iteration _previous_collect_timestamp = timestamp_sample; - // 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 @@ -621,26 +670,17 @@ void PAW3902::RunImpl() // Super Low Light Mode, SQUAL < 0x55, Shutter ≥ 0x0BC0 const uint16_t shutter = (buf.data.Shutter_Upper << 8) | buf.data.Shutter_Lower; - if ((buf.data.SQUAL < 0x19) && (shutter >= 0x0BC0)) { - PX4_DEBUG("false motion report, discarding"); - perf_count(_false_motion_perf); - perf_end(_sample_perf); - - // reset - _flow_dt_sum_usec = 0; - _flow_sum_x = 0; - _flow_sum_y = 0; - _flow_sample_counter = 0; - _flow_quality_sum = 0; - - return; - } - - const int16_t delta_x_raw = ((int16_t)buf.data.Delta_X_H << 8) | buf.data.Delta_X_L; - const int16_t delta_y_raw = ((int16_t)buf.data.Delta_Y_H << 8) | buf.data.Delta_Y_L; + bool data_valid = true; switch (_mode) { case Mode::Bright: + if ((buf.data.SQUAL < 0x19) && (shutter >= 0x1FF0)) { + // false motion report, discarding + perf_count(_false_motion_perf); + ResetAccumulatedData(); + data_valid = false; + } + if ((shutter >= 0x1FFE) && (buf.data.RawData_Sum < 0x3C)) { // Bright -> LowLight _bright_to_low_counter++; @@ -656,6 +696,13 @@ void PAW3902::RunImpl() break; case Mode::LowLight: + if ((buf.data.SQUAL < 0x46) && (shutter >= 0x1FF0)) { + // false motion report, discarding + perf_count(_false_motion_perf); + ResetAccumulatedData(); + data_valid = false; + } + if ((shutter >= 0x1FFE) && (buf.data.RawData_Sum < 0x5A)) { // LowLight -> SuperLowLight _low_to_bright_counter = 0; @@ -665,22 +712,32 @@ void PAW3902::RunImpl() ChangeMode(Mode::SuperLowLight); } - } else if ((shutter < 0x0BB8)) { + } else if (shutter < 0x0BB8) { // LowLight -> Bright - _low_to_superlow_counter = 0; _low_to_bright_counter++; + _low_to_superlow_counter = 0; if (_low_to_bright_counter >= 10) { // AND valid for 10 consecutive frames ChangeMode(Mode::Bright); } + + } else { + _low_to_bright_counter = 0; + _low_to_superlow_counter = 0; } break; case Mode::SuperLowLight: + if ((buf.data.SQUAL < 0x55) && (shutter >= 0x0BC0)) { + // false motion report, discarding + perf_count(_false_motion_perf); + ResetAccumulatedData(); + data_valid = false; + } - // SuperLowLight -> LowLight - if ((shutter < 0x03E8)) { + if (shutter < 0x03E8) { + // SuperLowLight -> LowLight _superlow_to_low_counter++; if (_superlow_to_low_counter >= 10) { // AND valid for 10 consecutive frames @@ -694,7 +751,10 @@ void PAW3902::RunImpl() break; } - if (buf.data.SQUAL > 0) { + if (data_valid && (buf.data.SQUAL > 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); + _flow_dt_sum_usec += dt_flow; _flow_sum_x += delta_x_raw; _flow_sum_y += delta_y_raw; @@ -702,12 +762,7 @@ void PAW3902::RunImpl() _flow_quality_sum += buf.data.SQUAL; } else { - // reset - _flow_dt_sum_usec = 0; - _flow_sum_x = 0; - _flow_sum_y = 0; - _flow_sample_counter = 0; - _flow_quality_sum = 0; + ResetAccumulatedData(); return; } @@ -743,9 +798,29 @@ void PAW3902::RunImpl() report.min_ground_distance = 0.08f; // Datasheet: 80mm report.max_ground_distance = 30.0f; // Datasheet: infinity + + switch (_mode) { + case Mode::Bright: + report.mode = optical_flow_s::MODE_BRIGHT; + break; + + case Mode::LowLight: + report.mode = optical_flow_s::MODE_LOWLIGHT; + break; + + case Mode::SuperLowLight: + report.mode = optical_flow_s::MODE_SUPER_LOWLIGHT; + break; + } + report.timestamp = hrt_absolute_time(); _optical_flow_pub.publish(report); + ResetAccumulatedData(); +} + +void PAW3902::ResetAccumulatedData() +{ // reset _flow_dt_sum_usec = 0; _flow_sum_x = 0; @@ -762,6 +837,8 @@ void PAW3902::print_status() perf_print_counter(_interval_perf); perf_print_counter(_comms_errors); perf_print_counter(_false_motion_perf); - perf_print_counter(_mode_change_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); } diff --git a/src/drivers/optical_flow/paw3902/PAW3902.hpp b/src/drivers/optical_flow/paw3902/PAW3902.hpp index 5f46a42db9..87d0d96c27 100644 --- a/src/drivers/optical_flow/paw3902/PAW3902.hpp +++ b/src/drivers/optical_flow/paw3902/PAW3902.hpp @@ -63,7 +63,7 @@ class PAW3902 : public device::SPI, public I2CSPIDriver { public: PAW3902(I2CSPIBusOption bus_option, int bus, int devid, int bus_frequency, spi_mode_e spi_mode, - float yaw_rotation_degrees = NAN); + spi_drdy_gpio_t drdy_gpio, float yaw_rotation_degrees = NAN); virtual ~PAW3902(); static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, @@ -77,19 +77,30 @@ public: void RunImpl(); private: + void exit_and_cleanup() override; + int probe() override; + static int DataReadyInterruptCallback(int irq, void *context, void *arg); + void DataReady(); + bool DataReadyInterruptConfigure(); + bool DataReadyInterruptDisable(); + uint8_t RegisterRead(uint8_t reg, int retries = 3); void RegisterWrite(uint8_t reg, uint8_t data); bool RegisterWriteVerified(uint8_t reg, uint8_t data, int retries = 5); bool Reset(); - bool ModeBright(); - bool ModeLowLight(); - bool ModeSuperLowLight(); + void EnableLed(); + + void ModeBright(); + void ModeLowLight(); + void ModeSuperLowLight(); - bool ChangeMode(Mode newMode); + bool ChangeMode(Mode newMode, bool force = false); + + void ResetAccumulatedData(); uORB::PublicationMulti _optical_flow_pub{ORB_ID(optical_flow)}; @@ -97,11 +108,15 @@ private: 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 _mode_change_perf{perf_alloc(PC_COUNT, MODULE_NAME": mode change")}; 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 + 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}; diff --git a/src/drivers/optical_flow/paw3902/PixArt_PAW3902JF_Registers.hpp b/src/drivers/optical_flow/paw3902/PixArt_PAW3902JF_Registers.hpp index a012c3f0e8..321295797a 100644 --- a/src/drivers/optical_flow/paw3902/PixArt_PAW3902JF_Registers.hpp +++ b/src/drivers/optical_flow/paw3902/PixArt_PAW3902JF_Registers.hpp @@ -46,6 +46,10 @@ 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; + enum Register : uint8_t { Product_ID = 0x00, Revision_ID = 0x01, @@ -76,8 +80,8 @@ enum Product_ID_Bit : uint8_t { }; enum class Mode { - Bright = 0, - LowLight = 1, + Bright = 0, + LowLight = 1, SuperLowLight = 2, }; diff --git a/src/drivers/optical_flow/paw3902/paw3902_main.cpp b/src/drivers/optical_flow/paw3902/paw3902_main.cpp index 8bca7ba583..65d01defa3 100644 --- a/src/drivers/optical_flow/paw3902/paw3902_main.cpp +++ b/src/drivers/optical_flow/paw3902/paw3902_main.cpp @@ -47,7 +47,7 @@ I2CSPIDriverBase *PAW3902::instantiate(const BusCLIArguments &cli, const BusInst int runtime_instance) { PAW3902 *instance = new PAW3902(iterator.configuredBusOption(), iterator.bus(), iterator.devid(), cli.bus_frequency, - cli.spi_mode, cli.custom1); + cli.spi_mode, iterator.DRDYGPIO(), cli.custom1); if (!instance) { PX4_ERR("alloc failed");