fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-30 00:14:08 +08:00
Code Issues Packages Projects Releases Wiki Activity

PMW3901 improvements (#12977)

Browse Source 
* PMW3901: use frame count and quality metric

* PMW3901: set qual to 0 for unsuccessful SPI reads

* PMW3901: improve comment for collect_time

* PMW3901: set qual to zero for huge flow values
This commit is contained in:
Nico van Duijn 2019-09-17 16:54:56 +02:00 committed by Daniel Agar
parent 72e93a9c36
commit 18eaeb564d
2 changed files with 22 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

31
src/drivers/optical_flow/pmw3901/PMW3901.cpp
View File

@ -314,8 +314,12 @@ PMW3901::Run()
readMotionCount(delta_x_raw, delta_y_raw, qual);
_flow_sum_x += delta_x_raw;
_flow_sum_y += delta_y_raw;
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) {
@ -336,19 +340,12 @@ PMW3901::Run()
rotate_3f(_yaw_rotation, report.pixel_flow_x_integral, report.pixel_flow_y_integral, zeroval);
rotate_3f(_yaw_rotation, report.gyro_x_rate_integral, report.gyro_y_rate_integral, report.gyro_z_rate_integral);
report.frame_count_since_last_readout = 4; // microseconds
report.frame_count_since_last_readout = _flow_sample_counter; // number of frames
report.integration_timespan = _flow_dt_sum_usec; // microseconds
report.sensor_id = 0;
report.quality = _flow_sample_counter > 0 ? _flow_quality_sum / _flow_sample_counter : 0;
// This sensor doesn't provide any quality metric. However if the sensor is unable to calculate the optical flow it will
// output 0 for the delta. Hence, we set the measurement to "invalid" (quality = 0) if the values are smaller than FLT_EPSILON
if (fabsf(report.pixel_flow_x_integral) < FLT_EPSILON && fabsf(report.pixel_flow_y_integral) < FLT_EPSILON) {
report.quality = 0;
} else {
report.quality = qual;
}
/* No gyro on this board */
report.gyro_x_rate_integral = NAN;
@ -363,6 +360,8 @@ PMW3901::Run()
_flow_dt_sum_usec = 0;
_flow_sum_x = 0;
_flow_sum_y = 0;
_flow_sample_counter = 0;
_flow_quality_sum = 0;
_optical_flow_pub.publish(report);
@ -379,6 +378,7 @@ PMW3901::readMotionCount(int16_t &deltaX, int16_t &deltaY, uint8_t &qual)
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;
@ -386,7 +386,14 @@ PMW3901::readMotionCount(int16_t &deltaX, int16_t &deltaY, uint8_t &qual)
deltaX = ((int16_t)data[5] << 8) | data[3];
deltaY = ((int16_t)data[9] << 8) | data[7];
qual = data[11];
// 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];
}
ret = OK;

4
src/drivers/optical_flow/pmw3901/PMW3901.hpp
View File

@ -103,7 +103,7 @@ protected:
private:
const uint64_t _collect_time{15000}; // usecs, optical flow data publish rate
const uint64_t _collect_time{15000}; // usecs, ensures flow data is published every second iteration of Run() (100Hz -> 50Hz)
uORB::PublicationMulti<optical_flow_s> _optical_flow_pub{ORB_ID(optical_flow)};
@ -117,6 +117,8 @@ private:
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};
/**
* Initialise the automatic measurement state machine and start it.