aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_OpticalFlow: added common driver for PX4Flow

Browse Source 
used on Linux and NuttX boards
This commit is contained in:
Andrew Tridgell 2016-11-26 14:21:28 +11:00
parent 52715c5d16
commit 06dfbc3e09
10 changed files with 193 additions and 402 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

184
libraries/AP_OpticalFlow/AP_OpticalFlow_Linux.cpp
View File

@ -1,184 +0,0 @@
/*
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
/*
* AP_OpticalFlow_Linux.cpp - ardupilot library for the PX4Flow sensor.
* inspired by the PX4Firmware code.
*
* @author: Víctor Mayoral Vilches
*
* Address range 0x42 - 0x49
*/
#include <utility>
#include <AP_HAL/AP_HAL.h>
#include "OpticalFlow.h"
#define PX4FLOW_DEBUG 1
#if CONFIG_HAL_BOARD == HAL_BOARD_LINUX
#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, AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev)
: OpticalFlow_backend(_frontend)
, _dev(std::move(dev))
{
}
void AP_OpticalFlow_Linux::init(void)
{
// only initialise once
if (initialised) {
return;
}
// take i2c bus sempahore
if (!_dev->get_semaphore()->take(200)) {
return;
}
// 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 (!_dev->read_registers(0, val, I2C_FRAME_SIZE)) {
goto fail;
}
// success
initialised = true;
_dev->register_periodic_callback(100000, FUNCTOR_BIND_MEMBER(&AP_OpticalFlow_Linux::timer, bool));
fail:
_dev->get_semaphore()->give();
}
bool AP_OpticalFlow_Linux::request_measurement()
{
// send measure request to sensor
return _dev->write_register(PX4FLOW_REG, 0);
}
bool AP_OpticalFlow_Linux::timer(void)
{
// 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 (!_dev->read_registers(PX4FLOW_REG, val, I2C_FRAME_SIZE + I2C_INTEGRAL_FRAME_SIZE)) {
goto fail_transfer;
}
memcpy(&f_integral, &(val[I2C_FRAME_SIZE]), I2C_INTEGRAL_FRAME_SIZE);
}
if (PX4FLOW_REG == 0x16) {
if (!_dev->read_registers(PX4FLOW_REG, val, I2C_INTEGRAL_FRAME_SIZE)) {
goto fail_transfer;
}
memcpy(&f_integral, val, I2C_INTEGRAL_FRAME_SIZE);
}
// reduce error count
if (num_errors > 0) {
num_errors--;
}
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++;
return true;
}
// update - read latest values from sensor and fill in x,y and totals.
void AP_OpticalFlow_Linux::update(void)
{
// return immediately if not initialised or more than half of last 40 reads have failed
if (!initialised || num_errors >= 20) {
return;
}
if (!_sem->take_nonblocking()) {
return;
}
if (!new_report) {
_sem->give();
return;
}
// process
struct OpticalFlow::OpticalFlow_state state;
state.device_id = report.sensor_id;
state.surface_quality = report.quality;
if (report.integration_timespan > 0) {
const Vector2f flowScaler = _flowScaler();
float flowScaleFactorX = 1.0f + 0.001f * flowScaler.x;
float flowScaleFactorY = 1.0f + 0.001f * flowScaler.y;
float integralToRate = 1e6f / float(report.integration_timespan);
state.flowRate.x = flowScaleFactorX * integralToRate * float(report.pixel_flow_x_integral); // rad/sec measured optically about the X sensor axis
state.flowRate.y = flowScaleFactorY * integralToRate * float(report.pixel_flow_y_integral); // rad/sec measured optically about the Y sensor axis
state.bodyRate.x = integralToRate * float(report.gyro_x_rate_integral); // rad/sec measured inertially about the X sensor axis
state.bodyRate.y = integralToRate * float(report.gyro_y_rate_integral); // rad/sec measured inertially about the Y sensor axis
} else {
state.flowRate.zero();
state.bodyRate.zero();
}
// 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,
(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

86
libraries/AP_OpticalFlow/AP_OpticalFlow_Linux.h
View File

@ -1,86 +0,0 @@
/*
* 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
*/
#pragma once
#include "OpticalFlow.h"
#include <AP_Common/AP_Common.h>
#include <AP_HAL/I2CDevice.h>
#include <AP_HAL/utility/OwnPtr.h>
#include <AP_Math/AP_Math.h>
class AP_OpticalFlow_Linux : public OpticalFlow_backend
{
public:
// constructor
AP_OpticalFlow_Linux(OpticalFlow &_frontend, AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev);
// initialise the sensor
void init();
// read latest values from sensor and fill in x,y and totals.
void update(void);
private:
typedef struct PACKED {
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 PACKED {
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;
uint8_t padding_not_used;
} 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;
AP_HAL::OwnPtr<AP_HAL::I2CDevice> _dev;
// request the sensor produce a measurement, returns true on success
bool request_measurement();
bool timer(void);
bool initialised;
uint16_t num_errors;
optical_flow_s report;
bool new_report;
};

106
libraries/AP_OpticalFlow/AP_OpticalFlow_PX4.cpp
View File

@ -1,106 +0,0 @@
/*
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
/*
* AP_OpticalFlow_PX4.cpp - ardupilot library for PX4Flow sensor
*
*/
#include <AP_HAL/AP_HAL.h>
#include "OpticalFlow.h"
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
#include <AP_BoardConfig/AP_BoardConfig.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <unistd.h>
#include <drivers/drv_px4flow.h>
#include <uORB/topics/optical_flow.h>
extern const AP_HAL::HAL& hal;
AP_OpticalFlow_PX4::AP_OpticalFlow_PX4(OpticalFlow &_frontend) :
OpticalFlow_backend(_frontend)
{}
extern "C" int px4flow_main(int, char **);
void AP_OpticalFlow_PX4::init(void)
{
#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1) || defined(CONFIG_ARCH_BOARD_PX4FMU_V2) || defined(CONFIG_ARCH_BOARD_VRBRAIN_V51) || defined(CONFIG_ARCH_BOARD_VRBRAIN_V52) || defined(CONFIG_ARCH_BOARD_VRBRAIN_V54) || defined(CONFIG_ARCH_BOARD_VRUBRAIN_V51) || defined(CONFIG_ARCH_BOARD_VRUBRAIN_V52) || defined(CONFIG_ARCH_BOARD_VRCORE_V10)
if (!AP_BoardConfig::px4_start_driver(px4flow_main, "px4flow", "start")) {
hal.console->printf("Unable to start px4flow driver\n");
} else {
// give it time to initialise
hal.scheduler->delay(500);
}
#endif
_fd = open(PX4FLOW0_DEVICE_PATH, O_RDONLY);
// check for failure to open device
if (_fd == -1) {
return;
}
// change to 10Hz update
if (ioctl(_fd, SENSORIOCSPOLLRATE, 10) != 0) {
hal.console->printf("Unable to set flow rate to 10Hz\n");
}
}
// update - read latest values from sensor and fill in x,y and totals.
void AP_OpticalFlow_PX4::update(void)
{
// return immediately if not initialised
if (_fd == -1) {
return;
}
struct optical_flow_s report;
while (::read(_fd, &report, sizeof(optical_flow_s)) == sizeof(optical_flow_s) &&
report.timestamp != _last_timestamp) {
struct OpticalFlow::OpticalFlow_state state;
state.device_id = report.sensor_id;
state.surface_quality = report.quality;
if (report.integration_timespan > 0) {
const Vector2f flowScaler = _flowScaler();
float flowScaleFactorX = 1.0f + 0.001f * flowScaler.x;
float flowScaleFactorY = 1.0f + 0.001f * flowScaler.y;
float integralToRate = 1e6f / float(report.integration_timespan);
// rad/sec measured optically about the X body axis
state.flowRate.x = flowScaleFactorX * integralToRate * report.pixel_flow_x_integral;
state.flowRate.y = flowScaleFactorY * integralToRate * report.pixel_flow_y_integral;
_applyYaw(state.flowRate);
// rad/sec measured inertially about the X body axis
state.bodyRate.x = integralToRate * report.gyro_x_rate_integral;
state.bodyRate.y = integralToRate * report.gyro_y_rate_integral;
_applyYaw(state.bodyRate);
} else {
state.flowRate.zero();
state.bodyRate.zero();
}
_last_timestamp = report.timestamp;
_update_frontend(state);
}
}
#endif // CONFIG_HAL_BOARD == HAL_BOARD_PX4

20
libraries/AP_OpticalFlow/AP_OpticalFlow_PX4.h
View File

@ -1,20 +0,0 @@
#pragma once
#include "OpticalFlow.h"
class AP_OpticalFlow_PX4 : public OpticalFlow_backend
{
public:
/// constructor
AP_OpticalFlow_PX4(OpticalFlow &_frontend);
// init - initialise the sensor
void init();
// update - read latest values from sensor and fill in x,y and totals.
void update(void);
private:
int _fd; // file descriptor for sensor
uint64_t _last_timestamp; // time of last update (used to avoid processing old reports)
};

128
libraries/AP_OpticalFlow/AP_OpticalFlow_PX4Flow.cpp Normal file
View File

@ -0,0 +1,128 @@
/*
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
/*
driver for PX4Flow optical flow sensor
*/
#include <AP_HAL/AP_HAL.h>
#include "AP_OpticalFlow_PX4Flow.h"
#include <AP_Math/edc.h>
#include <AP_AHRS/AP_AHRS.h>
#include <utility>
#include "OpticalFlow.h"
#include <stdio.h>
extern const AP_HAL::HAL& hal;
#define PX4FLOW_BASE_I2C_ADDR 0x42
// constructor
AP_OpticalFlow_PX4Flow::AP_OpticalFlow_PX4Flow(OpticalFlow &_frontend) :
OpticalFlow_backend(_frontend)
{
}
// detect the device
AP_OpticalFlow_PX4Flow *AP_OpticalFlow_PX4Flow::detect(OpticalFlow &_frontend)
{
AP_OpticalFlow_PX4Flow *sensor = new AP_OpticalFlow_PX4Flow(_frontend);
if (!sensor) {
return nullptr;
}
if (!sensor->setup_sensor()) {
delete sensor;
return nullptr;
}
return sensor;
}
/*
look for the sensor on different buses
*/
bool AP_OpticalFlow_PX4Flow::scan_buses(void)
{
for (uint8_t bus=0; bus<2; bus++) {
#ifdef HAL_OPTFLOW_PX4FLOW_I2C_BUS
// only one bus from HAL
if (bus != HAL_OPTFLOW_PX4FLOW_I2C_BUS) {
continue;
}
#endif
AP_HAL::OwnPtr<AP_HAL::Device> tdev = hal.i2c_mgr->get_device(bus, PX4FLOW_BASE_I2C_ADDR + get_bus_id());
if (!tdev) {
continue;
}
if (!tdev->get_semaphore()->take(0)) {
continue;
}
struct i2c_integral_frame frame;
bool ok = tdev->read_registers(REG_INTEGRAL_FRAME, (uint8_t *)&frame, sizeof(frame));
tdev->get_semaphore()->give();
if (ok) {
printf("Found PX4Flow on bus %u\n", bus);
dev = std::move(tdev);
break;
}
}
return !!dev;
}
// setup the device
bool AP_OpticalFlow_PX4Flow::setup_sensor(void)
{
if (!scan_buses()) {
return false;
}
// read at 10Hz
dev->register_periodic_callback(100000, FUNCTOR_BIND_MEMBER(&AP_OpticalFlow_PX4Flow::timer, bool));
return true;
}
// update - read latest values from sensor and fill in x,y and totals.
void AP_OpticalFlow_PX4Flow::update(void)
{
}
// timer to read sensor
bool AP_OpticalFlow_PX4Flow::timer(void)
{
struct i2c_integral_frame frame;
if (!dev->read_registers(REG_INTEGRAL_FRAME, (uint8_t *)&frame, sizeof(frame))) {
return true;
}
struct OpticalFlow::OpticalFlow_state state {};
state.device_id = get_bus_id();
if (frame.integration_timespan > 0) {
const Vector2f flowScaler = _flowScaler();
float flowScaleFactorX = 1.0f + 0.001f * flowScaler.x;
float flowScaleFactorY = 1.0f + 0.001f * flowScaler.y;
float integralToRate = 1.0e6 / frame.integration_timespan;
state.surface_quality = frame.qual;
state.flowRate = Vector2f(frame.pixel_flow_x_integral * flowScaleFactorX,
frame.pixel_flow_y_integral * flowScaleFactorY) * 1.0e-4 * integralToRate;
state.bodyRate = Vector2f(frame.gyro_x_rate_integral, frame.gyro_y_rate_integral) * 1.0e-4 * integralToRate;
_applyYaw(state.flowRate);
_applyYaw(state.bodyRate);
}
_update_frontend(state);
return true;
}

48
libraries/AP_OpticalFlow/AP_OpticalFlow_PX4Flow.h Normal file
View File

@ -0,0 +1,48 @@
#pragma once
#include "OpticalFlow.h"
#include <AP_HAL/utility/OwnPtr.h>
class AP_OpticalFlow_PX4Flow : public OpticalFlow_backend
{
public:
/// constructor
AP_OpticalFlow_PX4Flow(OpticalFlow &_frontend);
// init - initialise the sensor
void init() override {}
// update - read latest values from sensor and fill in x,y and totals.
void update(void) override;
// detect if the sensor is available
static AP_OpticalFlow_PX4Flow *detect(OpticalFlow &_frontend);
private:
AP_HAL::OwnPtr<AP_HAL::Device> dev;
static const uint8_t REG_INTEGRAL_FRAME = 0x16;
// I2C data on register REG_INTEGRAL_FRAME
struct PACKED 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;
};
// scan I2C bus addresses and buses
bool scan_buses(void);
// setup sensor
bool setup_sensor(void);
bool timer(void);
};

1
libraries/AP_OpticalFlow/AP_OpticalFlow_Pixart.cpp
View File

@ -25,6 +25,7 @@
#include <AP_AHRS/AP_AHRS.h>
#include <utility>
#include "OpticalFlow.h"
#include "AP_OpticalFlow_Pixart.h"
#include "AP_OpticalFlow_Pixart_SROM.h"
#include <stdio.h>

14
libraries/AP_OpticalFlow/OpticalFlow.cpp
View File

@ -1,6 +1,9 @@
#include <AP_BoardConfig/AP_BoardConfig.h>
#include "OpticalFlow.h"
#include "AP_OpticalFlow_Onboard.h"
#include "AP_OpticalFlow_SITL.h"
#include "AP_OpticalFlow_Pixart.h"
#include "AP_OpticalFlow_PX4Flow.h"
extern const AP_HAL::HAL& hal;
@ -55,6 +58,13 @@ const AP_Param::GroupInfo OpticalFlow::var_info[] = {
// @User: Advanced
AP_GROUPINFO("_POS", 4, OpticalFlow, _pos_offset, 0.0f),
// @Param: _BUS_ID
// @DisplayName: ID on the bus
// @Description: This is used to select between multiple possible bus IDs for some sensor types. For PX4Flow you can choose 0 to 7 for the 8 possible addresses on the I2C bus.
// @Range: 0 255
// @User: Advanced
AP_GROUPINFO("_BUS_ID", 5, OpticalFlow, _bus_id, 0),
AP_GROUPEND
};
@ -80,7 +90,7 @@ void OpticalFlow::init(void)
backend = AP_OpticalFlow_Pixart::detect(*this);
}
if (backend == nullptr) {
backend = new AP_OpticalFlow_PX4(*this);
backend = AP_OpticalFlow_PX4Flow::detect(*this);
}
#elif CONFIG_HAL_BOARD == HAL_BOARD_SITL
backend = new AP_OpticalFlow_SITL(*this);
@ -89,7 +99,7 @@ void OpticalFlow::init(void)
CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BBBMINI
backend = new AP_OpticalFlow_Onboard(*this);
#elif CONFIG_HAL_BOARD == HAL_BOARD_LINUX
backend = new AP_OpticalFlow_Linux(*this, hal.i2c_mgr->get_device(HAL_OPTFLOW_PX4FLOW_I2C_BUS, HAL_OPTFLOW_PX4FLOW_I2C_ADDRESS));
backend = AP_OpticalFlow_PX4Flow::detect(*this);
#endif
}

5
libraries/AP_OpticalFlow/OpticalFlow.h
View File

@ -89,6 +89,7 @@ private:
AP_Int16 _flowScalerY; // Y axis flow scale factor correction - parts per thousand
AP_Int16 _yawAngle_cd; // yaw angle of sensor X axis with respect to vehicle X axis - centi degrees
AP_Vector3f _pos_offset; // position offset of the flow sensor in the body frame
AP_Int8 _bus_id; // ID on bus (some sensors only)
// state filled in by backend
struct OpticalFlow_state _state;
@ -97,7 +98,3 @@ private:
};
#include "OpticalFlow_backend.h"
#include "AP_OpticalFlow_SITL.h"
#include "AP_OpticalFlow_PX4.h"
#include "AP_OpticalFlow_Linux.h"
#include "AP_OpticalFlow_Pixart.h"

3
libraries/AP_OpticalFlow/OpticalFlow_backend.h
View File

@ -53,6 +53,9 @@ protected:
// get access to AHRS object
AP_AHRS_NavEKF &get_ahrs(void) { return frontend._ahrs; }
// get bus ID parameter
uint8_t get_bus_id(void) const { return frontend._bus_id; }
// semaphore for access to shared frontend data
AP_HAL::Semaphore *_sem;