diff --git a/libraries/AP_OpticalFlow/AP_OpticalFlow_PX4.cpp b/libraries/AP_OpticalFlow/AP_OpticalFlow_PX4.cpp index e138d5001f..4925605c36 100644 --- a/libraries/AP_OpticalFlow/AP_OpticalFlow_PX4.cpp +++ b/libraries/AP_OpticalFlow/AP_OpticalFlow_PX4.cpp @@ -63,10 +63,10 @@ void AP_OpticalFlow_PX4::update(void) while (::read(_fd, &report, sizeof(optical_flow_s)) == sizeof(optical_flow_s) && report.timestamp != _last_timestamp) { _device_id = report.sensor_id; _surface_quality = report.quality; - _raw.x = report.flow_raw_x; - _raw.y = report.flow_raw_y; - _velocity.x = report.flow_comp_x_m; - _velocity.y = report.flow_comp_y_m; + _flowRate.x = report.pixel_flow_x_integral / (report.integration_timespan / 1e6f); // rad/sec measured optically about the X sensor axis + _flowRate.y = report.pixel_flow_y_integral / (report.integration_timespan / 1e6f); // rad/sec measured optically about the Y sensor axis + _bodyRate.x = report.gyro_x_rate_integral / (report.integration_timespan / 1e6f); // rad/sec measured inertially about the X sensor axis + _bodyRate.y = report.gyro_y_rate_integral / (report.integration_timespan / 1e6f); // rad/sec measured inertially about the Y sensor axis _last_timestamp = report.timestamp; _last_update = hal.scheduler->millis(); } diff --git a/libraries/AP_OpticalFlow/OpticalFlow.cpp b/libraries/AP_OpticalFlow/OpticalFlow.cpp index baa08664a2..6cbf92cc5f 100644 --- a/libraries/AP_OpticalFlow/OpticalFlow.cpp +++ b/libraries/AP_OpticalFlow/OpticalFlow.cpp @@ -18,7 +18,6 @@ OpticalFlow::OpticalFlow(const AP_AHRS &ahrs) : _ahrs(ahrs), _device_id(0), _surface_quality(0), - _ground_distance_m(0.0f), _last_update(0) { AP_Param::setup_object_defaults(this, var_info); diff --git a/libraries/AP_OpticalFlow/OpticalFlow.h b/libraries/AP_OpticalFlow/OpticalFlow.h index 207976d8ed..0b81ce6a28 100644 --- a/libraries/AP_OpticalFlow/OpticalFlow.h +++ b/libraries/AP_OpticalFlow/OpticalFlow.h @@ -54,17 +54,14 @@ public: uint8_t quality() const { return _surface_quality; } // raw - returns the raw movement from the sensor - const Vector2i& raw() const { return _raw; } + const Vector2f& flowRate() const { return _flowRate; } // velocity - returns the velocity in m/s - const Vector2f& velocity() const { return _velocity; } + const Vector2f& bodyRate() const { return _bodyRate; } // device_id - returns device id uint8_t device_id() const { return _device_id; } - // return ground distance in meters (if available) - float ground_distance_m() const { return _ground_distance_m; } - // last_update() - returns system time of last sensor update uint32_t last_update() const { return _last_update; } @@ -85,10 +82,9 @@ protected: // internal variables uint8_t _device_id; // device id - uint8_t _surface_quality; // image quality (below 15 you can't trust the dx,dy values returned) - Vector2i _raw; // raw x,y values from sensor - Vector2f _velocity; // x, y velocity in m/s - float _ground_distance_m; // ground distance in m + 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 _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. uint32_t _last_update; // millis() time of last update };