mirror of https://github.com/ArduPilot/ardupilot
AP_OpticalFlow: added common driver for PX4Flow
used on Linux and NuttX boards
This commit is contained in:
Andrew Tridgell
parent
52715c5d16
commit
06dfbc3e09
|
|
@ -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
|
||||
|
|
@ -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;
|
||||
};
|
||||
|
|
@ -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
|
||||
|
|
@ -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)
|
||||
};
|
||||
|
|
@ -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;
|
||||
}
|
||||
|
|
@ -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);
|
||||
};
|
||||
|
|
@ -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>
|
||||
|
||||
|
|
|
|||
|
|
@ -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
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -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"
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
Loading…
Reference in New Issue