dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

drivers/optical_flow/paa3905: backup scheduling to fetch 0 flow 

- this sensor provides a MOTION interrupt used for default scheduling, but we also need to publish
   zero flow information
This commit is contained in:
Daniel Agar 2023-09-15 14:57:02 -04:00
parent efbed3bfc1
commit d351f16d04
2 changed files with 81 additions and 22 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

89
src/drivers/optical_flow/paa3905/PAA3905.cpp
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2022 PX4 Development Team. All rights reserved.
* Copyright (c) 2022-2023 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
@ -194,13 +194,13 @@ void PAA3905::RunImpl()
_state = STATE::READ;
if (DataReadyInterruptConfigure()) {
_data_ready_interrupt_enabled = true;
_motion_interrupt_enabled = true;
// backup schedule as a watchdog timeout
ScheduleDelayed(1_s);
} else {
_data_ready_interrupt_enabled = false;
_motion_interrupt_enabled = false;
ScheduleOnInterval(_scheduled_interval_us, _scheduled_interval_us);
}
@ -222,7 +222,7 @@ void PAA3905::RunImpl()
case STATE::READ: {
hrt_abstime timestamp_sample = now;
if (_data_ready_interrupt_enabled) {
if (_motion_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);
@ -258,12 +258,6 @@ void PAA3905::RunImpl()
PX4_ERR("invalid RawData_Sum > 0x98");
}
// Number of Features = SQUAL * 4
// RawData_Sum maximum register value is 0x98
bool data_valid = (buffer.data.SQUAL > 0)
&& (buffer.data.RawData_Sum <= 0x98)
&& (_discard_reading == 0);
// Bit [5:0] check if chip is working correctly
// 0x3F: chip is working correctly
if ((buffer.data.Observation & 0x3F) != 0x3F) {
@ -313,7 +307,7 @@ void PAA3905::RunImpl()
if (prev_mode != _mode) {
// update scheduling on mode change
if (!_data_ready_interrupt_enabled) {
if (!_motion_interrupt_enabled) {
ScheduleOnInterval(_scheduled_interval_us, _scheduled_interval_us);
}
}
@ -348,6 +342,14 @@ void PAA3905::RunImpl()
const uint32_t shutter = (shutter_upper << 16) | (shutter_middle << 8) | shutter_lower;
// Number of Features = SQUAL * 4
// RawData_Sum maximum register value is 0x98
bool data_valid = (buffer.data.SQUAL > 0)
&& (buffer.data.RawData_Sum > 0)
&& (buffer.data.RawData_Sum <= 0x98)
&& (shutter > 0)
&& (_discard_reading == 0);
switch (_mode) {
case Mode::Bright:
sensor_optical_flow.integration_timespan_us = SAMPLE_INTERVAL_MODE_0;
@ -392,12 +394,17 @@ void PAA3905::RunImpl()
// motion in burst transfer
const bool motion_reported = (buffer.data.Motion & Motion_Bit::MotionOccurred);
if (data_valid) {
if (motion_reported) {
// only populate flow if data valid (motion and quality > 0)
const int16_t delta_x_raw = combine(buffer.data.Delta_X_H, buffer.data.Delta_X_L);
const int16_t delta_y_raw = combine(buffer.data.Delta_Y_H, buffer.data.Delta_Y_L);
const int16_t delta_x_raw = combine(buffer.data.Delta_X_H, buffer.data.Delta_X_L);
const int16_t delta_y_raw = combine(buffer.data.Delta_Y_H, buffer.data.Delta_Y_L);
if (data_valid) {
const bool zero_flow = (delta_x_raw == 0) && (delta_y_raw == 0);
const bool little_to_no_flow = (abs(delta_x_raw) <= 1) && (abs(delta_y_raw) <= 1);
bool publish = false;
if (motion_reported) {
// 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};
@ -412,15 +419,53 @@ void PAA3905::RunImpl()
sensor_optical_flow.pixel_flow[1] = pixel_flow_rotated(1) * SCALE;
sensor_optical_flow.quality = buffer.data.SQUAL;
publish = true;
_last_motion = timestamp_sample;
} else if (zero_flow && (timestamp_sample > _last_motion)) {
// no motion, but burst read looks valid and we should have seen new data by now if there was any motion
const bool burst_read_changed = (delta_x_raw != _delta_x_raw_prev) || (delta_y_raw != _delta_y_raw_prev)
|| (shutter != _shutter_prev)
|| (buffer.data.RawData_Sum != _raw_data_sum_prev)
|| (buffer.data.SQUAL != _quality_prev);
if (burst_read_changed) {
sensor_optical_flow.pixel_flow[0] = 0;
sensor_optical_flow.pixel_flow[1] = 0;
sensor_optical_flow.quality = buffer.data.SQUAL;
publish = true;
}
}
// only publish when there's motion or at least every second
if (motion_reported || (hrt_elapsed_time(&_last_publish) >= kBackupScheduleIntervalUs)) {
// only publish when there's valid data or on timeout
if (publish || (hrt_elapsed_time(&_last_publish) >= kBackupScheduleIntervalUs)) {
sensor_optical_flow.timestamp = hrt_absolute_time();
_sensor_optical_flow_pub.publish(sensor_optical_flow);
_last_publish = sensor_optical_flow.timestamp;
_last_publish = sensor_optical_flow.timestamp_sample;
}
// backup schedule if we're reliant on the motion interrupt and there's very little flow
if (_motion_interrupt_enabled && little_to_no_flow) {
switch (_mode) {
case Mode::Bright:
ScheduleDelayed(SAMPLE_INTERVAL_MODE_0);
break;
case Mode::LowLight:
ScheduleDelayed(SAMPLE_INTERVAL_MODE_1);
break;
case Mode::SuperLowLight:
ScheduleDelayed(SAMPLE_INTERVAL_MODE_2);
break;
}
}
success = true;
@ -430,6 +475,12 @@ void PAA3905::RunImpl()
}
}
_delta_x_raw_prev = delta_x_raw;
_delta_y_raw_prev = delta_y_raw;
_shutter_prev = shutter;
_raw_data_sum_prev = buffer.data.RawData_Sum;
_quality_prev = buffer.data.SQUAL;
} else {
perf_count(_bad_transfer_perf);
}

14
src/drivers/optical_flow/paa3905/PAA3905.hpp
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2022 PX4 Development Team. All rights reserved.
* Copyright (c) 2022-2023 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
@ -114,14 +114,22 @@ private:
hrt_abstime _reset_timestamp{0};
hrt_abstime _last_publish{0};
hrt_abstime _last_motion{0};
int16_t _delta_x_raw_prev{0};
int16_t _delta_y_raw_prev{0};
uint32_t _shutter_prev{0};
uint8_t _quality_prev{0};
uint8_t _raw_data_sum_prev{0};
int _failure_count{0};
int _discard_reading{0};
px4::atomic<hrt_abstime> _drdy_timestamp_sample{0};
bool _data_ready_interrupt_enabled{false};
bool _motion_interrupt_enabled{false};
uint32_t _scheduled_interval_us{SAMPLE_INTERVAL_MODE_0 / 2};
static constexpr uint32_t kBackupScheduleIntervalUs{200_ms};
static constexpr uint32_t kBackupScheduleIntervalUs{SAMPLE_INTERVAL_MODE_2}; // longest expected interval
Mode _mode{Mode::LowLight};