diff --git a/src/drivers/optical_flow/pmw3901/CMakeLists.txt b/src/drivers/optical_flow/pmw3901/CMakeLists.txt index 33bb3c4511..a2cf883b2f 100644 --- a/src/drivers/optical_flow/pmw3901/CMakeLists.txt +++ b/src/drivers/optical_flow/pmw3901/CMakeLists.txt @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2019 PX4 Development Team. All rights reserved. +# Copyright (c) 2018-2022 PX4 Development Team. All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions @@ -38,4 +38,9 @@ px4_add_module( pmw3901_main.cpp PMW3901.cpp PMW3901.hpp + PixArt_PMW3901_Registers.hpp + DEPENDS + conversion + drivers__device + px4_work_queue ) diff --git a/src/drivers/optical_flow/pmw3901/PMW3901.cpp b/src/drivers/optical_flow/pmw3901/PMW3901.cpp index ae64cff8c7..d13a869e50 100644 --- a/src/drivers/optical_flow/pmw3901/PMW3901.cpp +++ b/src/drivers/optical_flow/pmw3901/PMW3901.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2018 PX4 Development Team. All rights reserved. + * Copyright (c) 2018-2022 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -33,74 +33,200 @@ #include "PMW3901.hpp" -static constexpr uint32_t TIME_us_TSWW = 11; // - actually 10.5us +static constexpr int16_t combine(uint8_t msb, uint8_t lsb) +{ + return (msb << 8u) | lsb; +} PMW3901::PMW3901(const I2CSPIDriverConfig &config) : SPI(config), I2CSPIDriver(config), - _sample_perf(perf_alloc(PC_ELAPSED, "pmw3901: read")), - _comms_errors(perf_alloc(PC_COUNT, "pmw3901: com err")), - _yaw_rotation(config.rotation) + _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) { + PX4_INFO("using yaw rotation %.3f degrees (%.3f radians)", + (double)yaw_rotation_degrees, (double)math::radians(yaw_rotation_degrees)); + + _rotation = matrix::Dcmf{matrix::Eulerf{0.f, 0.f, math::radians(yaw_rotation_degrees)}}; + + } else { + _rotation.identity(); + } } PMW3901::~PMW3901() { // free perf counters - perf_free(_sample_perf); - perf_free(_comms_errors); + perf_free(_cycle_perf); + perf_free(_interval_perf); + perf_free(_reset_perf); + perf_free(_false_motion_perf); + perf_free(_no_motion_interrupt_perf); } -int -PMW3901::sensorInit() +int PMW3901::init() { - uint8_t data[5] {}; + /* do SPI init (and probe) first */ + if (SPI::init() != OK) { + return PX4_ERROR; + } - // Power on reset - writeRegister(0x3A, 0x5A); - usleep(5000); + Configure(); - // Reading the motion registers one time - readRegister(0x02, &data[0], 1); - readRegister(0x03, &data[1], 1); - readRegister(0x04, &data[2], 1); - readRegister(0x05, &data[3], 1); - readRegister(0x06, &data[4], 1); + return PX4_OK; +} - usleep(1000); +int PMW3901::probe() +{ + 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); + 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; + } + + return PX4_ERROR; +} + +int PMW3901::DataReadyInterruptCallback(int irq, void *context, void *arg) +{ + static_cast(arg)->DataReady(); + return 0; +} + +void PMW3901::DataReady() +{ + _drdy_timestamp_sample.store(hrt_absolute_time()); + ScheduleNow(); +} + +bool PMW3901::DataReadyInterruptConfigure() +{ + if (_drdy_gpio == 0) { + _data_ready_interrupt_enabled = false; + return false; + } + + // 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 PMW3901::DataReadyInterruptDisable() +{ + _data_ready_interrupt_enabled = false; + + if (_drdy_gpio == 0) { + return false; + } + + return px4_arch_gpiosetevent(_drdy_gpio, false, false, false, nullptr, nullptr) == 0; +} + +void PMW3901::exit_and_cleanup() +{ + DataReadyInterruptDisable(); + I2CSPIDriverBase::exit_and_cleanup(); +} + +void PMW3901::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 PMW3901::Configure() +{ + Reset(); + + SetPerformanceOptimizationRegisters(); + + if (DataReadyInterruptConfigure()) { + // backup schedule + ScheduleDelayed(500_ms); + + } else { + ScheduleOnInterval(_scheduled_interval_us, _scheduled_interval_us); + } +} + +void PMW3901::SetPerformanceOptimizationRegisters() +{ // set performance optimization registers // from PixArt PMW3901MB Optical Motion Tracking chip demo kit V3.20 (21 Aug 2018) unsigned char v = 0; unsigned char c1 = 0; unsigned char c2 = 0; - writeRegister(0x7F, 0x00); - writeRegister(0x55, 0x01); - writeRegister(0x50, 0x07); - writeRegister(0x7f, 0x0e); - writeRegister(0x43, 0x10); + RegisterWrite(0x7F, 0x00); + RegisterWrite(0x55, 0x01); + RegisterWrite(0x50, 0x07); + RegisterWrite(0x7f, 0x0e); + RegisterWrite(0x43, 0x10); - readRegister(0x67, &v, 1); + v = RegisterRead(0x67); // if bit7 is set if (v & (1 << 7)) { - writeRegister(0x48, 0x04); + RegisterWrite(0x48, 0x04); } else { - writeRegister(0x48, 0x02); + RegisterWrite(0x48, 0x02); } - writeRegister(0x7F, 0x00); - writeRegister(0x51, 0x7b); - writeRegister(0x50, 0x00); - writeRegister(0x55, 0x00); + RegisterWrite(0x7F, 0x00); + RegisterWrite(0x51, 0x7b); + RegisterWrite(0x50, 0x00); + RegisterWrite(0x55, 0x00); - writeRegister(0x7F, 0x0e); - readRegister(0x73, &v, 1); + RegisterWrite(0x7F, 0x0e); + v = RegisterRead(0x73); if (v == 0) { - readRegister(0x70, &c1, 1); + c1 = RegisterRead(0x70); if (c1 <= 28) { c1 = c1 + 14; @@ -113,304 +239,257 @@ PMW3901::sensorInit() c1 = 0x3F; } - readRegister(0x71, &c2, 1); + c2 = RegisterRead(0x71); c2 = ((unsigned short)c2 * 45) / 100; - writeRegister(0x7f, 0x00); - writeRegister(0x61, 0xAD); - writeRegister(0x51, 0x70); - writeRegister(0x7f, 0x0e); - writeRegister(0x70, c1); - writeRegister(0x71, c2); + RegisterWrite(0x7f, 0x00); + RegisterWrite(0x61, 0xAD); + RegisterWrite(0x51, 0x70); + RegisterWrite(0x7f, 0x0e); + RegisterWrite(0x70, c1); + RegisterWrite(0x71, c2); } - writeRegister(0x7F, 0x00); - writeRegister(0x61, 0xAD); - writeRegister(0x7F, 0x03); - writeRegister(0x40, 0x00); - writeRegister(0x7F, 0x05); - writeRegister(0x41, 0xB3); - writeRegister(0x43, 0xF1); - writeRegister(0x45, 0x14); - writeRegister(0x5B, 0x32); - writeRegister(0x5F, 0x34); - writeRegister(0x7B, 0x08); - writeRegister(0x7F, 0x06); - writeRegister(0x44, 0x1B); - writeRegister(0x40, 0xBF); - writeRegister(0x4E, 0x3F); - writeRegister(0x7F, 0x08); - writeRegister(0x65, 0x20); - writeRegister(0x6A, 0x18); - writeRegister(0x7F, 0x09); - writeRegister(0x4F, 0xAF); - writeRegister(0x5F, 0x40); - writeRegister(0x48, 0x80); - writeRegister(0x49, 0x80); - writeRegister(0x57, 0x77); - writeRegister(0x60, 0x78); - writeRegister(0x61, 0x78); - writeRegister(0x62, 0x08); - writeRegister(0x63, 0x50); - writeRegister(0x7F, 0x0A); - writeRegister(0x45, 0x60); - writeRegister(0x7F, 0x00); - writeRegister(0x4D, 0x11); - writeRegister(0x55, 0x80); - writeRegister(0x74, 0x21); - writeRegister(0x75, 0x1F); - writeRegister(0x4A, 0x78); - writeRegister(0x4B, 0x78); - writeRegister(0x44, 0x08); - writeRegister(0x45, 0x50); - writeRegister(0x64, 0xFF); - writeRegister(0x65, 0x1F); - writeRegister(0x7F, 0x14); - writeRegister(0x65, 0x67); - writeRegister(0x66, 0x08); - writeRegister(0x63, 0x70); - writeRegister(0x7F, 0x15); - writeRegister(0x48, 0x48); - writeRegister(0x7F, 0x07); - writeRegister(0x41, 0x0D); - writeRegister(0x43, 0x14); - writeRegister(0x4B, 0x0E); - writeRegister(0x45, 0x0F); - writeRegister(0x44, 0x42); - writeRegister(0x4C, 0x80); - writeRegister(0x7F, 0x10); - writeRegister(0x5B, 0x02); - writeRegister(0x7F, 0x07); - writeRegister(0x40, 0x41); - writeRegister(0x70, 0x00); + RegisterWrite(0x7F, 0x00); + RegisterWrite(0x61, 0xAD); + RegisterWrite(0x7F, 0x03); + RegisterWrite(0x40, 0x00); + RegisterWrite(0x7F, 0x05); + RegisterWrite(0x41, 0xB3); + RegisterWrite(0x43, 0xF1); + RegisterWrite(0x45, 0x14); + RegisterWrite(0x5B, 0x32); + RegisterWrite(0x5F, 0x34); + RegisterWrite(0x7B, 0x08); + RegisterWrite(0x7F, 0x06); + RegisterWrite(0x44, 0x1B); + RegisterWrite(0x40, 0xBF); + RegisterWrite(0x4E, 0x3F); + RegisterWrite(0x7F, 0x08); + RegisterWrite(0x65, 0x20); + RegisterWrite(0x6A, 0x18); + RegisterWrite(0x7F, 0x09); + RegisterWrite(0x4F, 0xAF); + RegisterWrite(0x5F, 0x40); + RegisterWrite(0x48, 0x80); + RegisterWrite(0x49, 0x80); + RegisterWrite(0x57, 0x77); + RegisterWrite(0x60, 0x78); + RegisterWrite(0x61, 0x78); + RegisterWrite(0x62, 0x08); + RegisterWrite(0x63, 0x50); + RegisterWrite(0x7F, 0x0A); + RegisterWrite(0x45, 0x60); + RegisterWrite(0x7F, 0x00); + RegisterWrite(0x4D, 0x11); + RegisterWrite(0x55, 0x80); + RegisterWrite(0x74, 0x21); + RegisterWrite(0x75, 0x1F); + RegisterWrite(0x4A, 0x78); + RegisterWrite(0x4B, 0x78); + RegisterWrite(0x44, 0x08); + RegisterWrite(0x45, 0x50); + RegisterWrite(0x64, 0xFF); + RegisterWrite(0x65, 0x1F); + RegisterWrite(0x7F, 0x14); + RegisterWrite(0x65, 0x67); + RegisterWrite(0x66, 0x08); + RegisterWrite(0x63, 0x70); + RegisterWrite(0x7F, 0x15); + RegisterWrite(0x48, 0x48); + RegisterWrite(0x7F, 0x07); + RegisterWrite(0x41, 0x0D); + RegisterWrite(0x43, 0x14); + RegisterWrite(0x4B, 0x0E); + RegisterWrite(0x45, 0x0F); + RegisterWrite(0x44, 0x42); + RegisterWrite(0x4C, 0x80); + RegisterWrite(0x7F, 0x10); + RegisterWrite(0x5B, 0x02); + RegisterWrite(0x7F, 0x07); + RegisterWrite(0x40, 0x41); + RegisterWrite(0x70, 0x00); - px4_usleep(10000); // delay 10ms + px4_usleep(10'000); // delay 10ms - writeRegister(0x32, 0x44); - writeRegister(0x7F, 0x07); - writeRegister(0x40, 0x40); - writeRegister(0x7F, 0x06); - writeRegister(0x62, 0xF0); - writeRegister(0x63, 0x00); - writeRegister(0x7F, 0x0D); - writeRegister(0x48, 0xC0); - writeRegister(0x6F, 0xD5); - writeRegister(0x7F, 0x00); - writeRegister(0x5B, 0xA0); - writeRegister(0x4E, 0xA8); - writeRegister(0x5A, 0x50); - writeRegister(0x40, 0x80); - - return PX4_OK; + RegisterWrite(0x32, 0x44); + RegisterWrite(0x7F, 0x07); + RegisterWrite(0x40, 0x40); + RegisterWrite(0x7F, 0x06); + RegisterWrite(0x62, 0xF0); + RegisterWrite(0x63, 0x00); + RegisterWrite(0x7F, 0x0D); + RegisterWrite(0x48, 0xC0); + RegisterWrite(0x6F, 0xD5); + RegisterWrite(0x7F, 0x00); + RegisterWrite(0x5B, 0xA0); + RegisterWrite(0x4E, 0xA8); + RegisterWrite(0x5A, 0x50); + RegisterWrite(0x40, 0x80); } -int -PMW3901::init() +uint8_t PMW3901::RegisterRead(uint8_t reg) { - // get yaw rotation from sensor frame to body frame - param_t rot = param_find("SENS_FLOW_ROT"); + // tSWR SPI Time Between Write And Read Commands + const hrt_abstime elapsed_last_write = hrt_elapsed_time(&_last_write_time); - if (rot != PARAM_INVALID) { - int32_t val = 0; - param_get(rot, &val); - - _yaw_rotation = (enum Rotation)val; + if (elapsed_last_write < TIME_TSWR_us) { + px4_udelay(TIME_TSWR_us - elapsed_last_write); } - /* For devices competing with NuttX SPI drivers on a bus (Crazyflie SD Card expansion board) */ - SPI::set_lockmode(LOCK_THREADS); + // tSRW/tSRR SPI Time Between Read And Subsequent Commands + const hrt_abstime elapsed_last_read = hrt_elapsed_time(&_last_read_time); - /* do SPI init (and probe) first */ - if (SPI::init() != OK) { - return PX4_ERROR; + if (elapsed_last_write < TIME_TSRW_TSRR_us) { + px4_udelay(TIME_TSRW_TSRR_us - elapsed_last_read); } - sensorInit(); - - _previous_collect_timestamp = hrt_absolute_time(); - - start(); - - return PX4_OK; -} - -int -PMW3901::probe() -{ - uint8_t data[2] {}; - - readRegister(0x00, &data[0], 1); // chip id - - // Test the SPI communication, checking chipId and inverse chipId - if (data[0] == 0x49) { - return OK; - } - - // not found on any address - return -EIO; -} - -int -PMW3901::readRegister(unsigned reg, uint8_t *data, unsigned count) -{ - uint8_t cmd[5]; // read up to 4 bytes - + 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); - int ret = transfer(&cmd[0], &cmd[0], count + 1); - - if (OK != ret) { - perf_count(_comms_errors); - DEVICE_LOG("spi::transfer returned %d", ret); - return ret; - } - - memcpy(&data[0], &cmd[1], count); - - return ret; + return cmd[1]; } -int -PMW3901::writeRegister(unsigned reg, uint8_t data) +void PMW3901::RegisterWrite(uint8_t reg, uint8_t data) { - uint8_t cmd[2]; // write 1 byte - int ret; + // 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; - - ret = transfer(&cmd[0], nullptr, 2); - - if (OK != ret) { - perf_count(_comms_errors); - DEVICE_LOG("spi::transfer returned %d", ret); - return ret; - } - - px4_usleep(TIME_us_TSWW); - - return ret; + transfer(&cmd[0], nullptr, sizeof(cmd)); + hrt_store_absolute_time(&_last_write_time); } -void -PMW3901::RunImpl() +void PMW3901::RunImpl() { - perf_begin(_sample_perf); + const hrt_abstime now = hrt_absolute_time(); - int16_t delta_x_raw = 0; - int16_t delta_y_raw = 0; - uint8_t qual = 0; - float delta_x = 0.0f; - float delta_y = 0.0f; + perf_begin(_cycle_perf); + perf_count(_interval_perf); - uint64_t timestamp = hrt_absolute_time(); - uint64_t dt_flow = timestamp - _previous_collect_timestamp; - _previous_collect_timestamp = timestamp; - - _flow_dt_sum_usec += dt_flow; - - readMotionCount(delta_x_raw, delta_y_raw, qual); - - if (qual > 0) { - _flow_sum_x += delta_x_raw; - _flow_sum_y += delta_y_raw; - _flow_sample_counter ++; - _flow_quality_sum += qual; - } - - // returns if the collect time has not been reached - if (_flow_dt_sum_usec < _collect_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; } - delta_x = (float)_flow_sum_x / 385.0f; // proportional factor + convert from pixels to radians - delta_y = (float)_flow_sum_y / 385.0f; // proportional factor + convert from pixels to radians + hrt_abstime timestamp_sample = 0; - sensor_optical_flow_s report{}; - report.timestamp_sample = timestamp; + 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); - report.pixel_flow[0] = static_cast(delta_x); - report.pixel_flow[1] = static_cast(delta_y); + if (now < drdy_timestamp_sample + _scheduled_interval_us) { + timestamp_sample = drdy_timestamp_sample; - // rotate measurements in yaw from sensor frame to body frame according to parameter SENS_FLOW_ROT - float zeroval = 0.0f; - rotate_3f(_yaw_rotation, report.pixel_flow[0], report.pixel_flow[1], zeroval); + } else { + perf_count(_no_motion_interrupt_perf); + } - report.integration_timespan_us = _flow_dt_sum_usec; // microseconds - - report.quality = _flow_sample_counter > 0 ? _flow_quality_sum / _flow_sample_counter : 0; - - /* 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 = 5.0f; // Datasheet: 7.4 rad/s - report.min_ground_distance = 0.1f; // Datasheet: 80mm - report.max_ground_distance = 30.0f; // Datasheet: infinity - - _flow_dt_sum_usec = 0; - _flow_sum_x = 0; - _flow_sum_y = 0; - _flow_sample_counter = 0; - _flow_quality_sum = 0; - - report.timestamp = hrt_absolute_time(); - _sensor_optical_flow_pub.publish(report); - - perf_end(_sample_perf); -} - -int -PMW3901::readMotionCount(int16_t &deltaX, int16_t &deltaY, uint8_t &qual) -{ - uint8_t data[12] = { DIR_READ(0x02), 0, DIR_READ(0x03), 0, DIR_READ(0x04), 0, - DIR_READ(0x05), 0, DIR_READ(0x06), 0, DIR_READ(0x07), 0 - }; - - int ret = transfer(&data[0], &data[0], 12); - - if (OK != ret) { - qual = 0; - perf_count(_comms_errors); - DEVICE_LOG("spi::transfer returned %d", ret); - return ret; + // push backup schedule back + ScheduleDelayed(500_ms); } - deltaX = ((int16_t)data[5] << 8) | data[3]; - deltaY = ((int16_t)data[9] << 8) | data[7]; + struct TransferBuffer { + uint8_t cmd = Register::Motion_Burst; + BURST_TRANSFER data{}; + } buf{}; + static_assert(sizeof(buf) == (12 + 1)); - // If the reported flow is impossibly large, we just got garbage from the SPI - if (deltaX > 240 || deltaY > 240 || deltaX < -240 || deltaY < -240) { - qual = 0; - - } else { - qual = data[11]; + if (timestamp_sample == 0) { + timestamp_sample = hrt_absolute_time(); } - ret = OK; + if (transfer((uint8_t *)&buf, (uint8_t *)&buf, sizeof(buf)) != 0) { + perf_end(_cycle_perf); + return; + } - return ret; + 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 + // SQUAL < 25, Shutter ≥ 0x1F00 + + // 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); + + // quality < 25 (0x19) and shutter >= 7936 (0x1F00) + if ((buf.data.SQUAL < 0x19) && (shutter >= 0x1F00)) { + // false motion report, discarding + perf_count(_false_motion_perf); + data_valid = false; + } + + 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 + + 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) / 385.0f; // proportional factor + convert from pixels to radians + report.pixel_flow[1] = pixel_flow_rotated(1) / 385.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; + } + } + + perf_end(_cycle_perf); } -void -PMW3901::start() -{ - // schedule a cycle to start things - ScheduleOnInterval(PMW3901_SAMPLE_INTERVAL, PMW3901_US); -} - -void -PMW3901::stop() -{ - ScheduleClear(); -} - -void -PMW3901::print_status() +void PMW3901::print_status() { I2CSPIDriverBase::print_status(); - perf_print_counter(_sample_perf); - perf_print_counter(_comms_errors); + + perf_print_counter(_cycle_perf); + perf_print_counter(_interval_perf); + perf_print_counter(_reset_perf); + perf_print_counter(_false_motion_perf); + perf_print_counter(_no_motion_interrupt_perf); } diff --git a/src/drivers/optical_flow/pmw3901/PMW3901.hpp b/src/drivers/optical_flow/pmw3901/PMW3901.hpp index a84f786028..de4f1d52d9 100644 --- a/src/drivers/optical_flow/pmw3901/PMW3901.hpp +++ b/src/drivers/optical_flow/pmw3901/PMW3901.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2018-2019 PX4 Development Team. All rights reserved. + * Copyright (c) 2018-2022 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -33,89 +33,89 @@ /** * @file PMW3901.hpp - * @author Daniele Pettenuzzo * - * Driver for the pmw3901 optical flow sensor connected via SPI. + * Driver for the PMW3901MB-TXQT: Optical Motion Tracking Chip */ +#pragma once + +#include "PixArt_PMW3901_Registers.hpp" + #include #include #include -#include +#include #include #include #include -#include #include #include #include -#include -/* Configuration Constants */ +using namespace time_literals; +using namespace PixArt_PMW3901; -#define PMW3901_SPI_BUS_SPEED (2000000L) // 2MHz - -#define DIR_WRITE(a) ((a) | (1 << 7)) -#define DIR_READ(a) ((a) & 0x7f) - -/* PMW3901 Registers addresses */ -#define PMW3901_US 1000 /* 1 ms */ -#define PMW3901_SAMPLE_INTERVAL 10000 /* 10 ms */ +#define DIR_WRITE(a) ((a) | Bit7) +#define DIR_READ(a) ((a) & 0x7F) class PMW3901 : public device::SPI, public I2CSPIDriver { public: PMW3901(const I2CSPIDriverConfig &config); - virtual ~PMW3901(); static void print_usage(); - virtual int init(); + int init() override; - void print_status(); + void print_status() override; void RunImpl(); -protected: - virtual int probe(); - private: + void exit_and_cleanup() override; - const uint64_t _collect_time{15000}; // usecs, ensures flow data is published every second iteration of Run() (100Hz -> 50Hz) + 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); + void RegisterWrite(uint8_t reg, uint8_t data); + + void Configure(); + + void SetPerformanceOptimizationRegisters(); uORB::PublicationMulti _sensor_optical_flow_pub{ORB_ID(sensor_optical_flow)}; - perf_counter_t _sample_perf; - perf_counter_t _comms_errors; + 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 _no_motion_interrupt_perf{nullptr}; - uint64_t _previous_collect_timestamp{0}; + const spi_drdy_gpio_t _drdy_gpio; - enum Rotation _yaw_rotation; + matrix::Dcmf _rotation; - int _flow_sum_x{0}; - int _flow_sum_y{0}; - uint64_t _flow_dt_sum_usec{0}; - uint16_t _flow_quality_sum{0}; - uint8_t _flow_sample_counter{0}; + int _discard_reading{3}; - /** - * Initialise the automatic measurement state machine and start it. - * - * @note This function is called at open and error time. It might make sense - * to make it more aggressive about resetting the bus in case of errors. - */ - void start(); + uint32_t _scheduled_interval_us{SAMPLE_INTERVAL_MODE}; - /** - * Stop the automatic measurement state machine. - */ - void stop(); + px4::atomic _drdy_timestamp_sample{0}; + bool _data_ready_interrupt_enabled{false}; + hrt_abstime _last_write_time{0}; + hrt_abstime _last_read_time{0}; - int readRegister(unsigned reg, uint8_t *data, unsigned count); - int writeRegister(unsigned reg, uint8_t data); + // 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; - int sensorInit(); - int readMotionCount(int16_t &deltaX, int16_t &deltaY, uint8_t &qual); + hrt_abstime _last_good_data{0}; + hrt_abstime _last_reset{0}; }; diff --git a/src/drivers/optical_flow/pmw3901/PixArt_PMW3901_Registers.hpp b/src/drivers/optical_flow/pmw3901/PixArt_PMW3901_Registers.hpp new file mode 100644 index 0000000000..78720d9e2d --- /dev/null +++ b/src/drivers/optical_flow/pmw3901/PixArt_PMW3901_Registers.hpp @@ -0,0 +1,107 @@ +/**************************************************************************** + * + * Copyright (c) 2018-2022 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#include + +// TODO: move to a central header +static constexpr uint8_t Bit0 = (1 << 0); +static constexpr uint8_t Bit1 = (1 << 1); +static constexpr uint8_t Bit2 = (1 << 2); +static constexpr uint8_t Bit3 = (1 << 3); +static constexpr uint8_t Bit4 = (1 << 4); +static constexpr uint8_t Bit5 = (1 << 5); +static constexpr uint8_t Bit6 = (1 << 6); +static constexpr uint8_t Bit7 = (1 << 7); + +namespace PixArt_PMW3901 +{ + +static constexpr uint8_t PRODUCT_ID = 0x49; +static constexpr uint8_t REVISION_ID = 0x00; +static constexpr uint8_t PRODUCT_ID_INVERSE = 0xB6; + +static constexpr uint32_t SAMPLE_INTERVAL_MODE{1000000 / 126}; // 126 fps + +static constexpr uint32_t SPI_SPEED = 2 * 1000 * 1000; // 2MHz SPI serial interface + +// Various time delays +static constexpr uint32_t TIME_TSWW_us = 45; // SPI Time Between Write Commands +static constexpr uint32_t TIME_TSWR_us = 45; // SPI Time Between Write and Read Commands +static constexpr uint32_t TIME_TSRW_TSRR_us = 20; // SPI Time Between Read And Subsequent Commands +static constexpr uint32_t TIME_TSRAD_us = 35; // SPI Read Address-Data Delay + +enum Register : uint8_t { + Product_ID = 0x00, + Revision_ID = 0x01, + Motion = 0x02, + Delta_X_L = 0x03, + Delta_X_H = 0x04, + Delta_Y_L = 0x05, + Delta_Y_H = 0x06, + Squal = 0x07, + RawData_Sum = 0x08, + Maximum_RawData = 0x09, + Minimum_RawData = 0x0A, + Shutter_Lower = 0x0B, + Shutter_Upper = 0x0C, + + Observation = 0x15, + Motion_Burst = 0x16, + + Power_Up_Reset = 0x3A, + + Inverse_Product_ID = 0x5F, +}; + +enum Motion_Bit : uint8_t { + MOT = Bit7, // Motion since last report +}; + +struct BURST_TRANSFER { + uint8_t Motion; + uint8_t Observation; + uint8_t Delta_X_L; + uint8_t Delta_X_H; + uint8_t Delta_Y_L; + uint8_t Delta_Y_H; + uint8_t SQUAL; + uint8_t RawData_Sum; + uint8_t Maximum_RawData; + uint8_t Minimum_RawData; + uint8_t Shutter_Upper; + uint8_t Shutter_Lower; +}; + +} diff --git a/src/drivers/optical_flow/pmw3901/parameters.c b/src/drivers/optical_flow/pmw3901/parameters.c index 74e9a48266..1ff10bb094 100644 --- a/src/drivers/optical_flow/pmw3901/parameters.c +++ b/src/drivers/optical_flow/pmw3901/parameters.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2019 PX4 Development Team. All rights reserved. + * Copyright (c) 2018-2022 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions diff --git a/src/drivers/optical_flow/pmw3901/pmw3901_main.cpp b/src/drivers/optical_flow/pmw3901/pmw3901_main.cpp index c68f2e4fc8..fa58b4fd20 100644 --- a/src/drivers/optical_flow/pmw3901/pmw3901_main.cpp +++ b/src/drivers/optical_flow/pmw3901/pmw3901_main.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2018, 2021 PX4 Development Team. All rights reserved. + * Copyright (c) 2018-2022 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -34,31 +34,28 @@ #include "PMW3901.hpp" #include -extern "C" __EXPORT int pmw3901_main(int argc, char *argv[]); - -void -PMW3901::print_usage() +void PMW3901::print_usage() { PRINT_MODULE_USAGE_NAME("pmw3901", "driver"); PRINT_MODULE_USAGE_COMMAND("start"); PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(false, true); - PRINT_MODULE_USAGE_PARAM_INT('R', 0, 0, 35, "Rotation", true); + PRINT_MODULE_USAGE_PARAM_INT('Y', 0, 0, 359, "custom yaw rotation (degrees)", true); PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); } -int -pmw3901_main(int argc, char *argv[]) +extern "C" __EXPORT int pmw3901_main(int argc, char *argv[]) { - int ch; + int ch = 0; using ThisDriver = PMW3901; BusCLIArguments cli{false, true}; + cli.custom1 = -1; cli.spi_mode = SPIDEV_MODE0; - cli.default_spi_frequency = PMW3901_SPI_BUS_SPEED; + cli.default_spi_frequency = SPI_SPEED; - while ((ch = cli.getOpt(argc, argv, "R:")) != EOF) { + while ((ch = cli.getOpt(argc, argv, "Y:")) != EOF) { switch (ch) { - case 'R': - cli.rotation = (enum Rotation)atoi(cli.optArg()); + case 'Y': + cli.custom1 = atoi(cli.optArg()); break; } } @@ -74,13 +71,11 @@ pmw3901_main(int argc, char *argv[]) if (!strcmp(verb, "start")) { return ThisDriver::module_start(cli, iterator); - } - if (!strcmp(verb, "stop")) { + } else if (!strcmp(verb, "stop")) { return ThisDriver::module_stop(iterator); - } - if (!strcmp(verb, "status")) { + } else if (!strcmp(verb, "status")) { return ThisDriver::module_status(iterator); }