Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-18 06:38:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_OpticalFlow : Update PX4Flow interface

Browse Source 
Interface now uses the integral of flow and body rates
This commit is contained in:
priseborough 2014-10-31 23:52:50 +11:00 committed by Andrew Tridgell
parent 0d774d301d
commit cb4d5986e0
3 changed files with 9 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
libraries/AP_OpticalFlow/AP_OpticalFlow_PX4.cpp
View File

@ -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();
}

1
libraries/AP_OpticalFlow/OpticalFlow.cpp
View File

@ -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);

14
libraries/AP_OpticalFlow/OpticalFlow.h
View File

@ -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
};