Browse Source

AP_OpticalFlow: Remove unused device ID tracking

master
Michael du Breuil 6 years ago committed by Randy Mackay
parent
commit
b1bc4268a9
  1. 1
      libraries/AP_OpticalFlow/AP_OpticalFlow_CXOF.cpp
  2. 1
      libraries/AP_OpticalFlow/AP_OpticalFlow_MAV.cpp
  3. 1
      libraries/AP_OpticalFlow/AP_OpticalFlow_Onboard.cpp
  4. 1
      libraries/AP_OpticalFlow/AP_OpticalFlow_PX4Flow.cpp
  5. 1
      libraries/AP_OpticalFlow/AP_OpticalFlow_Pixart.cpp
  6. 1
      libraries/AP_OpticalFlow/AP_OpticalFlow_SITL.cpp
  7. 4
      libraries/AP_OpticalFlow/OpticalFlow.h

1
libraries/AP_OpticalFlow/AP_OpticalFlow_CXOF.cpp

@ -158,7 +158,6 @@ void AP_OpticalFlow_CXOF::update(void)
} }
struct OpticalFlow::OpticalFlow_state state {}; struct OpticalFlow::OpticalFlow_state state {};
state.device_id = 0x43; // 'C'
// average surface quality scaled to be between 0 and 255 // average surface quality scaled to be between 0 and 255
state.surface_quality = (constrain_int16(qual_sum / count, 64, 78) - 64) * 255 / 14; state.surface_quality = (constrain_int16(qual_sum / count, 64, 78) - 64) * 255 / 14;

1
libraries/AP_OpticalFlow/AP_OpticalFlow_MAV.cpp

@ -46,7 +46,6 @@ void AP_OpticalFlow_MAV::update(void)
struct OpticalFlow::OpticalFlow_state state {}; struct OpticalFlow::OpticalFlow_state state {};
state.device_id = sensor_id;
state.surface_quality = quality_sum / count; state.surface_quality = quality_sum / count;
// calculate dt // calculate dt

1
libraries/AP_OpticalFlow/AP_OpticalFlow_Onboard.cpp

@ -53,7 +53,6 @@ void AP_OpticalFlow_Onboard::update()
} }
struct OpticalFlow::OpticalFlow_state state; struct OpticalFlow::OpticalFlow_state state;
state.device_id = OPTICALFLOW_ONBOARD_ID;
state.surface_quality = data_frame.quality; state.surface_quality = data_frame.quality;
if (data_frame.delta_time > 0) { if (data_frame.delta_time > 0) {
const Vector2f flowScaler = _flowScaler(); const Vector2f flowScaler = _flowScaler();

1
libraries/AP_OpticalFlow/AP_OpticalFlow_PX4Flow.cpp

@ -114,7 +114,6 @@ void AP_OpticalFlow_PX4Flow::timer(void)
return; return;
} }
struct OpticalFlow::OpticalFlow_state state {}; struct OpticalFlow::OpticalFlow_state state {};
state.device_id = get_address();
if (frame.integration_timespan > 0) { if (frame.integration_timespan > 0) {
const Vector2f flowScaler = _flowScaler(); const Vector2f flowScaler = _flowScaler();

1
libraries/AP_OpticalFlow/AP_OpticalFlow_Pixart.cpp

@ -330,7 +330,6 @@ void AP_OpticalFlow_Pixart::update(void)
last_update_ms = now; last_update_ms = now;
struct OpticalFlow::OpticalFlow_state state; struct OpticalFlow::OpticalFlow_state state;
state.device_id = 1;
state.surface_quality = burst.squal; state.surface_quality = burst.squal;
if (integral.sum_us > 0) { if (integral.sum_us > 0) {

1
libraries/AP_OpticalFlow/AP_OpticalFlow_SITL.cpp

@ -66,7 +66,6 @@ void AP_OpticalFlow_SITL::update(void)
radians(_sitl->state.yawDeg)); radians(_sitl->state.yawDeg));
state.device_id = 1;
state.surface_quality = 51; state.surface_quality = 51;
// sensor position offset in body frame // sensor position offset in body frame

4
libraries/AP_OpticalFlow/OpticalFlow.h

@ -76,14 +76,10 @@ public:
// velocity - returns the velocity in m/s // velocity - returns the velocity in m/s
const Vector2f& bodyRate() const { return _state.bodyRate; } const Vector2f& bodyRate() const { return _state.bodyRate; }
// device_id - returns device id
uint8_t device_id() const { return _state.device_id; }
// last_update() - returns system time of last sensor update // last_update() - returns system time of last sensor update
uint32_t last_update() const { return _last_update_ms; } uint32_t last_update() const { return _last_update_ms; }
struct OpticalFlow_state { struct OpticalFlow_state {
uint8_t device_id; // device id
uint8_t surface_quality; // image quality (below TBD you can't trust the dx,dy values returned) uint8_t surface_quality; // image quality (below TBD you can't trust the dx,dy values returned)
Vector2f flowRate; // optical flow angular rate in rad/sec measured about the X and Y body axis. A RH rotation about a sensor axis produces a positive rate. Vector2f flowRate; // optical flow angular rate in rad/sec measured about the X and Y body axis. A RH rotation about a sensor axis produces a positive rate.
Vector2f bodyRate; // body inertial angular rate in rad/sec measured about the X and Y body axis. A RH rotation about a sensor axis produces a positive rate. Vector2f bodyRate; // body inertial angular rate in rad/sec measured about the X and Y body axis. A RH rotation about a sensor axis produces a positive rate.

Loading…
Cancel
Save