AP_OpticalFlow: fixed threading in opticalflow for linux

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

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

View File

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

View File

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