diff --git a/src/drivers/optical_flow/paa3905/PAA3905.cpp b/src/drivers/optical_flow/paa3905/PAA3905.cpp index b6457946a0..ede8687c8d 100644 --- a/src/drivers/optical_flow/paa3905/PAA3905.cpp +++ b/src/drivers/optical_flow/paa3905/PAA3905.cpp @@ -43,6 +43,10 @@ PAA3905::PAA3905(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,27 +56,21 @@ PAA3905::PAA3905(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(); } } PAA3905::~PAA3905() { // 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 PAA3905::init() @@ -84,35 +82,35 @@ int PAA3905::init() Configure(); - _previous_collect_timestamp = hrt_absolute_time(); - return PX4_OK; } int PAA3905::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 PAA3905::DataReadyInterruptCallback(int irq, void *context, void *arg) @@ -123,12 +121,14 @@ int PAA3905::DataReadyInterruptCallback(int irq, void *context, void *arg) void PAA3905::DataReady() { + _drdy_timestamp_sample.store(hrt_absolute_time()); ScheduleNow(); } bool PAA3905::DataReadyInterruptConfigure() { if (_drdy_gpio == 0) { + _data_ready_interrupt_enabled = false; return false; } @@ -144,12 +144,12 @@ bool PAA3905::DataReadyInterruptConfigure() bool PAA3905::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; } @@ -159,8 +159,10 @@ void PAA3905::exit_and_cleanup() I2CSPIDriverBase::exit_and_cleanup(); } -void PAA3905::Configure() +void PAA3905::Reset() { + perf_count(_reset_perf); + DataReadyInterruptDisable(); ScheduleClear(); @@ -169,274 +171,312 @@ void PAA3905::Configure() px4_usleep(1000); _last_reset = hrt_absolute_time(); - StandardDetectionSetting(); - ModeAuto012(); + _discard_reading = 3; - CheckMode(); + // 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); +} - switch (_mode) { - case Mode::Bright: - _scheduled_interval_us = SAMPLE_INTERVAL_MODE_0; - break; +void PAA3905::Configure() +{ + Reset(); - case Mode::LowLight: - _scheduled_interval_us = SAMPLE_INTERVAL_MODE_1; - break; + ConfigureStandardDetectionSetting(); - case Mode::SuperLowLight: - _scheduled_interval_us = SAMPLE_INTERVAL_MODE_2; - break; - } + ConfigureAutomaticModeSwitching(); EnableLed(); - _discard_reading = 3; - _valid_count = 0; + // Read Register 0x15. Check Bit [7:6] for AMS mode + const uint8_t Observation = RegisterRead(Register::Observation); + UpdateMode(Observation); 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); } } -void PAA3905::CheckMode() +void PAA3905::ConfigureStandardDetectionSetting() { - // Read Register 0x15. Check Bit [7:6] for AMS mode and store it into a variable. - const uint8_t Observation = RegisterRead(Register::Observation); + // Standard Detection Setting is recommended for general tracking operations. In this mode, the chip can detect + // when it is operating over striped, checkerboard, and glossy tile surfaces where tracking performance is + // compromised. - // Bit [7:6] AMS mode - const uint8_t ams_mode = (Observation & (Bit7 | Bit6)) >> 5; + RegisterWrite(0x7F, 0x00); + RegisterWrite(0x51, 0xFF); + RegisterWrite(0x4E, 0x2A); + RegisterWrite(0x66, 0x3E); + RegisterWrite(0x7F, 0x14); + RegisterWrite(0x7E, 0x71); + RegisterWrite(0x55, 0x00); + RegisterWrite(0x59, 0x00); + RegisterWrite(0x6F, 0x2C); + RegisterWrite(0x7F, 0x05); + RegisterWrite(0x4D, 0xAC); + RegisterWrite(0x4E, 0x32); + RegisterWrite(0x7F, 0x09); + RegisterWrite(0x5C, 0xAF); + RegisterWrite(0x5F, 0xAF); + RegisterWrite(0x70, 0x08); + RegisterWrite(0x71, 0x04); + RegisterWrite(0x72, 0x06); + RegisterWrite(0x74, 0x3C); + RegisterWrite(0x75, 0x28); + RegisterWrite(0x76, 0x20); + RegisterWrite(0x4E, 0xBF); + RegisterWrite(0x7F, 0x03); + RegisterWrite(0x64, 0x14); + RegisterWrite(0x65, 0x0A); + RegisterWrite(0x66, 0x10); + RegisterWrite(0x55, 0x3C); + RegisterWrite(0x56, 0x28); + RegisterWrite(0x57, 0x20); + RegisterWrite(0x4A, 0x2D); - if (ams_mode == 0x0) { - // Mode 0 - _mode = Mode::SuperLowLight; - - } else if (ams_mode == 0x1) { - // Mode 1 - _mode = Mode::LowLight; - - } else if (ams_mode == 0x2) { - // Mode 2 - _mode = Mode::SuperLowLight; - } + RegisterWrite(0x4B, 0x2D); + RegisterWrite(0x4E, 0x4B); + RegisterWrite(0x69, 0xFA); + RegisterWrite(0x7F, 0x05); + RegisterWrite(0x69, 0x1F); + RegisterWrite(0x47, 0x1F); + RegisterWrite(0x48, 0x0C); + RegisterWrite(0x5A, 0x20); + RegisterWrite(0x75, 0x0F); + RegisterWrite(0x4A, 0x0F); + RegisterWrite(0x42, 0x02); + RegisterWrite(0x45, 0x03); + RegisterWrite(0x65, 0x00); + RegisterWrite(0x67, 0x76); + RegisterWrite(0x68, 0x76); + RegisterWrite(0x6A, 0xC5); + RegisterWrite(0x43, 0x00); + RegisterWrite(0x7F, 0x06); + RegisterWrite(0x4A, 0x18); + RegisterWrite(0x4B, 0x0C); + RegisterWrite(0x4C, 0x0C); + RegisterWrite(0x4D, 0x0C); + RegisterWrite(0x46, 0x0A); + RegisterWrite(0x59, 0xCD); + RegisterWrite(0x7F, 0x0A); + RegisterWrite(0x4A, 0x2A); + RegisterWrite(0x48, 0x96); + RegisterWrite(0x52, 0xB4); + RegisterWrite(0x7F, 0x00); + RegisterWrite(0x5B, 0xA0); } -void PAA3905::StandardDetectionSetting() +void PAA3905::ConfigureEnhancedDetectionMode() { - RegisterWriteVerified(0x7F, 0x00); - RegisterWriteVerified(0x51, 0xFF); - RegisterWriteVerified(0x4E, 0x2A); - RegisterWriteVerified(0x66, 0x3E); - RegisterWriteVerified(0x7F, 0x14); - RegisterWriteVerified(0x7E, 0x71); - RegisterWriteVerified(0x55, 0x00); - RegisterWriteVerified(0x59, 0x00); - RegisterWriteVerified(0x6F, 0x2C); - RegisterWriteVerified(0x7F, 0x05); - RegisterWriteVerified(0x4D, 0xAC); - RegisterWriteVerified(0x4E, 0x32); - RegisterWriteVerified(0x7F, 0x09); - RegisterWriteVerified(0x5C, 0xAF); - RegisterWriteVerified(0x5F, 0xAF); - RegisterWriteVerified(0x70, 0x08); - RegisterWriteVerified(0x71, 0x04); - RegisterWriteVerified(0x72, 0x06); - RegisterWriteVerified(0x74, 0x3C); - RegisterWriteVerified(0x75, 0x28); - RegisterWriteVerified(0x76, 0x20); - RegisterWriteVerified(0x4E, 0xBF); - RegisterWriteVerified(0x7F, 0x03); - RegisterWriteVerified(0x64, 0x14); - RegisterWriteVerified(0x65, 0x0A); - RegisterWriteVerified(0x66, 0x10); - RegisterWriteVerified(0x55, 0x3C); - RegisterWriteVerified(0x56, 0x28); - RegisterWriteVerified(0x57, 0x20); - RegisterWriteVerified(0x4A, 0x2D); + // Enhance Detection Setting relatively has better detection sensitivity, it is recommended where yaw motion + // detection is required, and also where more sensitive challenging surface detection is required. The recommended + // operating height must be greater than 15 cm to avoid false detection on challenging surfaces due to increasing of + // sensitivity. - RegisterWriteVerified(0x4B, 0x2D); - RegisterWriteVerified(0x4E, 0x4B); - RegisterWriteVerified(0x69, 0xFA); - RegisterWriteVerified(0x7F, 0x05); - RegisterWriteVerified(0x69, 0x1F); - RegisterWriteVerified(0x47, 0x1F); - RegisterWriteVerified(0x48, 0x0C); - RegisterWriteVerified(0x5A, 0x20); - RegisterWriteVerified(0x75, 0x0F); - RegisterWriteVerified(0x4A, 0x0F); - RegisterWriteVerified(0x42, 0x02); - RegisterWriteVerified(0x45, 0x03); - RegisterWriteVerified(0x65, 0x00); - RegisterWriteVerified(0x67, 0x76); - RegisterWriteVerified(0x68, 0x76); - RegisterWriteVerified(0x6A, 0xC5); - RegisterWriteVerified(0x43, 0x00); - RegisterWriteVerified(0x7F, 0x06); - RegisterWriteVerified(0x4A, 0x18); - RegisterWriteVerified(0x4B, 0x0C); - RegisterWriteVerified(0x4C, 0x0C); - RegisterWriteVerified(0x4D, 0x0C); - RegisterWriteVerified(0x46, 0x0A); - RegisterWriteVerified(0x59, 0xCD); - RegisterWriteVerified(0x7F, 0x0A); - RegisterWriteVerified(0x4A, 0x2A); - RegisterWriteVerified(0x48, 0x96); - RegisterWriteVerified(0x52, 0xB4); - RegisterWriteVerified(0x7F, 0x00); - RegisterWriteVerified(0x5B, 0xA0); + RegisterWrite(0x7F, 0x00); + RegisterWrite(0x51, 0xFF); + RegisterWrite(0x4E, 0x2A); + RegisterWrite(0x66, 0x26); + RegisterWrite(0x7F, 0x14); + RegisterWrite(0x7E, 0x71); + RegisterWrite(0x55, 0x00); + RegisterWrite(0x59, 0x00); + RegisterWrite(0x6F, 0x2C); + RegisterWrite(0x7F, 0x05); + RegisterWrite(0x4D, 0xAC); + RegisterWrite(0x4E, 0x65); + RegisterWrite(0x7F, 0x09); + RegisterWrite(0x5C, 0xAF); + RegisterWrite(0x5F, 0xAF); + RegisterWrite(0x70, 0x00); + RegisterWrite(0x71, 0x00); + RegisterWrite(0x72, 0x00); + RegisterWrite(0x74, 0x14); + RegisterWrite(0x75, 0x14); + RegisterWrite(0x76, 0x06); + RegisterWrite(0x4E, 0x8F); + RegisterWrite(0x7F, 0x03); + RegisterWrite(0x64, 0x00); + RegisterWrite(0x65, 0x00); + RegisterWrite(0x66, 0x00); + RegisterWrite(0x55, 0x14); + RegisterWrite(0x56, 0x14); + RegisterWrite(0x57, 0x06); + RegisterWrite(0x4A, 0x20); + + RegisterWrite(0x4B, 0x20); + RegisterWrite(0x4E, 0x32); + RegisterWrite(0x69, 0xFE); + RegisterWrite(0x7F, 0x05); + RegisterWrite(0x69, 0x14); + RegisterWrite(0x47, 0x14); + RegisterWrite(0x48, 0x1C); + RegisterWrite(0x5A, 0x20); + RegisterWrite(0x75, 0xE5); + RegisterWrite(0x4A, 0x05); + RegisterWrite(0x42, 0x04); + RegisterWrite(0x45, 0x03); + RegisterWrite(0x65, 0x00); + RegisterWrite(0x67, 0x50); + RegisterWrite(0x68, 0x50); + RegisterWrite(0x6A, 0xC5); + RegisterWrite(0x43, 0x00); + RegisterWrite(0x7F, 0x06); + RegisterWrite(0x4A, 0x1E); + RegisterWrite(0x4B, 0x1E); + RegisterWrite(0x4C, 0x34); + RegisterWrite(0x4D, 0x34); + RegisterWrite(0x46, 0x32); + RegisterWrite(0x59, 0x0D); + RegisterWrite(0x7F, 0x0A); + RegisterWrite(0x4A, 0x2A); + RegisterWrite(0x48, 0x96); + RegisterWrite(0x52, 0xB4); + RegisterWrite(0x7F, 0x00); + RegisterWrite(0x5B, 0xA0); } -void PAA3905::EnhancedDetectionMode() -{ - RegisterWriteVerified(0x7F, 0x00); - RegisterWriteVerified(0x51, 0xFF); - RegisterWriteVerified(0x4E, 0x2A); - RegisterWriteVerified(0x66, 0x26); - RegisterWriteVerified(0x7F, 0x14); - RegisterWriteVerified(0x7E, 0x71); - RegisterWriteVerified(0x55, 0x00); - RegisterWriteVerified(0x59, 0x00); - RegisterWriteVerified(0x6F, 0x2C); - RegisterWriteVerified(0x7F, 0x05); - RegisterWriteVerified(0x4D, 0xAC); - RegisterWriteVerified(0x4E, 0x65); - RegisterWriteVerified(0x7F, 0x09); - RegisterWriteVerified(0x5C, 0xAF); - RegisterWriteVerified(0x5F, 0xAF); - RegisterWriteVerified(0x70, 0x00); - RegisterWriteVerified(0x71, 0x00); - RegisterWriteVerified(0x72, 0x00); - RegisterWriteVerified(0x74, 0x14); - RegisterWriteVerified(0x75, 0x14); - RegisterWriteVerified(0x76, 0x06); - RegisterWriteVerified(0x4E, 0x8F); - RegisterWriteVerified(0x7F, 0x03); - RegisterWriteVerified(0x64, 0x00); - RegisterWriteVerified(0x65, 0x00); - RegisterWriteVerified(0x66, 0x00); - RegisterWriteVerified(0x55, 0x14); - RegisterWriteVerified(0x56, 0x14); - RegisterWriteVerified(0x57, 0x06); - RegisterWriteVerified(0x4A, 0x20); - - RegisterWriteVerified(0x4B, 0x20); - RegisterWriteVerified(0x4E, 0x32); - RegisterWriteVerified(0x69, 0xFE); - RegisterWriteVerified(0x7F, 0x05); - RegisterWriteVerified(0x69, 0x14); - RegisterWriteVerified(0x47, 0x14); - RegisterWriteVerified(0x48, 0x1C); - RegisterWriteVerified(0x5A, 0x20); - RegisterWriteVerified(0x75, 0xE5); - RegisterWriteVerified(0x4A, 0x05); - RegisterWriteVerified(0x42, 0x04); - RegisterWriteVerified(0x45, 0x03); - RegisterWriteVerified(0x65, 0x00); - RegisterWriteVerified(0x67, 0x50); - RegisterWriteVerified(0x68, 0x50); - RegisterWriteVerified(0x6A, 0xC5); - RegisterWriteVerified(0x43, 0x00); - RegisterWriteVerified(0x7F, 0x06); - RegisterWriteVerified(0x4A, 0x1E); - RegisterWriteVerified(0x4B, 0x1E); - RegisterWriteVerified(0x4C, 0x34); - RegisterWriteVerified(0x4D, 0x34); - RegisterWriteVerified(0x46, 0x32); - RegisterWriteVerified(0x59, 0x0D); - RegisterWriteVerified(0x7F, 0x0A); - RegisterWriteVerified(0x4A, 0x2A); - RegisterWriteVerified(0x48, 0x96); - RegisterWriteVerified(0x52, 0xB4); - RegisterWriteVerified(0x7F, 0x00); - RegisterWriteVerified(0x5B, 0xA0); -} - -void PAA3905::ModeAuto012() +void PAA3905::ConfigureAutomaticModeSwitching() { // Automatic switching between Mode 0, 1 and 2: - RegisterWriteVerified(0x7F, 0x08); - RegisterWriteVerified(0x68, 0x02); - RegisterWriteVerified(0x7F, 0x00); + RegisterWrite(0x7F, 0x08); + 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 + // RegisterWrite(0x7F, 0x00); } void PAA3905::EnableLed() { // Enable LED_N controls - RegisterWriteVerified(0x7F, 0x14); - RegisterWriteVerified(0x6F, 0x0C); - RegisterWriteVerified(0x7F, 0x00); + RegisterWrite(0x7F, 0x14); + RegisterWrite(0x6F, 0x0C); + RegisterWrite(0x7F, 0x00); } -uint8_t PAA3905::RegisterRead(uint8_t reg, int retries) +bool PAA3905::UpdateMode(const uint8_t observation) { - for (int i = 0; i < retries; i++) { - px4_udelay(TIME_us_TSRAD); - uint8_t cmd[2] {reg, 0}; + bool mode_changed = false; - if (transfer(&cmd[0], &cmd[0], sizeof(cmd)) == 0) { - return cmd[1]; + // Bit [7:6] AMS mode + const uint8_t ams_mode = (Observation & (Bit7 | Bit6)) >> 5; + + 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; } - perf_count(_comms_errors); - return 0; + return mode_changed; +} + +uint8_t PAA3905::RegisterRead(uint8_t reg) +{ + // tSWR SPI Time Between Write And Read Commands + const hrt_abstime elapsed_last_write = hrt_elapsed_time(&_last_write_time); + + if (elapsed_last_write < TIME_TSWR_us) { + px4_udelay(TIME_TSWR_us - elapsed_last_write); + } + + // 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 PAA3905::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 PAA3905::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 PAA3905::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)) { + // 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; } - 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; @@ -444,37 +484,52 @@ void PAA3905::RunImpl() } buf{}; static_assert(sizeof(buf) == (14 + 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; } - CheckMode(); // update _mode variable + // 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 - const uint32_t shutter = (buf.data.Shutter_Upper << 16) | (buf.data.Shutter_Middle << 8) | buf.data.Shutter_Lower; + // 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) { @@ -513,96 +568,66 @@ void PAA3905::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 PAA3905::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 PAA3905::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/paa3905/PAA3905.hpp b/src/drivers/optical_flow/paa3905/PAA3905.hpp index 8235e923d3..337ed19659 100644 --- a/src/drivers/optical_flow/paa3905/PAA3905.hpp +++ b/src/drivers/optical_flow/paa3905/PAA3905.hpp @@ -32,9 +32,9 @@ ****************************************************************************/ /** - * @file paa3905.hpp + * @file PAA3905.hpp * - * Driver for the Pixart paa3905 optical flow sensors connected via SPI. + * Driver for the PAA3905E1-Q: Optical Motion Tracking Chip */ #pragma once @@ -48,7 +48,6 @@ #include #include #include -#include #include #include #include @@ -56,8 +55,8 @@ using namespace time_literals; using namespace PixArt_PAA3905; -#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 PAA3905 : public device::SPI, public I2CSPIDriver { @@ -78,59 +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 StandardDetectionSetting(); - void EnhancedDetectionMode(); - void ModeAuto012(); - - void CheckMode(); void Configure(); - void ResetAccumulatedData(); + void ConfigureAutomaticModeSwitching(); + + void ConfigureModeBright(); + void ConfigureModeLowLight(); + void ConfigureModeSuperLowLight(); + + void ConfigureStandardDetectionSetting(); + void ConfigureEnhancedDetectionMode(); + + void EnableLed(); + + bool UpdateMode(const uint8_t observation); 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")}; - - 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}; - - int _valid_count{0}; + uint32_t _scheduled_interval_us{SAMPLE_INTERVAL_MODE_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/paa3905/PixArt_PAA3905_Registers.hpp b/src/drivers/optical_flow/paa3905/PixArt_PAA3905_Registers.hpp index 8eb5a99602..de67e0cf4e 100644 --- a/src/drivers/optical_flow/paa3905/PixArt_PAA3905_Registers.hpp +++ b/src/drivers/optical_flow/paa3905/PixArt_PAA3905_Registers.hpp @@ -33,8 +33,8 @@ #pragma once -namespace PixArt_PAA3905 -{ +#include + // TODO: move to a central header static constexpr uint8_t Bit0 = (1 << 0); static constexpr uint8_t Bit1 = (1 << 1); @@ -45,19 +45,24 @@ static constexpr uint8_t Bit5 = (1 << 5); static constexpr uint8_t Bit6 = (1 << 6); static constexpr uint8_t Bit7 = (1 << 7); -static constexpr uint8_t PRODUCT_ID = 0xA2; -static constexpr uint8_t REVISION_ID = 0x00; +namespace PixArt_PAA3905 +{ + +static constexpr uint8_t PRODUCT_ID = 0xA2; +static constexpr uint8_t REVISION_ID = 0x00; static constexpr uint8_t PRODUCT_ID_INVERSE = 0x5D; -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 paa3905 -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, @@ -86,9 +91,20 @@ enum Register : uint8_t { Inverse_Product_ID = 0x5F, }; -// Observation +enum Motion_Bit : uint8_t { + MotionOccurred = Bit7, // Motion since last report + + ChallengingSurface = Bit0, // Challenging surface is detected +}; + enum Observation_Bit : uint8_t { - Reset = 0x5A, + // Bit [7:6] + AMS_mode_0 = 0, + AMS_mode_1 = Bit6, + AMS_mode_2 = Bit7, + + // Bit [5:0] + WorkingCorrectly = 0x3F, }; enum class Mode { diff --git a/src/drivers/optical_flow/paa3905/paa3905_main.cpp b/src/drivers/optical_flow/paa3905/paa3905_main.cpp index 6c5e16b2b3..ca43b70b7b 100644 --- a/src/drivers/optical_flow/paa3905/paa3905_main.cpp +++ b/src/drivers/optical_flow/paa3905/paa3905_main.cpp @@ -49,7 +49,7 @@ extern "C" __EXPORT int paa3905_main(int argc, char *argv[]) using ThisDriver = PAA3905; 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) { diff --git a/src/drivers/optical_flow/paa3905/parameters.c b/src/drivers/optical_flow/paa3905/parameters.c index 9aa2da7c63..2646721d94 100644 --- a/src/drivers/optical_flow/paa3905/parameters.c +++ b/src/drivers/optical_flow/paa3905/parameters.c @@ -32,7 +32,7 @@ ****************************************************************************/ /** - * paa3905 Optical Flow + * PAA3905 Optical Flow * * @reboot_required true *