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

AP_OpticalFlow: fixed threading in opticalflow for linux

Browse Source
This commit is contained in:
Andrew Tridgell 2016-11-04 15:09:19 +11:00
parent 5cf768ced2
commit 6e2fbf1988
3 changed files with 44 additions and 40 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

58
libraries/AP_OpticalFlow/AP_OpticalFlow_Linux.cpp
View File

@ -41,7 +41,8 @@ extern const AP_HAL::HAL& hal;
AP_OpticalFlow_Linux::AP_OpticalFlow_Linux(OpticalFlow &_frontend, AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev)
: OpticalFlow_backend(_frontend)
, _dev(std::move(dev))
{}
{
}
void AP_OpticalFlow_Linux::init(void)
{
@ -65,6 +66,8 @@ void AP_OpticalFlow_Linux::init(void)
// success
initialised = true;
_dev->register_periodic_callback(100000, FUNCTOR_BIND_MEMBER(&AP_OpticalFlow_Linux::timer, bool));
fail:
_dev->get_semaphore()->give();
}
@ -75,13 +78,8 @@ bool AP_OpticalFlow_Linux::request_measurement()
return _dev->write_register(PX4FLOW_REG, 0);
}
bool AP_OpticalFlow_Linux::read(optical_flow_s* report)
bool AP_OpticalFlow_Linux::timer(void)
{
// take i2c bus sempahore (non blocking)
if (!_dev->get_semaphore()->take_nonblocking()) {
return false;
}
// request measurement
request_measurement();
@ -103,53 +101,49 @@ bool AP_OpticalFlow_Linux::read(optical_flow_s* report)
memcpy(&f_integral, val, I2C_INTEGRAL_FRAME_SIZE);
}
_dev->get_semaphore()->give();
// reduce error count
if (num_errors > 0) {
num_errors--;
}
report->pixel_flow_x_integral = static_cast<float>(f_integral.pixel_flow_x_integral) / 10000.0f; //convert to radians
report->pixel_flow_y_integral = static_cast<float>(f_integral.pixel_flow_y_integral) / 10000.0f; //convert to radians
report->frame_count_since_last_readout = f_integral.frame_count_since_last_readout;
report->ground_distance_m = static_cast<float>(f_integral.ground_distance) / 1000.0f; // convert to meters
report->quality = f_integral.qual; // 0:bad, 255 max quality
report->gyro_x_rate_integral = static_cast<float>(f_integral.gyro_x_rate_integral) / 10000.0f; // convert to radians
report->gyro_y_rate_integral = static_cast<float>(f_integral.gyro_y_rate_integral) / 10000.0f; // convert to radians
report->gyro_z_rate_integral = static_cast<float>(f_integral.gyro_z_rate_integral) / 10000.0f; // convert to radians
report->integration_timespan = f_integral.integration_timespan; // microseconds
report->time_since_last_sonar_update = f_integral.sonar_timestamp; // microseconds
report->gyro_temperature = f_integral.gyro_temperature; // Temperature * 100 in centi-degrees Celsius
report->sensor_id = 0;
if (_sem->take(0)) {
report.pixel_flow_x_integral = static_cast<float>(f_integral.pixel_flow_x_integral) / 10000.0f; //convert to radians
report.pixel_flow_y_integral = static_cast<float>(f_integral.pixel_flow_y_integral) / 10000.0f; //convert to radians
report.frame_count_since_last_readout = f_integral.frame_count_since_last_readout;
report.ground_distance_m = static_cast<float>(f_integral.ground_distance) / 1000.0f; // convert to meters
report.quality = f_integral.qual; // 0:bad, 255 max quality
report.gyro_x_rate_integral = static_cast<float>(f_integral.gyro_x_rate_integral) / 10000.0f; // convert to radians
report.gyro_y_rate_integral = static_cast<float>(f_integral.gyro_y_rate_integral) / 10000.0f; // convert to radians
report.gyro_z_rate_integral = static_cast<float>(f_integral.gyro_z_rate_integral) / 10000.0f; // convert to radians
report.integration_timespan = f_integral.integration_timespan; // microseconds
report.time_since_last_sonar_update = f_integral.sonar_timestamp; // microseconds
report.gyro_temperature = f_integral.gyro_temperature; // Temperature * 100 in centi-degrees Celsius
report.sensor_id = 0;
new_report = true;
_sem->give();
}
return true;
fail_transfer:
num_errors++;
_dev->get_semaphore()->give();
return false;
return true;
}
// update - read latest values from sensor and fill in x,y and totals.
void AP_OpticalFlow_Linux::update(void)
{
optical_flow_s report;
// return immediately if not initialised or more than half of last 40 reads have failed
if (!initialised || num_errors >= 20) {
return;
}
// throttle reads to no more than 10hz
uint32_t now = AP_HAL::millis();
if (now - last_read_ms < 100) {
if (!_sem->take_nonblocking()) {
return;
}
last_read_ms = now;
// read the report from the sensor
if (!read(&report)) {
if (!new_report) {
_sem->give();
return;
}
@ -174,6 +168,8 @@ void AP_OpticalFlow_Linux::update(void)
// copy results to front end
_update_frontend(state);
_sem->give();
#if PX4FLOW_DEBUG
hal.console->printf("PX4FLOW id:%u qual:%u FlowRateX:%4.2f Y:%4.2f BodyRateX:%4.2f y:%4.2f\n",
(unsigned)state.device_id,

11
libraries/AP_OpticalFlow/AP_OpticalFlow_Linux.h
View File

@ -76,10 +76,11 @@ private:
// request the sensor produce a measurement, returns true on success
bool request_measurement();
// read from sensor, returns true if successful
bool read(optical_flow_s* report);
bool timer(void);
bool initialised = false;
uint16_t num_errors = 0;
uint32_t last_read_ms = 0;
bool initialised;
uint16_t num_errors;
optical_flow_s report;
bool new_report;
};

9
libraries/AP_OpticalFlow/OpticalFlow_backend.h
View File

@ -20,13 +20,17 @@
#include "OpticalFlow.h"
extern const AP_HAL::HAL& hal;
class OpticalFlow_backend
{
friend class OpticalFlow;
public:
// constructor
OpticalFlow_backend(OpticalFlow &_frontend) : frontend(_frontend) {}
OpticalFlow_backend(OpticalFlow &_frontend) : frontend(_frontend) {
_sem = hal.util->new_semaphore();
}
// init - initialise sensor
virtual void init() = 0;
@ -46,4 +50,7 @@ protected:
// get the yaw angle in radians
float _yawAngleRad(void) const { return radians(float(frontend._yawAngle_cd) * 0.01f); }
// semaphore for access to shared frontend data
AP_HAL::Semaphore *_sem;
};