|
|
|
@ -91,7 +91,7 @@ PAW3902::probe()
@@ -91,7 +91,7 @@ PAW3902::probe()
|
|
|
|
|
{ |
|
|
|
|
uint8_t product_id = registerRead(Register::Product_ID); |
|
|
|
|
|
|
|
|
|
PX4_INFO("DEVICE_ID: %X", product_id); |
|
|
|
|
PX4_DEBUG("DEVICE_ID: %X", product_id); |
|
|
|
|
|
|
|
|
|
// Test the SPI communication, checking chipId and inverse chipId
|
|
|
|
|
if (product_id != PRODUCT_ID) { |
|
|
|
@ -99,7 +99,7 @@ PAW3902::probe()
@@ -99,7 +99,7 @@ PAW3902::probe()
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
uint8_t revision_id = registerRead(Register::Revision_ID); |
|
|
|
|
PX4_INFO("REVISION_ID: %X", revision_id); |
|
|
|
|
PX4_DEBUG("REVISION_ID: %X", revision_id); |
|
|
|
|
|
|
|
|
|
if (revision_id != REVISION_ID) { |
|
|
|
|
return PX4_ERROR; |
|
|
|
@ -131,7 +131,7 @@ bool
@@ -131,7 +131,7 @@ bool
|
|
|
|
|
PAW3902::changeMode(Mode newMode) |
|
|
|
|
{ |
|
|
|
|
if (newMode != _mode) { |
|
|
|
|
PX4_INFO("changing from mode %d -> %d", static_cast<int>(_mode), static_cast<int>(newMode)); |
|
|
|
|
PX4_DEBUG("changing from mode %d -> %d", static_cast<int>(_mode), static_cast<int>(newMode)); |
|
|
|
|
ScheduleClear(); |
|
|
|
|
reset(); |
|
|
|
|
|
|
|
|
@ -162,8 +162,8 @@ PAW3902::changeMode(Mode newMode)
@@ -162,8 +162,8 @@ PAW3902::changeMode(Mode newMode)
|
|
|
|
|
// Approximate Resolution = (Register Value + 1) * (50 / 8450) ≈ 0.6% of data point in Figure 19
|
|
|
|
|
// The maximum register value is 0xA8. The minimum register value is 0.
|
|
|
|
|
uint8_t resolution = registerRead(Register::Resolution); |
|
|
|
|
PX4_INFO("Resolution: %X", resolution); |
|
|
|
|
PX4_INFO("Resolution is approx: %.3f", (double)((resolution + 1.0f) * (50.0f / 8450.0f))); |
|
|
|
|
PX4_DEBUG("Resolution: %X", resolution); |
|
|
|
|
PX4_DEBUG("Resolution is approx: %.3f", (double)((resolution + 1.0f) * (50.0f / 8450.0f))); |
|
|
|
|
|
|
|
|
|
return true; |
|
|
|
|
} |
|
|
|
@ -562,19 +562,6 @@ PAW3902::RunImpl()
@@ -562,19 +562,6 @@ PAW3902::RunImpl()
|
|
|
|
|
// update for next iteration
|
|
|
|
|
_previous_collect_timestamp = timestamp_sample; |
|
|
|
|
|
|
|
|
|
// PX4_INFO("data.Motion %d", buf.data.Motion);
|
|
|
|
|
// PX4_INFO("data.Observation %d", buf.data.Observation);
|
|
|
|
|
// PX4_INFO("data.Delta_X_L %d", buf.data.Delta_X_L);
|
|
|
|
|
// PX4_INFO("data.Delta_X_H %d", buf.data.Delta_X_H);
|
|
|
|
|
// PX4_INFO("data.Delta_Y_L %d", buf.data.Delta_Y_L);
|
|
|
|
|
// PX4_INFO("data.Delta_Y_H %d", buf.data.Delta_Y_H);
|
|
|
|
|
// PX4_INFO("data.SQUAL %d", buf.data.SQUAL);
|
|
|
|
|
// PX4_INFO("data.RawData_Sum %d", buf.data.RawData_Sum);
|
|
|
|
|
// PX4_INFO("data.Maximum_RawData %d", buf.data.Maximum_RawData);
|
|
|
|
|
// PX4_INFO("data.Minimum_RawData %d", buf.data.Minimum_RawData);
|
|
|
|
|
// PX4_INFO("data.Shutter_Upper %d", buf.data.Shutter_Upper);
|
|
|
|
|
// PX4_INFO("data.Shutter_Lower %d", buf.data.Shutter_Lower);
|
|
|
|
|
|
|
|
|
|
const int16_t delta_x_raw = ((int16_t)buf.data.Delta_X_H << 8) | buf.data.Delta_X_L; |
|
|
|
|
const int16_t delta_y_raw = ((int16_t)buf.data.Delta_Y_H << 8) | buf.data.Delta_Y_L; |
|
|
|
|
|
|
|
|
@ -662,9 +649,6 @@ PAW3902::RunImpl()
@@ -662,9 +649,6 @@ PAW3902::RunImpl()
|
|
|
|
|
optical_flow_s report{}; |
|
|
|
|
report.timestamp = timestamp_sample; |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
//PX4_INFO("X: %d Y: %d", _flow_sum_x, _flow_sum_y);
|
|
|
|
|
|
|
|
|
|
report.pixel_flow_x_integral = (float)_flow_sum_x / 500.0f; // proportional factor + convert from pixels to radians
|
|
|
|
|
report.pixel_flow_y_integral = (float)_flow_sum_y / 500.0f; // proportional factor + convert from pixels to radians
|
|
|
|
|
|
|
|
|
|