mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-18 06:38:29 -04:00
AP_OpticalFlow: fixed threading in opticalflow for linux
This commit is contained in:
parent
5cf768ced2
commit
6e2fbf1988
@ -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,
|
||||||
|
@ -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;
|
|
||||||
};
|
};
|
||||||
|
@ -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;
|
||||||
};
|
};
|
||||||
|
Loading…
Reference in New Issue
Block a user