fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-03 15:28:53 +08:00
Code Issues Packages Projects Releases Wiki Activity

drivers/optical_flow/paa3905: cleanup/overhaul

Browse Source 
- remove internal accumulation and publish every valid raw sample synchronized with sensor
 - store timestamp_sample from motion interrupt
 - improve timing requirements from datasheet (minimum delays after register read/write)
This commit is contained in:
Daniel Agar
2022-06-20 20:56:56 -04:00
parent 1fbe3c4ab3
commit e5f081d9ac
5 changed files with 439 additions and 397 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/optical_flow/paa3905/PAA3905.cpp
+370 -345
View File
@@ -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);
}
src/drivers/optical_flow/paa3905/PAA3905.hpp
+39 -38
View File
@@ -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 <drivers/device/spi.h>
#include <conversion/rotation.h>
#include <lib/perf/perf_counter.h>
#include <lib/parameters/param.h>
#include <drivers/drv_hrt.h>
#include <uORB/PublicationMulti.hpp>
#include <uORB/topics/sensor_optical_flow.h>
@@ -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<PAA3905>
{
@@ -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_s> _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<hrt_abstime> _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};
};
src/drivers/optical_flow/paa3905/PixArt_PAA3905_Registers.hpp
+28 -12
View File
@@ -33,8 +33,8 @@
#pragma once
namespace PixArt_PAA3905
{
#include <cstdint>
// 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 {
src/drivers/optical_flow/paa3905/paa3905_main.cpp
+1 -1
View File
@@ -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) {
src/drivers/optical_flow/paa3905/parameters.c
+1 -1
View File
@@ -32,7 +32,7 @@
****************************************************************************/
/**
* paa3905 Optical Flow
* PAA3905 Optical Flow
*
* @reboot_required true
*