diff --git a/boards/nxp/fmuk66-v3/default.px4board b/boards/nxp/fmuk66-v3/default.px4board index 7baee2656e..63032fb003 100644 --- a/boards/nxp/fmuk66-v3/default.px4board +++ b/boards/nxp/fmuk66-v3/default.px4board @@ -23,10 +23,7 @@ CONFIG_DRIVERS_LIGHTS_RGBLED=y CONFIG_DRIVERS_LIGHTS_RGBLED_NCP5623C=y CONFIG_DRIVERS_LIGHTS_RGBLED_PWM=y CONFIG_COMMON_MAGNETOMETER=y -CONFIG_DRIVERS_OPTICAL_FLOW_PAW3902=y -CONFIG_DRIVERS_OPTICAL_FLOW_PMW3901=y -CONFIG_DRIVERS_OPTICAL_FLOW_PX4FLOW=y -CONFIG_DRIVERS_OPTICAL_FLOW_THONEFLOW=y +CONFIG_COMMON_OPTICAL_FLOW=y CONFIG_DRIVERS_OSD=y CONFIG_DRIVERS_PCA9685=y CONFIG_DRIVERS_PCA9685_PWM_OUT=y @@ -55,6 +52,7 @@ CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_FW_ATT_CONTROL=y CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y CONFIG_MODULES_FW_POS_CONTROL_L1=y +CONFIG_MODULES_GIMBAL=y CONFIG_MODULES_GYRO_CALIBRATION=y CONFIG_MODULES_GYRO_FFT=y CONFIG_MODULES_LAND_DETECTOR=y @@ -80,7 +78,6 @@ CONFIG_MODULES_SIMULATION_SENSOR_GPS_SIM=y CONFIG_MODULES_SIMULATION_SENSOR_MAG_SIM=y CONFIG_MODULES_SIMULATION_SIMULATOR_SIH=y CONFIG_MODULES_TEMPERATURE_COMPENSATION=y -CONFIG_MODULES_GIMBAL=y CONFIG_MODULES_VTOL_ATT_CONTROL=y CONFIG_SYSTEMCMDS_DUMPFILE=y CONFIG_SYSTEMCMDS_I2CDETECT=y diff --git a/src/drivers/optical_flow/Kconfig b/src/drivers/optical_flow/Kconfig index 7926d5039b..59c7d42608 100644 --- a/src/drivers/optical_flow/Kconfig +++ b/src/drivers/optical_flow/Kconfig @@ -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 diff --git a/src/drivers/optical_flow/paa3905/CMakeLists.txt b/src/drivers/optical_flow/paa3905/CMakeLists.txt index 0e699c9b81..baf8b50fde 100644 --- a/src/drivers/optical_flow/paa3905/CMakeLists.txt +++ b/src/drivers/optical_flow/paa3905/CMakeLists.txt @@ -34,6 +34,8 @@ px4_add_module( MODULE drivers__optical_flow__paa3905 MAIN paa3905 + COMPILE_FLAGS + #-DDEBUG_BUILD SRCS paa3905_main.cpp PAA3905.cpp diff --git a/src/drivers/optical_flow/paa3905/Kconfig b/src/drivers/optical_flow/paa3905/Kconfig index 927ebd2c94..f8883145b1 100644 --- a/src/drivers/optical_flow/paa3905/Kconfig +++ b/src/drivers/optical_flow/paa3905/Kconfig @@ -2,4 +2,4 @@ menuconfig DRIVERS_OPTICAL_FLOW_PAA3905 bool "paa3905" default n ---help--- - Enable support for paa3905 + Enable support for PixArt paa3905 diff --git a/src/drivers/optical_flow/paa3905/PAA3905.cpp b/src/drivers/optical_flow/paa3905/PAA3905.cpp index cd87391c84..23cdee2f13 100644 --- a/src/drivers/optical_flow/paa3905/PAA3905.cpp +++ b/src/drivers/optical_flow/paa3905/PAA3905.cpp @@ -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(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(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); -} diff --git a/src/drivers/optical_flow/paa3905/PAA3905.hpp b/src/drivers/optical_flow/paa3905/PAA3905.hpp index 337ed19659..a6d8b4afbf 100644 --- a/src/drivers/optical_flow/paa3905/PAA3905.hpp +++ b/src/drivers/optical_flow/paa3905/PAA3905.hpp @@ -41,14 +41,12 @@ #include "PixArt_PAA3905_Registers.hpp" -#include -#include -#include -#include -#include -#include -#include #include +#include +#include +#include +#include +#include #include #include @@ -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_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 _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}; }; diff --git a/src/drivers/optical_flow/paa3905/PixArt_PAA3905_Registers.hpp b/src/drivers/optical_flow/paa3905/PixArt_PAA3905_Registers.hpp index de67e0cf4e..aa642f0b82 100644 --- a/src/drivers/optical_flow/paa3905/PixArt_PAA3905_Registers.hpp +++ b/src/drivers/optical_flow/paa3905/PixArt_PAA3905_Registers.hpp @@ -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 }; diff --git a/src/drivers/optical_flow/paa3905/paa3905_main.cpp b/src/drivers/optical_flow/paa3905/paa3905_main.cpp index ca43b70b7b..08dfba6b2b 100644 --- a/src/drivers/optical_flow/paa3905/paa3905_main.cpp +++ b/src/drivers/optical_flow/paa3905/paa3905_main.cpp @@ -32,6 +32,8 @@ ****************************************************************************/ #include "PAA3905.hpp" + +#include #include void PAA3905::print_usage() diff --git a/src/drivers/optical_flow/paw3902/CMakeLists.txt b/src/drivers/optical_flow/paw3902/CMakeLists.txt index 2bd4380bf9..f76c0b5606 100644 --- a/src/drivers/optical_flow/paw3902/CMakeLists.txt +++ b/src/drivers/optical_flow/paw3902/CMakeLists.txt @@ -34,6 +34,8 @@ px4_add_module( MODULE drivers__optical_flow__paw3902 MAIN paw3902 + COMPILE_FLAGS + #-DDEBUG_BUILD SRCS paw3902_main.cpp PAW3902.cpp diff --git a/src/drivers/optical_flow/paw3902/Kconfig b/src/drivers/optical_flow/paw3902/Kconfig index c3a8f3a82f..bbc6b03923 100644 --- a/src/drivers/optical_flow/paw3902/Kconfig +++ b/src/drivers/optical_flow/paw3902/Kconfig @@ -2,4 +2,4 @@ menuconfig DRIVERS_OPTICAL_FLOW_PAW3902 bool "paw3902" default n ---help--- - Enable support for paw3902 \ No newline at end of file + Enable support for PixArt paw3902 diff --git a/src/drivers/optical_flow/paw3902/PAW3902.cpp b/src/drivers/optical_flow/paw3902/PAW3902.cpp index de8e6c039c..dabf34d6bf 100644 --- a/src/drivers/optical_flow/paw3902/PAW3902.cpp +++ b/src/drivers/optical_flow/paw3902/PAW3902.cpp @@ -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(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(_mode), static_cast(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(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); -} diff --git a/src/drivers/optical_flow/paw3902/PAW3902.hpp b/src/drivers/optical_flow/paw3902/PAW3902.hpp index 11951be383..44e1479753 100644 --- a/src/drivers/optical_flow/paw3902/PAW3902.hpp +++ b/src/drivers/optical_flow/paw3902/PAW3902.hpp @@ -41,14 +41,12 @@ #include "PixArt_PAW3902_Registers.hpp" -#include -#include -#include -#include -#include -#include -#include #include +#include +#include +#include +#include +#include #include #include @@ -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_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 _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 _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}; }; diff --git a/src/drivers/optical_flow/paw3902/PixArt_PAW3902_Registers.hpp b/src/drivers/optical_flow/paw3902/PixArt_PAW3902_Registers.hpp index eceb490200..5d78a4025f 100644 --- a/src/drivers/optical_flow/paw3902/PixArt_PAW3902_Registers.hpp +++ b/src/drivers/optical_flow/paw3902/PixArt_PAW3902_Registers.hpp @@ -84,7 +84,7 @@ enum Register : uint8_t { Power_Up_Reset = 0x3A, - Resolution = 0x4E, + Resolution = 0x4E, Inverse_Product_ID = 0x5F, }; diff --git a/src/drivers/optical_flow/paw3902/paw3902_main.cpp b/src/drivers/optical_flow/paw3902/paw3902_main.cpp index f8f383f1ee..e3eb857dc2 100644 --- a/src/drivers/optical_flow/paw3902/paw3902_main.cpp +++ b/src/drivers/optical_flow/paw3902/paw3902_main.cpp @@ -32,6 +32,8 @@ ****************************************************************************/ #include "PAW3902.hpp" + +#include #include 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) {