fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-25 16:23:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

added px4flow integral frame, adjusted px4flow i2c driver, adjusted postition_estimator_inav

Browse Source
This commit is contained in:
dominiho
2014-10-28 12:35:20 +01:00
parent 1ff9e4d665
commit 276dc7fbbc
3 changed files with 319 additions and 326 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/px4flow/px4flow.cpp
+299 -312
View File
@@ -73,15 +73,15 @@
#include <board_config.h> #include <board_config.h>
/* Configuration Constants */ /* Configuration Constants */
#define PX4FLOW_BUS PX4_I2C_BUS_EXPANSION #define PX4FLOW_BUS PX4_I2C_BUS_ESC//PX4_I2C_BUS_EXPANSION
#define I2C_FLOW_ADDRESS 0x42 //* 7-bit address. 8-bit address is 0x84 #define I2C_FLOW_ADDRESS 0x42 //* 7-bit address. 8-bit address is 0x84
//range 0x42 - 0x49 //range 0x42 - 0x49
/* PX4FLOW Registers addresses */ /* PX4FLOW Registers addresses */
#define PX4FLOW_REG 0x00 /* Measure Register */ //#define PX4FLOW_REG 0x00 /* Measure Register 0*/
#define PX4FLOW_REG 0x16 /* Measure Register 22*/
#define PX4FLOW_CONVERSION_INTERVAL 8000 /* 8ms 125Hz */
#define PX4FLOW_CONVERSION_INTERVAL 20000 //in microseconds! 20000 = 50 Hz 100000 = 10Hz
/* oddly, ERROR is not defined for c++ */ /* oddly, ERROR is not defined for c++ */
#ifdef ERROR #ifdef ERROR
# undef ERROR # undef ERROR
@@ -92,94 +92,103 @@ static const int ERROR = -1;
# error This requires CONFIG_SCHED_WORKQUEUE. # error This requires CONFIG_SCHED_WORKQUEUE.
#endif #endif
//struct i2c_frame struct i2c_frame {
//{ uint16_t frame_count;
// uint16_t frame_count; int16_t pixel_flow_x_sum;
// int16_t pixel_flow_x_sum; int16_t pixel_flow_y_sum;
// int16_t pixel_flow_y_sum; int16_t flow_comp_m_x;
// int16_t flow_comp_m_x; int16_t flow_comp_m_y;
// int16_t flow_comp_m_y; int16_t qual;
// int16_t qual; int16_t gyro_x_rate;
// int16_t gyro_x_rate; int16_t gyro_y_rate;
// int16_t gyro_y_rate; int16_t gyro_z_rate;
// int16_t gyro_z_rate; uint8_t gyro_range;
// uint8_t gyro_range; uint8_t sonar_timestamp;
// uint8_t sonar_timestamp; int16_t ground_distance;
// int16_t ground_distance; };
//}; struct i2c_frame f;
//
//struct i2c_frame f;
class PX4FLOW : public device::I2C 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 time_since_last_sonar_update;
uint16_t ground_distance;
uint8_t qual;
}__attribute__((packed));
struct i2c_integral_frame f_integral;
class PX4FLOW: public device::I2C {
public: public:
PX4FLOW(int bus = PX4FLOW_BUS, int address = I2C_FLOW_ADDRESS); PX4FLOW(int bus = PX4FLOW_BUS, int address = I2C_FLOW_ADDRESS);
virtual ~PX4FLOW(); virtual ~PX4FLOW();
virtual int init(); virtual int init();
virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); virtual ssize_t read(struct file *filp, char *buffer, size_t buflen);
virtual int ioctl(struct file *filp, int cmd, unsigned long arg); virtual int ioctl(struct file *filp, int cmd, unsigned long arg);
/** /**
* Diagnostics - print some basic information about the driver. * Diagnostics - print some basic information about the driver.
*/ */
void print_info(); void print_info();
protected: protected:
virtual int probe(); virtual int probe();
private: private:
work_s _work; work_s _work;
RingBuffer *_reports; RingBuffer *_reports;bool _sensor_ok;
bool _sensor_ok; int _measure_ticks;bool _collect_phase;
int _measure_ticks;
bool _collect_phase;
orb_advert_t _px4flow_topic; orb_advert_t _px4flow_topic;
perf_counter_t _sample_perf; perf_counter_t _sample_perf;
perf_counter_t _comms_errors; perf_counter_t _comms_errors;
perf_counter_t _buffer_overflows; perf_counter_t _buffer_overflows;
/** /**
* Test whether the device supported by the driver is present at a * Test whether the device supported by the driver is present at a
* specific address. * specific address.
* *
* @param address The I2C bus address to probe. * @param address The I2C bus address to probe.
* @return True if the device is present. * @return True if the device is present.
*/ */
int probe_address(uint8_t address); int probe_address(uint8_t address);
/** /**
* Initialise the automatic measurement state machine and start it. * Initialise the automatic measurement state machine and start it.
* *
* @note This function is called at open and error time. It might make sense * @note This function is called at open and error time. It might make sense
* to make it more aggressive about resetting the bus in case of errors. * to make it more aggressive about resetting the bus in case of errors.
*/ */
void start(); void start();
/** /**
* Stop the automatic measurement state machine. * Stop the automatic measurement state machine.
*/ */
void stop(); void stop();
/** /**
* Perform a poll cycle; collect from the previous measurement * Perform a poll cycle; collect from the previous measurement
* and start a new one. * and start a new one.
*/ */
void cycle(); void cycle();
int measure(); int measure();
int collect(); int collect();
/** /**
* Static trampoline from the workq context; because we don't have a * Static trampoline from the workq context; because we don't have a
* generic workq wrapper yet. * generic workq wrapper yet.
* *
* @param arg Instance pointer for the driver that is polling. * @param arg Instance pointer for the driver that is polling.
*/ */
static void cycle_trampoline(void *arg); static void cycle_trampoline(void *arg);
}; };
@@ -189,16 +198,12 @@ private:
extern "C" __EXPORT int px4flow_main(int argc, char *argv[]); extern "C" __EXPORT int px4flow_main(int argc, char *argv[]);
PX4FLOW::PX4FLOW(int bus, int address) : PX4FLOW::PX4FLOW(int bus, int address) :
I2C("PX4FLOW", PX4FLOW_DEVICE_PATH, bus, address, 400000),//400khz I2C("PX4FLOW", PX4FLOW_DEVICE_PATH, bus, address, 400000), //400khz
_reports(nullptr), _reports(nullptr), _sensor_ok(false), _measure_ticks(0), _collect_phase(
_sensor_ok(false), false), _px4flow_topic(-1), _sample_perf(
_measure_ticks(0), perf_alloc(PC_ELAPSED, "px4flow_read")), _comms_errors(
_collect_phase(false), perf_alloc(PC_COUNT, "px4flow_comms_errors")), _buffer_overflows(
_px4flow_topic(-1), perf_alloc(PC_COUNT, "px4flow_buffer_overflows")) {
_sample_perf(perf_alloc(PC_ELAPSED, "px4flow_read")),
_comms_errors(perf_alloc(PC_COUNT, "px4flow_comms_errors")),
_buffer_overflows(perf_alloc(PC_COUNT, "px4flow_buffer_overflows"))
{
// enable debug() calls // enable debug() calls
_debug_enabled = true; _debug_enabled = true;
@@ -206,8 +211,7 @@ PX4FLOW::PX4FLOW(int bus, int address) :
memset(&_work, 0, sizeof(_work)); memset(&_work, 0, sizeof(_work));
} }
PX4FLOW::~PX4FLOW() PX4FLOW::~PX4FLOW() {
{
/* make sure we are truly inactive */ /* make sure we are truly inactive */
stop(); stop();
@@ -216,9 +220,7 @@ PX4FLOW::~PX4FLOW()
delete _reports; delete _reports;
} }
int int PX4FLOW::init() {
PX4FLOW::init()
{
int ret = ERROR; int ret = ERROR;
/* do I2C init (and probe) first */ /* do I2C init (and probe) first */
@@ -226,103 +228,79 @@ PX4FLOW::init()
goto out; goto out;
/* allocate basic report buffers */ /* allocate basic report buffers */
_reports = new RingBuffer(2, sizeof(struct optical_flow_s)); _reports = new RingBuffer(2, sizeof(optical_flow_s));
if (_reports == nullptr) if (_reports == nullptr)
goto out; goto out;
/* get a publish handle on the px4flow topic */
struct optical_flow_s zero_report;
memset(&zero_report, 0, sizeof(zero_report));
_px4flow_topic = orb_advertise(ORB_ID(optical_flow), &zero_report);
if (_px4flow_topic < 0)
debug("failed to create px4flow object. Did you start uOrb?");
ret = OK; ret = OK;
/* sensor is ok, but we don't really know if it is within range */ /* sensor is ok, but we don't really know if it is within range */
_sensor_ok = true; _sensor_ok = true;
out: out: return ret;
return ret;
} }
int int PX4FLOW::probe() {
PX4FLOW::probe()
{
uint8_t val[22];
// to be sure this is not a ll40ls Lidar (which can also be on
// 0x42) we check if a 22 byte transfer works from address
// 0. The ll40ls gives an error for that, whereas the flow
// happily returns some data
if (transfer(nullptr, 0, &val[0], 22) != OK) {
return -EIO;
}
// that worked, so start a measurement cycle
return measure(); return measure();
} }
int int PX4FLOW::ioctl(struct file *filp, int cmd, unsigned long arg) {
PX4FLOW::ioctl(struct file *filp, int cmd, unsigned long arg)
{
switch (cmd) { switch (cmd) {
case SENSORIOCSPOLLRATE: { case SENSORIOCSPOLLRATE: {
switch (arg) { switch (arg) {
/* switching to manual polling */ /* switching to manual polling */
case SENSOR_POLLRATE_MANUAL: case SENSOR_POLLRATE_MANUAL:
stop(); stop();
_measure_ticks = 0; _measure_ticks = 0;
return OK; return OK;
/* external signalling (DRDY) not supported */ /* external signalling (DRDY) not supported */
case SENSOR_POLLRATE_EXTERNAL: case SENSOR_POLLRATE_EXTERNAL:
/* zero would be bad */ /* zero would be bad */
case 0: case 0:
return -EINVAL;
/* set default/max polling rate */
case SENSOR_POLLRATE_MAX:
case SENSOR_POLLRATE_DEFAULT: {
/* do we need to start internal polling? */
bool want_start = (_measure_ticks == 0);
/* set interval for next measurement to minimum legal value */
_measure_ticks = USEC2TICK(PX4FLOW_CONVERSION_INTERVAL);
/* if we need to start the poll state machine, do it */
if (want_start)
start();
return OK;
}
/* adjust to a legal polling interval in Hz */
default: {
/* do we need to start internal polling? */
bool want_start = (_measure_ticks == 0);
/* convert hz to tick interval via microseconds */
unsigned ticks = USEC2TICK(1000000 / arg);
/* check against maximum rate */
if (ticks < USEC2TICK(PX4FLOW_CONVERSION_INTERVAL))
return -EINVAL; return -EINVAL;
/* set default/max polling rate */ /* update interval for next measurement */
case SENSOR_POLLRATE_MAX: _measure_ticks = ticks;
case SENSOR_POLLRATE_DEFAULT: {
/* do we need to start internal polling? */
bool want_start = (_measure_ticks == 0);
/* set interval for next measurement to minimum legal value */ /* if we need to start the poll state machine, do it */
_measure_ticks = USEC2TICK(PX4FLOW_CONVERSION_INTERVAL); if (want_start)
start();
/* if we need to start the poll state machine, do it */ return OK;
if (want_start)
start();
return OK;
}
/* adjust to a legal polling interval in Hz */
default: {
/* do we need to start internal polling? */
bool want_start = (_measure_ticks == 0);
/* convert hz to tick interval via microseconds */
unsigned ticks = USEC2TICK(1000000 / arg);
/* check against maximum rate */
if (ticks < USEC2TICK(PX4FLOW_CONVERSION_INTERVAL))
return -EINVAL;
/* update interval for next measurement */
_measure_ticks = ticks;
/* if we need to start the poll state machine, do it */
if (want_start)
start();
return OK;
}
}
} }
}
}
case SENSORIOCGPOLLRATE: case SENSORIOCGPOLLRATE:
if (_measure_ticks == 0) if (_measure_ticks == 0)
@@ -358,11 +336,10 @@ PX4FLOW::ioctl(struct file *filp, int cmd, unsigned long arg)
} }
} }
ssize_t ssize_t PX4FLOW::read(struct file *filp, char *buffer, size_t buflen) {
PX4FLOW::read(struct file *filp, char *buffer, size_t buflen)
{
unsigned count = buflen / sizeof(struct optical_flow_s); unsigned count = buflen / sizeof(struct optical_flow_s);
struct optical_flow_s *rbuf = reinterpret_cast<struct optical_flow_s *>(buffer); struct optical_flow_s *rbuf =
reinterpret_cast<struct optical_flow_s *>(buffer);
int ret = 0; int ret = 0;
/* buffer must be large enough */ /* buffer must be large enough */
@@ -398,8 +375,8 @@ PX4FLOW::read(struct file *filp, char *buffer, size_t buflen)
break; break;
} }
/* wait for it to complete */ // /* wait for it to complete */
usleep(PX4FLOW_CONVERSION_INTERVAL); // usleep(PX4FLOW_CONVERSION_INTERVAL);
/* run the collection phase */ /* run the collection phase */
if (OK != collect()) { if (OK != collect()) {
@@ -407,6 +384,9 @@ PX4FLOW::read(struct file *filp, char *buffer, size_t buflen)
break; break;
} }
/* wait for it to complete */
usleep(PX4FLOW_CONVERSION_INTERVAL);
/* state machine will have generated a report, copy it out */ /* state machine will have generated a report, copy it out */
if (_reports->get(rbuf)) { if (_reports->get(rbuf)) {
ret = sizeof(*rbuf); ret = sizeof(*rbuf);
@@ -417,9 +397,7 @@ PX4FLOW::read(struct file *filp, char *buffer, size_t buflen)
return ret; return ret;
} }
int int PX4FLOW::measure() {
PX4FLOW::measure()
{
int ret; int ret;
/* /*
@@ -428,11 +406,10 @@ PX4FLOW::measure()
uint8_t cmd = PX4FLOW_REG; uint8_t cmd = PX4FLOW_REG;
ret = transfer(&cmd, 1, nullptr, 0); ret = transfer(&cmd, 1, nullptr, 0);
if (OK != ret) if (OK != ret) {
{
perf_count(_comms_errors); perf_count(_comms_errors);
log("i2c::transfer returned %d", ret); log("i2c::transfer returned %d", ret);
printf("i2c::transfer flow returned %d"); printf("i2c::transfer flow returned %d");
return ret; return ret;
} }
ret = OK; ret = OK;
@@ -440,56 +417,90 @@ PX4FLOW::measure()
return ret; return ret;
} }
int int PX4FLOW::collect() {
PX4FLOW::collect() int ret = -EIO;
{
int ret = -EIO;
/* read from the sensor */ /* read from the sensor */
uint8_t val[22] = {0, 0,0, 0,0, 0,0, 0,0, 0,0, 0,0, 0,0, 0,0, 0,0, 0,0, 0}; uint8_t val[46] = { 0 };
perf_begin(_sample_perf); perf_begin(_sample_perf);
ret = transfer(nullptr, 0, &val[0], 22); if(PX4FLOW_REG==0x00){
ret = transfer(nullptr, 0, &val[0], 45); //read 45 bytes (22+23 : frame1 + frame2)
}
if(PX4FLOW_REG==0x16){
ret = transfer(nullptr, 0, &val[0], 23); //read 23 bytes (only frame2)
}
if (ret < 0) if (ret < 0) {
{
log("error reading from sensor: %d", ret); log("error reading from sensor: %d", ret);
perf_count(_comms_errors); perf_count(_comms_errors);
perf_end(_sample_perf); perf_end(_sample_perf);
return ret; return ret;
} }
// f.frame_count = val[1] << 8 | val[0]; if (PX4FLOW_REG == 0) {
// f.pixel_flow_x_sum= val[3] << 8 | val[2]; f.frame_count = val[1] << 8 | val[0];
// f.pixel_flow_y_sum= val[5] << 8 | val[4]; f.pixel_flow_x_sum = val[3] << 8 | val[2];
// f.flow_comp_m_x= val[7] << 8 | val[6]; f.pixel_flow_y_sum = val[5] << 8 | val[4];
// f.flow_comp_m_y= val[9] << 8 | val[8]; f.flow_comp_m_x = val[7] << 8 | val[6];
// f.qual= val[11] << 8 | val[10]; f.flow_comp_m_y = val[9] << 8 | val[8];
// f.gyro_x_rate= val[13] << 8 | val[12]; f.qual = val[11] << 8 | val[10];
// f.gyro_y_rate= val[15] << 8 | val[14]; f.gyro_x_rate = val[13] << 8 | val[12];
// f.gyro_z_rate= val[17] << 8 | val[16]; f.gyro_y_rate = val[15] << 8 | val[14];
// f.gyro_range= val[18]; f.gyro_z_rate = val[17] << 8 | val[16];
// f.sonar_timestamp= val[19]; f.gyro_range = val[18];
// f.ground_distance= val[21] << 8 | val[20]; f.sonar_timestamp = val[19];
f.ground_distance = val[21] << 8 | val[20];
f_integral.frame_count_since_last_readout = val[23] << 8 | val[22];
f_integral.pixel_flow_x_integral = val[25] << 8 | val[24];
f_integral.pixel_flow_y_integral = val[27] << 8 | val[26];
f_integral.gyro_x_rate_integral = val[29] << 8 | val[28];
f_integral.gyro_y_rate_integral = val[31] << 8 | val[30];
f_integral.gyro_z_rate_integral = val[33] << 8 | val[32];
f_integral.integration_timespan = val[37] << 24 | val[36] << 16
| val[35] << 8 | val[34];
f_integral.time_since_last_sonar_update = val[41] << 24 | val[40] << 16
| val[39] << 8 | val[38];
f_integral.ground_distance = val[43] << 8 | val[42];
f_integral.qual = val[44];
}
if(PX4FLOW_REG==0x16){
f_integral.frame_count_since_last_readout = val[1] << 8 | val[0];
f_integral.pixel_flow_x_integral =val[3] << 8 | val[2];
f_integral.pixel_flow_y_integral =val[5] << 8 | val[4];
f_integral.gyro_x_rate_integral =val[7] << 8 | val[6];
f_integral.gyro_y_rate_integral =val[9] << 8 | val[8];
f_integral.gyro_z_rate_integral =val[11] << 8 | val[10];
f_integral.integration_timespan = val[15] <<24 |val[14] << 16 |val[13] << 8 |val[12];
f_integral.time_since_last_sonar_update = val[19] <<24 |val[18] << 16 |val[17] << 8 |val[16];
f_integral.ground_distance =val[21] <<8 |val[20];
f_integral.qual =val[22];
}
int16_t flowcx = val[7] << 8 | val[6];
int16_t flowcy = val[9] << 8 | val[8];
int16_t gdist = val[21] << 8 | val[20];
struct optical_flow_s report; struct optical_flow_s report;
report.flow_comp_x_m = float(flowcx)/1000.0f;
report.flow_comp_y_m = float(flowcy)/1000.0f;
report.flow_raw_x= val[3] << 8 | val[2];
report.flow_raw_y= val[5] << 8 | val[4];
report.ground_distance_m =float(gdist)/1000.0f;
report.quality= val[10];
report.sensor_id = 0;
report.timestamp = hrt_absolute_time(); report.timestamp = hrt_absolute_time();
report.pixel_flow_x_integral = float(f_integral.pixel_flow_x_integral) / 10000.0f;//convert to radians
report.pixel_flow_y_integral = 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 = 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= float(f_integral.gyro_x_rate_integral)/10000.0f;//convert to radians
report.gyro_y_rate_integral= float(f_integral.gyro_y_rate_integral)/10000.0f;//convert to radians
report.gyro_z_rate_integral= 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.time_since_last_sonar_update;//microseconds
report.sensor_id = 0;
if (_px4flow_topic < 0) {
/* publish it */ _px4flow_topic = orb_advertise(ORB_ID(optical_flow), &report);
orb_publish(ORB_ID(optical_flow), _px4flow_topic, &report); } else {
/* publish it */
orb_publish(ORB_ID(optical_flow), _px4flow_topic, &report);
}
/* post a report to the ring */ /* post a report to the ring */
if (_reports->force(&report)) { if (_reports->force(&report)) {
@@ -505,22 +516,17 @@ PX4FLOW::collect()
return ret; return ret;
} }
void void PX4FLOW::start() {
PX4FLOW::start()
{
/* reset the report ring and state machine */ /* reset the report ring and state machine */
_collect_phase = false; _collect_phase = false;
_reports->flush(); _reports->flush();
/* schedule a cycle to start things */ /* schedule a cycle to start things */
work_queue(HPWORK, &_work, (worker_t)&PX4FLOW::cycle_trampoline, this, 1); work_queue(HPWORK, &_work, (worker_t) & PX4FLOW::cycle_trampoline, this, 1);
/* notify about state change */ /* notify about state change */
struct subsystem_info_s info = { struct subsystem_info_s info = { true, true, true,
true, SUBSYSTEM_TYPE_OPTICALFLOW };
true,
true,
SUBSYSTEM_TYPE_OPTICALFLOW};
static orb_advert_t pub = -1; static orb_advert_t pub = -1;
if (pub > 0) { if (pub > 0) {
@@ -530,71 +536,54 @@ PX4FLOW::start()
} }
} }
void void PX4FLOW::stop() {
PX4FLOW::stop()
{
work_cancel(HPWORK, &_work); work_cancel(HPWORK, &_work);
} }
void void PX4FLOW::cycle_trampoline(void *arg) {
PX4FLOW::cycle_trampoline(void *arg) PX4FLOW *dev = (PX4FLOW *) arg;
{
PX4FLOW *dev = (PX4FLOW *)arg;
dev->cycle(); dev->cycle();
} }
void void PX4FLOW::cycle() {
PX4FLOW::cycle() // /* collection phase? */
{
/* collection phase? */
if (_collect_phase) {
/* perform collection */ // static uint64_t deltatime = hrt_absolute_time();
if (OK != collect()) {
log("collection error");
/* restart the measurement state machine */
start();
return;
}
/* next phase is measurement */
_collect_phase = false;
/*
* Is there a collect->measure gap?
*/
if (_measure_ticks > USEC2TICK(PX4FLOW_CONVERSION_INTERVAL)) {
/* schedule a fresh cycle call when we are ready to measure again */
work_queue(HPWORK,
&_work,
(worker_t)&PX4FLOW::cycle_trampoline,
this,
_measure_ticks - USEC2TICK(PX4FLOW_CONVERSION_INTERVAL));
return;
}
}
/* measurement phase */
if (OK != measure()) if (OK != measure())
log("measure error"); log("measure error");
/* next phase is collection */ //usleep(PX4FLOW_CONVERSION_INTERVAL/40);
_collect_phase = true;
/* perform collection */
if (OK != collect()) {
log("collection error");
/* restart the measurement state machine */
start();
return;
}
// deltatime = hrt_absolute_time()-deltatime;
//
//
// if(deltatime>PX4FLOW_CONVERSION_INTERVAL){
// deltatime=PX4FLOW_CONVERSION_INTERVAL;
// }
// work_queue(HPWORK, &_work, (worker_t) & PX4FLOW::cycle_trampoline, this,
// _measure_ticks-USEC2TICK(deltatime));
work_queue(HPWORK, &_work, (worker_t) & PX4FLOW::cycle_trampoline, this,
_measure_ticks);
// deltatime = hrt_absolute_time();
/* schedule a fresh cycle call when the measurement is done */
work_queue(HPWORK,
&_work,
(worker_t)&PX4FLOW::cycle_trampoline,
this,
USEC2TICK(PX4FLOW_CONVERSION_INTERVAL));
} }
void void PX4FLOW::print_info() {
PX4FLOW::print_info()
{
perf_print_counter(_sample_perf); perf_print_counter(_sample_perf);
perf_print_counter(_comms_errors); perf_print_counter(_comms_errors);
perf_print_counter(_buffer_overflows); perf_print_counter(_buffer_overflows);
@@ -605,8 +594,7 @@ PX4FLOW::print_info()
/** /**
* Local functions in support of the shell command. * Local functions in support of the shell command.
*/ */
namespace px4flow namespace px4flow {
{
/* oddly, ERROR is not defined for c++ */ /* oddly, ERROR is not defined for c++ */
#ifdef ERROR #ifdef ERROR
@@ -614,20 +602,18 @@ namespace px4flow
#endif #endif
const int ERROR = -1; const int ERROR = -1;
PX4FLOW *g_dev; PX4FLOW *g_dev;
void start(); void start();
void stop(); void stop();
void test(); void test();
void reset(); void reset();
void info(); void info();
/** /**
* Start the driver. * Start the driver.
*/ */
void void start() {
start()
{
int fd; int fd;
if (g_dev != nullptr) if (g_dev != nullptr)
@@ -653,10 +639,9 @@ start()
exit(0); exit(0);
fail: fail:
if (g_dev != nullptr) if (g_dev != nullptr) {
{
delete g_dev; delete g_dev;
g_dev = nullptr; g_dev = nullptr;
} }
@@ -667,15 +652,11 @@ fail:
/** /**
* Stop the driver * Stop the driver
*/ */
void stop() void stop() {
{ if (g_dev != nullptr) {
if (g_dev != nullptr)
{
delete g_dev; delete g_dev;
g_dev = nullptr; g_dev = nullptr;
} } else {
else
{
errx(1, "driver not running"); errx(1, "driver not running");
} }
exit(0); exit(0);
@@ -686,9 +667,7 @@ void stop()
* make sure we can collect data from the sensor in polled * make sure we can collect data from the sensor in polled
* and automatic modes. * and automatic modes.
*/ */
void void test() {
test()
{
struct optical_flow_s report; struct optical_flow_s report;
ssize_t sz; ssize_t sz;
int ret; int ret;
@@ -696,26 +675,27 @@ test()
int fd = open(PX4FLOW_DEVICE_PATH, O_RDONLY); int fd = open(PX4FLOW_DEVICE_PATH, O_RDONLY);
if (fd < 0) if (fd < 0)
err(1, "%s open failed (try 'px4flow start' if the driver is not running", PX4FLOW_DEVICE_PATH); err(1,
"%s open failed (try 'px4flow start' if the driver is not running",
PX4FLOW_DEVICE_PATH);
/* do a simple demand read */ /* do a simple demand read */
sz = read(fd, &report, sizeof(report)); sz = read(fd, &report, sizeof(report));
if (sz != sizeof(report)) if (sz != sizeof(report))
// err(1, "immediate read failed"); // err(1, "immediate read failed");
warnx("single read"); warnx("single read");
warnx("flowx: %0.2f m/s", (double)report.flow_comp_x_m); warnx("flowx: %0.2f m/s", (double) f.pixel_flow_x_sum);
warnx("flowy: %0.2f m/s", (double)report.flow_comp_y_m); warnx("flowy: %0.2f m/s", (double) f.pixel_flow_y_sum);
warnx("time: %lld", report.timestamp); warnx("time: %lld", report.timestamp);
/* start the sensor polling at 10Hz */
/* start the sensor polling at 2Hz */ if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 10)) // 2))
if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) errx(1, "failed to set 10Hz poll rate");
errx(1, "failed to set 2Hz poll rate");
/* read the sensor 5x and report each value */ /* read the sensor 5x and report each value */
for (unsigned i = 0; i < 5; i++) { for (unsigned i = 0; i < 10; i++) {
struct pollfd fds; struct pollfd fds;
/* wait for data to be ready */ /* wait for data to be ready */
@@ -733,9 +713,22 @@ test()
err(1, "periodic read failed"); err(1, "periodic read failed");
warnx("periodic read %u", i); warnx("periodic read %u", i);
warnx("flowx: %0.2f m/s", (double)report.flow_comp_x_m);
warnx("flowy: %0.2f m/s", (double)report.flow_comp_y_m); warnx("framecount_total: %u", f.frame_count);
warnx("time: %lld", report.timestamp); warnx("framecount_integral: %u",
f_integral.frame_count_since_last_readout);
warnx("pixel_flow_x_integral: %i", f_integral.pixel_flow_x_integral);
warnx("pixel_flow_y_integral: %i", f_integral.pixel_flow_y_integral);
warnx("gyro_x_rate_integral: %i", f_integral.gyro_x_rate_integral);
warnx("gyro_y_rate_integral: %i", f_integral.gyro_y_rate_integral);
warnx("gyro_z_rate_integral: %i", f_integral.gyro_z_rate_integral);
warnx("integration_timespan [us]: %u", f_integral.integration_timespan);
warnx("ground_distance: %0.2f m",
(double) f_integral.ground_distance / 1000);
warnx("time since last sonar update [us]: %i",
f_integral.time_since_last_sonar_update);
warnx("quality integration average : %i", f_integral.qual);
warnx("quality : %i", f.qual);
} }
@@ -746,9 +739,7 @@ test()
/** /**
* Reset the driver. * Reset the driver.
*/ */
void void reset() {
reset()
{
int fd = open(PX4FLOW_DEVICE_PATH, O_RDONLY); int fd = open(PX4FLOW_DEVICE_PATH, O_RDONLY);
if (fd < 0) if (fd < 0)
@@ -766,9 +757,7 @@ reset()
/** /**
* Print a little info about the driver. * Print a little info about the driver.
*/ */
void void info() {
info()
{
if (g_dev == nullptr) if (g_dev == nullptr)
errx(1, "driver not running"); errx(1, "driver not running");
@@ -780,20 +769,18 @@ info()
} // namespace } // namespace
int int px4flow_main(int argc, char *argv[]) {
px4flow_main(int argc, char *argv[])
{
/* /*
* Start/load the driver. * Start/load the driver.
*/ */
if (!strcmp(argv[1], "start")) if (!strcmp(argv[1], "start"))
px4flow::start(); px4flow::start();
/* /*
* Stop the driver * Stop the driver
*/ */
if (!strcmp(argv[1], "stop")) if (!strcmp(argv[1], "stop"))
px4flow::stop(); px4flow::stop();
/* /*
* Test the driver/device. * Test the driver/device.
src/modules/position_estimator_inav/position_estimator_inav_main.c
+6 -5
View File
@@ -298,7 +298,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
float w_flow = 0.0f; float w_flow = 0.0f;
float sonar_prev = 0.0f; float sonar_prev = 0.0f;
hrt_abstime flow_prev = 0; // time of last flow measurement //hrt_abstime flow_prev = 0; // time of last flow measurement
hrt_abstime sonar_time = 0; // time of last sonar measurement (not filtered) hrt_abstime sonar_time = 0; // time of last sonar measurement (not filtered)
hrt_abstime sonar_valid_time = 0; // time of last sonar measurement used for correction (filtered) hrt_abstime sonar_valid_time = 0; // time of last sonar measurement used for correction (filtered)
@@ -491,8 +491,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
orb_copy(ORB_ID(optical_flow), optical_flow_sub, &flow); orb_copy(ORB_ID(optical_flow), optical_flow_sub, &flow);
/* calculate time from previous update */ /* calculate time from previous update */
float flow_dt = flow_prev > 0 ? (flow.flow_timestamp - flow_prev) * 1e-6f : 0.1f; // float flow_dt = flow_prev > 0 ? (flow.flow_timestamp - flow_prev) * 1e-6f : 0.1f;
flow_prev = flow.flow_timestamp; // flow_prev = flow.flow_timestamp;
if ((flow.ground_distance_m > 0.31f) && if ((flow.ground_distance_m > 0.31f) &&
(flow.ground_distance_m < 4.0f) && (flow.ground_distance_m < 4.0f) &&
@@ -550,8 +550,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
/* convert raw flow to angular flow (rad/s) */ /* convert raw flow to angular flow (rad/s) */
float flow_ang[2]; float flow_ang[2];
flow_ang[0] = flow.flow_raw_x * params.flow_k / 1000.0f / flow_dt; //todo check direction of x und y axis
flow_ang[1] = flow.flow_raw_y * params.flow_k / 1000.0f / flow_dt; flow_ang[0] = flow.pixel_flow_x_integral/(float)flow.integration_timespan*1000000.0f;//flow.flow_raw_x * params.flow_k / 1000.0f / flow_dt;
flow_ang[1] = flow.pixel_flow_y_integral/(float)flow.integration_timespan*1000000.0f;//flow.flow_raw_y * params.flow_k / 1000.0f / flow_dt;
/* flow measurements vector */ /* flow measurements vector */
float flow_m[3]; float flow_m[3];
flow_m[0] = -flow_ang[0] * flow_dist; flow_m[0] = -flow_ang[0] * flow_dist;
src/modules/uORB/topics/optical_flow.h
+14 -9
View File
@@ -55,16 +55,21 @@
*/ */
struct optical_flow_s { struct optical_flow_s {
uint64_t timestamp; /**< in microseconds since system start */ uint64_t timestamp; /**< in microseconds since system start */
uint64_t flow_timestamp; /**< timestamp from flow sensor */
int16_t flow_raw_x; /**< flow in pixels in X direction, not rotation-compensated */
int16_t flow_raw_y; /**< flow in pixels in Y direction, not rotation-compensated */
float flow_comp_x_m; /**< speed over ground in meters, rotation-compensated */
float flow_comp_y_m; /**< speed over ground in meters, rotation-compensated */
float ground_distance_m; /**< Altitude / distance to ground in meters */
uint8_t quality; /**< Quality of the measurement, 0: bad quality, 255: maximum quality */
uint8_t sensor_id; /**< id of the sensor emitting the flow value */ 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 */
uint8_t quality; /**< Average of quality of accumulated frames, 0: bad quality, 255: maximum quality */
}; };