OpticalFlow_Linux: reworked driver
remove PANICs from init return semaphore if init fails add successful initialisation check before attempting to read from sensor structure made private where possible formatting fixes check I2C reads succeed add request_measurement to request sensor to produce measurement quit after 20 of previous 40 reads fail throttle reads to 10hz max
This commit is contained in:
Randy Mackay
parent
ed3366aaae
commit
4aef64c153
@ -25,145 +25,146 @@
|
||||
#include <AP_HAL.h>
|
||||
#include "OpticalFlow.h"
|
||||
|
||||
#define DEBUG 1
|
||||
#define RAW_READ 0
|
||||
#define PX4FLOW_DEBUG 1
|
||||
|
||||
#if CONFIG_HAL_BOARD == HAL_BOARD_LINUX
|
||||
|
||||
#define PX4FLOW_I2C_ADDRESS 0x42 // 7-bit address. 8-bit address is 0x84, range 0x42 - 0x49
|
||||
#define PX4FLOW_REG 0x16 // Measure Register 22
|
||||
#define I2C_FRAME_SIZE (sizeof(i2c_frame))
|
||||
#define I2C_INTEGRAL_FRAME_SIZE (sizeof(i2c_integral_frame))
|
||||
|
||||
extern const AP_HAL::HAL& hal;
|
||||
|
||||
AP_OpticalFlow_Linux::AP_OpticalFlow_Linux(OpticalFlow &_frontend) :
|
||||
OpticalFlow_backend(_frontend)
|
||||
OpticalFlow_backend(_frontend)
|
||||
{}
|
||||
|
||||
|
||||
void AP_OpticalFlow_Linux::init(void)
|
||||
{
|
||||
uint8_t buff[22];
|
||||
// only initialise once
|
||||
if (initialised) {
|
||||
return;
|
||||
}
|
||||
|
||||
// get pointer to i2c bus semaphore
|
||||
AP_HAL::Semaphore *i2c_sem = hal.i2c->get_semaphore();
|
||||
// get pointer to i2c bus semaphore
|
||||
AP_HAL::Semaphore *i2c_sem = hal.i2c->get_semaphore();
|
||||
if (i2c_sem == NULL) {
|
||||
return;
|
||||
}
|
||||
|
||||
// take i2c bus sempahore
|
||||
if (!i2c_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
|
||||
hal.scheduler->panic(PSTR("PX4FLOW: unable to get semaphore"));
|
||||
}
|
||||
// take i2c bus sempahore
|
||||
if (!i2c_sem->take(200)) {
|
||||
return;
|
||||
}
|
||||
|
||||
// to be sure this is not a ll40ls Lidar (which can also be on
|
||||
// 0x42) we check if a I2C_FRAME_SIZE byte transfer works from address
|
||||
// 0. The ll40ls gives an error for that, whereas the flow
|
||||
// happily returns some data
|
||||
uint8_t val[I2C_FRAME_SIZE];
|
||||
if (hal.i2c->readRegisters(I2C_FLOW_ADDRESS, 0, I2C_FRAME_SIZE, val))
|
||||
hal.scheduler->panic(PSTR("ll40ls Lidar"));
|
||||
|
||||
i2c_sem->give();
|
||||
// read from flow sensor to ensure it is not a ll40ls Lidar (which can be on 0x42)
|
||||
// read I2C_FRAME_SIZE bytes, the ll40ls will error whereas the flow happily returns data
|
||||
uint8_t val[I2C_FRAME_SIZE];
|
||||
if (hal.i2c->readRegisters(PX4FLOW_I2C_ADDRESS, 0, I2C_FRAME_SIZE, val) != 0) {
|
||||
i2c_sem->give();
|
||||
return;
|
||||
}
|
||||
|
||||
// success
|
||||
initialised = true;
|
||||
i2c_sem->give();
|
||||
}
|
||||
|
||||
int AP_OpticalFlow_Linux::read(optical_flow_s* report)
|
||||
bool AP_OpticalFlow_Linux::request_measurement()
|
||||
{
|
||||
// get pointer to i2c bus semaphore
|
||||
AP_HAL::Semaphore *i2c_sem = hal.i2c->get_semaphore();
|
||||
// send measure request to sensor
|
||||
uint8_t cmd = PX4FLOW_REG;
|
||||
if (hal.i2c->writeRegisters(PX4FLOW_I2C_ADDRESS, cmd, 0, nullptr) != 0) {
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
// take i2c bus sempahore
|
||||
if (!i2c_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
|
||||
hal.scheduler->panic(PSTR("PX4FLOW: unable to get semaphore"));
|
||||
}
|
||||
bool AP_OpticalFlow_Linux::read(optical_flow_s* report)
|
||||
{
|
||||
// get pointer to i2c bus semaphore
|
||||
AP_HAL::Semaphore *i2c_sem = hal.i2c->get_semaphore();
|
||||
if (i2c_sem == NULL) {
|
||||
num_errors++;
|
||||
return false;
|
||||
}
|
||||
|
||||
// take i2c bus sempahore (non blocking)
|
||||
if (!i2c_sem->take_nonblocking()) {
|
||||
num_errors++;
|
||||
return false;
|
||||
}
|
||||
|
||||
uint8_t val[I2C_FRAME_SIZE + I2C_INTEGRAL_FRAME_SIZE] = { 0 };
|
||||
// request measurement
|
||||
request_measurement();
|
||||
|
||||
uint8_t val[I2C_FRAME_SIZE + I2C_INTEGRAL_FRAME_SIZE] = {};
|
||||
i2c_integral_frame f_integral;
|
||||
|
||||
// Perform the writing and reading in a single command
|
||||
if (PX4FLOW_REG == 0x00) {
|
||||
if (hal.i2c->readRegisters(PX4FLOW_I2C_ADDRESS, 0, I2C_FRAME_SIZE + I2C_INTEGRAL_FRAME_SIZE, val) != 0) {
|
||||
num_errors++;
|
||||
i2c_sem->give();
|
||||
return false;
|
||||
}
|
||||
memcpy(&f_integral, &(val[I2C_FRAME_SIZE]), I2C_INTEGRAL_FRAME_SIZE);
|
||||
}
|
||||
|
||||
if (PX4FLOW_REG == 0x16) {
|
||||
if (hal.i2c->readRegisters(PX4FLOW_I2C_ADDRESS, 0, I2C_INTEGRAL_FRAME_SIZE, val) != 0) {
|
||||
num_errors++;
|
||||
i2c_sem->give();
|
||||
return false;
|
||||
}
|
||||
memcpy(&f_integral, val, I2C_INTEGRAL_FRAME_SIZE);
|
||||
}
|
||||
|
||||
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 RAW_READ
|
||||
hal.console->printf_P(PSTR("PX4FLOW: RAW_READ"));
|
||||
// Send the command to begin a measurement
|
||||
uint8_t cmd = PX4FLOW_REG;
|
||||
if (hal.i2c->write(I2C_FLOW_ADDRESS, 1, &cmd)){
|
||||
hal.console->printf_P(PSTR("PX4FLOW: Error while beginning a measurement"));
|
||||
i2c_sem->give();
|
||||
return 0;
|
||||
}
|
||||
|
||||
// Perform the reading
|
||||
if (PX4FLOW_REG == 0x00) {
|
||||
if (hal.i2c->read(I2C_FLOW_ADDRESS, I2C_FRAME_SIZE + I2C_INTEGRAL_FRAME_SIZE, val)){
|
||||
hal.console->printf_P(PSTR("PX4FLOW: Error while reading"));
|
||||
i2c_sem->give();
|
||||
return 0;
|
||||
// reduce error count
|
||||
if (num_errors > 0) {
|
||||
num_errors--;
|
||||
}
|
||||
}
|
||||
|
||||
if (PX4FLOW_REG == 0x16) {
|
||||
if (hal.i2c->read(I2C_FLOW_ADDRESS, I2C_INTEGRAL_FRAME_SIZE, val)){
|
||||
hal.console->printf_P(PSTR("PX4FLOW: Error while reading"));
|
||||
i2c_sem->give();
|
||||
return 0;
|
||||
}
|
||||
}
|
||||
|
||||
#else
|
||||
// Perform the writing and reading in a single command
|
||||
if (PX4FLOW_REG == 0x00) {
|
||||
if (hal.i2c->readRegisters(I2C_FLOW_ADDRESS, PX4FLOW_REG, I2C_FRAME_SIZE + I2C_INTEGRAL_FRAME_SIZE, val)){
|
||||
hal.console->printf_P(PSTR("PX4FLOW: Error while reading"));
|
||||
i2c_sem->give();
|
||||
return 0;
|
||||
}
|
||||
}
|
||||
|
||||
if (PX4FLOW_REG == 0x16) {
|
||||
if (hal.i2c->readRegisters(I2C_FLOW_ADDRESS, PX4FLOW_REG, I2C_INTEGRAL_FRAME_SIZE, val)){
|
||||
hal.console->printf_P(PSTR("PX4FLOW: Error while reading"));
|
||||
i2c_sem->give();
|
||||
return 0;
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
|
||||
if (PX4FLOW_REG == 0) {
|
||||
memcpy(&f, val, I2C_FRAME_SIZE);
|
||||
memcpy(&f_integral, &(val[I2C_FRAME_SIZE]), I2C_INTEGRAL_FRAME_SIZE);
|
||||
}
|
||||
|
||||
if (PX4FLOW_REG == 0x16) {
|
||||
memcpy(&f_integral, val, I2C_INTEGRAL_FRAME_SIZE);
|
||||
}
|
||||
|
||||
// report->timestamp = hrt_absolute_time();
|
||||
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;
|
||||
|
||||
hal.console->printf_P(PSTR("PX4FLOW measurement: ground_distance_m: %f\n"), report->ground_distance_m);
|
||||
|
||||
/*
|
||||
// rotate measurements according to parameter
|
||||
float zeroval = 0.0f;
|
||||
rotate_3f(_sensor_rotation, report.pixel_flow_x_integral, report.pixel_flow_y_integral, zeroval);
|
||||
*/
|
||||
|
||||
i2c_sem->give();
|
||||
return 1;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
// update - read latest values from sensor and fill in x,y and totals.
|
||||
void AP_OpticalFlow_Linux::update(void)
|
||||
{
|
||||
struct optical_flow_s report;
|
||||
|
||||
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 = hal.scheduler->millis();
|
||||
if (now - last_read_ms < 100) {
|
||||
return;
|
||||
}
|
||||
last_read_ms = now;
|
||||
|
||||
// read the report from the sensor
|
||||
read(&report);
|
||||
if (!read(&report)) {
|
||||
return;
|
||||
}
|
||||
|
||||
// process
|
||||
struct OpticalFlow::OpticalFlow_state state;
|
||||
@ -183,17 +184,18 @@ void AP_OpticalFlow_Linux::update(void)
|
||||
state.bodyRate.zero();
|
||||
}
|
||||
|
||||
#if DEBUG
|
||||
hal.console->printf_P(PSTR("PX4FLOW print: sensor_id: %d\n"), state.device_id);
|
||||
hal.console->printf_P(PSTR("PX4FLOW print: surface_quality: %d\n"), state.surface_quality);
|
||||
hal.console->printf_P(PSTR("PX4FLOW print: flowRate.x: %d\n"), state.flowRate.x);
|
||||
hal.console->printf_P(PSTR("PX4FLOW print: flowRate.y: %d\n"), state.flowRate.y);
|
||||
hal.console->printf_P(PSTR("PX4FLOW print: bodyRate.x: %d\n"), state.bodyRate.x);
|
||||
hal.console->printf_P(PSTR("PX4FLOW print: bodyRate.y: %d\n"), state.bodyRate.y);
|
||||
#endif
|
||||
// copy results to front end
|
||||
_update_frontend(state);
|
||||
|
||||
#if PX4FLOW_DEBUG
|
||||
hal.console->printf_P(PSTR("PX4FLOW id:%u qual:%u FlowRateX:%4.2f Y:%4.2f BodyRateX:%4.2f y:%4.2f\n"),
|
||||
(unsigned)state.device_id,
|
||||
(unsigned)state.surface_quality,
|
||||
(double)state.flowRate.x,
|
||||
(double)state.flowRate.y,
|
||||
(double)state.bodyRate.x,
|
||||
(double)state.bodyRate.y);
|
||||
#endif
|
||||
}
|
||||
|
||||
|
||||
|
||||
#endif // CONFIG_HAL_BOARD == HAL_BOARD_LINUX
|
||||
|
||||
@ -1,96 +1,84 @@
|
||||
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
||||
|
||||
/*
|
||||
* Portions of this driver were borrowed from the PX4Firmware px4flow driver which can be found here:
|
||||
* https://github.com/PX4/Firmware/blob/master/src/drivers/px4flow/px4flow.cpp
|
||||
*/
|
||||
|
||||
#ifndef AP_OpticalFlow_Linux_H
|
||||
#define AP_OpticalFlow_Linux_H
|
||||
|
||||
#include "OpticalFlow.h"
|
||||
|
||||
#include <AP_Math.h>
|
||||
|
||||
/* Configuration Constants */
|
||||
#define I2C_FLOW_ADDRESS 0x42 ///< 7-bit address. 8-bit address is 0x84, range 0x42 - 0x49
|
||||
|
||||
/* PX4FLOW Registers addresses */
|
||||
#define PX4FLOW_REG 0x16 ///< Measure Register 22
|
||||
|
||||
#define PX4FLOW_CONVERSION_INTERVAL 100000 ///< in microseconds! 20000 = 50 Hz 100000 = 10Hz
|
||||
#define PX4FLOW_I2C_MAX_BUS_SPEED 400000 ///< 400 KHz maximum speed
|
||||
|
||||
typedef struct i2c_frame
|
||||
{
|
||||
uint16_t frame_count;
|
||||
int16_t pixel_flow_x_sum;
|
||||
int16_t pixel_flow_y_sum;
|
||||
int16_t flow_comp_m_x;
|
||||
int16_t flow_comp_m_y;
|
||||
int16_t qual;
|
||||
int16_t gyro_x_rate;
|
||||
int16_t gyro_y_rate;
|
||||
int16_t gyro_z_rate;
|
||||
uint8_t gyro_range;
|
||||
uint8_t sonar_timestamp;
|
||||
int16_t ground_distance;
|
||||
} i2c_frame;
|
||||
|
||||
#define I2C_FRAME_SIZE (sizeof(i2c_frame))
|
||||
|
||||
|
||||
typedef struct i2c_integral_frame
|
||||
{
|
||||
uint16_t frame_count_since_last_readout;
|
||||
int16_t pixel_flow_x_integral;
|
||||
int16_t pixel_flow_y_integral;
|
||||
int16_t gyro_x_rate_integral;
|
||||
int16_t gyro_y_rate_integral;
|
||||
int16_t gyro_z_rate_integral;
|
||||
uint32_t integration_timespan;
|
||||
uint32_t sonar_timestamp;
|
||||
uint16_t ground_distance;
|
||||
int16_t gyro_temperature;
|
||||
uint8_t qual;
|
||||
} i2c_integral_frame;
|
||||
|
||||
#define I2C_INTEGRAL_FRAME_SIZE (sizeof(i2c_integral_frame))
|
||||
|
||||
/**
|
||||
* Optical flow in NED body frame in SI units.
|
||||
*
|
||||
* @see http://en.wikipedia.org/wiki/International_System_of_Units
|
||||
*/
|
||||
struct optical_flow_s {
|
||||
|
||||
uint64_t timestamp; /**< in microseconds since system start */
|
||||
uint8_t sensor_id; /**< id of the sensor emitting the flow value */
|
||||
float pixel_flow_x_integral; /**< accumulated optical flow in radians around x axis */
|
||||
float pixel_flow_y_integral; /**< accumulated optical flow in radians around y axis */
|
||||
float gyro_x_rate_integral; /**< accumulated gyro value in radians around x axis */
|
||||
float gyro_y_rate_integral; /**< accumulated gyro value in radians around y axis */
|
||||
float gyro_z_rate_integral; /**< accumulated gyro value in radians around z axis */
|
||||
float ground_distance_m; /**< Altitude / distance to ground in meters */
|
||||
uint32_t integration_timespan; /**<accumulation timespan in microseconds */
|
||||
uint32_t time_since_last_sonar_update;/**< time since last sonar update in microseconds */
|
||||
uint16_t frame_count_since_last_readout;/**< number of accumulated frames in timespan */
|
||||
int16_t gyro_temperature;/**< Temperature * 100 in centi-degrees Celsius */
|
||||
uint8_t quality; /**< Average of quality of accumulated frames, 0: bad quality, 255: maximum quality */
|
||||
};
|
||||
|
||||
class AP_OpticalFlow_Linux : public OpticalFlow_backend
|
||||
{
|
||||
public:
|
||||
/// constructor
|
||||
// constructor
|
||||
AP_OpticalFlow_Linux(OpticalFlow &_frontend);
|
||||
|
||||
// init - initialise the sensor
|
||||
// initialise the sensor
|
||||
void init();
|
||||
|
||||
// update - read latest values from sensor and fill in x,y and totals.
|
||||
void update(void);
|
||||
// read latest values from sensor and fill in x,y and totals.
|
||||
void update(void);
|
||||
|
||||
private:
|
||||
int read(optical_flow_s* report);
|
||||
void print(optical_flow_s* report);
|
||||
struct i2c_frame f;
|
||||
struct i2c_integral_frame f_integral;
|
||||
|
||||
typedef struct {
|
||||
uint16_t frame_count;
|
||||
int16_t pixel_flow_x_sum;
|
||||
int16_t pixel_flow_y_sum;
|
||||
int16_t flow_comp_m_x;
|
||||
int16_t flow_comp_m_y;
|
||||
int16_t qual;
|
||||
int16_t gyro_x_rate;
|
||||
int16_t gyro_y_rate;
|
||||
int16_t gyro_z_rate;
|
||||
uint8_t gyro_range;
|
||||
uint8_t sonar_timestamp;
|
||||
int16_t ground_distance;
|
||||
} i2c_frame;
|
||||
|
||||
typedef struct {
|
||||
uint16_t frame_count_since_last_readout;
|
||||
int16_t pixel_flow_x_integral;
|
||||
int16_t pixel_flow_y_integral;
|
||||
int16_t gyro_x_rate_integral;
|
||||
int16_t gyro_y_rate_integral;
|
||||
int16_t gyro_z_rate_integral;
|
||||
uint32_t integration_timespan;
|
||||
uint32_t sonar_timestamp;
|
||||
uint16_t ground_distance;
|
||||
int16_t gyro_temperature;
|
||||
uint8_t qual;
|
||||
} i2c_integral_frame;
|
||||
|
||||
typedef struct {
|
||||
uint64_t timestamp; // in microseconds since system start
|
||||
uint8_t sensor_id; // id of the sensor emitting the flow value
|
||||
float pixel_flow_x_integral; // accumulated optical flow in radians around x axis
|
||||
float pixel_flow_y_integral; // accumulated optical flow in radians around y axis
|
||||
float gyro_x_rate_integral; // accumulated gyro value in radians around x axis
|
||||
float gyro_y_rate_integral; // accumulated gyro value in radians around y axis
|
||||
float gyro_z_rate_integral; // accumulated gyro value in radians around z axis
|
||||
float ground_distance_m; // Altitude / distance to ground in meters
|
||||
uint32_t integration_timespan; // accumulation time span in microseconds
|
||||
uint32_t time_since_last_sonar_update; // time since last sonar update in microseconds
|
||||
uint16_t frame_count_since_last_readout;//number of accumulated frames in time span
|
||||
int16_t gyro_temperature; // Temperature * 100 in centi-degrees celsius
|
||||
uint8_t quality; // Average of quality of accumulated frames, 0: bad quality, 255: maximum quality
|
||||
} optical_flow_s;
|
||||
|
||||
// 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 initialised = false;
|
||||
uint16_t num_errors = 0;
|
||||
uint32_t last_read_ms = 0;
|
||||
};
|
||||
|
||||
#endif
|
||||
|
||||
Loading…
Reference in New Issue
Block a user