aboutsummaryrefslogtreecommitdiff
path: root/src
diff options
context:
space:
mode:
Diffstat (limited to 'src')
-rw-r--r--src/drivers/px4flow/px4flow.cpp345
-rw-r--r--src/examples/flow_position_estimator/flow_position_estimator_main.c4
-rw-r--r--src/modules/mavlink/mavlink_main.cpp2
-rw-r--r--src/modules/mavlink/mavlink_messages.cpp40
-rw-r--r--src/modules/mavlink/mavlink_receiver.cpp40
-rw-r--r--src/modules/mavlink/mavlink_receiver.h2
-rw-r--r--src/modules/position_estimator_inav/position_estimator_inav_main.c11
-rw-r--r--src/modules/sdlog2/sdlog2.c13
-rw-r--r--src/modules/sdlog2/sdlog2_messages.h18
-rw-r--r--src/modules/uORB/topics/optical_flow.h24
10 files changed, 292 insertions, 207 deletions
diff --git a/src/drivers/px4flow/px4flow.cpp b/src/drivers/px4flow/px4flow.cpp
index 804027b05..09ec4bf96 100644
--- a/src/drivers/px4flow/px4flow.cpp
+++ b/src/drivers/px4flow/px4flow.cpp
@@ -1,6 +1,6 @@
/****************************************************************************
*
- * Copyright (c) 2013 PX4 Development Team. All rights reserved.
+ * Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -73,15 +73,13 @@
#include <board_config.h>
/* Configuration Constants */
-#define PX4FLOW_BUS PX4_I2C_BUS_EXPANSION
#define I2C_FLOW_ADDRESS 0x42 //* 7-bit address. 8-bit address is 0x84
//range 0x42 - 0x49
/* PX4FLOW Registers addresses */
-#define PX4FLOW_REG 0x00 /* Measure Register */
-
-#define PX4FLOW_CONVERSION_INTERVAL 8000 /* 8ms 125Hz */
+#define PX4FLOW_REG 0x16 /* Measure Register 22*/
+#define PX4FLOW_CONVERSION_INTERVAL 20000 //in microseconds! 20000 = 50 Hz 100000 = 10Hz
/* oddly, ERROR is not defined for c++ */
#ifdef ERROR
# undef ERROR
@@ -92,28 +90,42 @@ static const int ERROR = -1;
# error This requires CONFIG_SCHED_WORKQUEUE.
#endif
-//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;
-//};
-//
-//struct i2c_frame f;
-
-class PX4FLOW : public device::I2C
+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;
+};
+struct i2c_frame f;
+
+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;
+ int16_t gyro_temperature;
+ uint8_t qual;
+} __attribute__((packed));
+struct i2c_integral_frame f_integral;
+
+
+class PX4FLOW: public device::I2C
{
public:
- PX4FLOW(int bus = PX4FLOW_BUS, int address = I2C_FLOW_ADDRESS);
+ PX4FLOW(int bus, int address = I2C_FLOW_ADDRESS);
virtual ~PX4FLOW();
virtual int init();
@@ -122,8 +134,8 @@ public:
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();
protected:
@@ -144,42 +156,41 @@ private:
perf_counter_t _buffer_overflows;
/**
- * Test whether the device supported by the driver is present at a
- * specific address.
- *
- * @param address The I2C bus address to probe.
- * @return True if the device is present.
- */
+ * Test whether the device supported by the driver is present at a
+ * specific address.
+ *
+ * @param address The I2C bus address to probe.
+ * @return True if the device is present.
+ */
int probe_address(uint8_t address);
/**
- * Initialise the automatic measurement state machine and start it.
- *
- * @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.
- */
+ * Initialise the automatic measurement state machine and start it.
+ *
+ * @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.
+ */
void start();
/**
- * Stop the automatic measurement state machine.
- */
+ * Stop the automatic measurement state machine.
+ */
void stop();
/**
- * Perform a poll cycle; collect from the previous measurement
- * and start a new one.
- */
+ * Perform a poll cycle; collect from the previous measurement
+ * and start a new one.
+ */
void cycle();
int measure();
int collect();
/**
- * Static trampoline from the workq context; because we don't have a
- * generic workq wrapper yet.
- *
- * @param arg Instance pointer for the driver that is polling.
- */
- static void cycle_trampoline(void *arg);
-
+ * Static trampoline from the workq context; because we don't have a
+ * generic workq wrapper yet.
+ *
+ * @param arg Instance pointer for the driver that is polling.
+ */
+ static void cycle_trampoline(void *arg);
};
@@ -189,7 +200,7 @@ private:
extern "C" __EXPORT int px4flow_main(int argc, char *argv[]);
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),
_sensor_ok(false),
_measure_ticks(0),
@@ -228,21 +239,12 @@ PX4FLOW::init()
}
/* allocate basic report buffers */
- _reports = new RingBuffer(2, sizeof(struct optical_flow_s));
+ _reports = new RingBuffer(2, sizeof(optical_flow_s));
if (_reports == nullptr) {
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) {
- warnx("failed to create px4flow object. Did you start uOrb?");
- }
-
ret = OK;
/* sensor is ok, but we don't really know if it is within range */
_sensor_ok = true;
@@ -410,9 +412,6 @@ PX4FLOW::read(struct file *filp, char *buffer, size_t buflen)
break;
}
- /* wait for it to complete */
- usleep(PX4FLOW_CONVERSION_INTERVAL);
-
/* run the collection phase */
if (OK != collect()) {
ret = -EIO;
@@ -442,6 +441,7 @@ PX4FLOW::measure()
if (OK != ret) {
perf_count(_comms_errors);
+ debug("i2c::transfer returned %d", ret);
return ret;
}
@@ -453,14 +453,20 @@ PX4FLOW::measure()
int
PX4FLOW::collect()
{
- int ret = -EIO;
+ int ret = -EIO;
/* 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[47] = { 0 };
perf_begin(_sample_perf);
- ret = transfer(nullptr, 0, &val[0], 22);
+ if (PX4FLOW_REG == 0x00) {
+ ret = transfer(nullptr, 0, &val[0], 47); // read 47 bytes (22+25 : frame1 + frame2)
+ }
+
+ if (PX4FLOW_REG == 0x16) {
+ ret = transfer(nullptr, 0, &val[0], 25); // read 25 bytes (only frame2)
+ }
if (ret < 0) {
debug("error reading from sensor: %d", ret);
@@ -469,36 +475,74 @@ PX4FLOW::collect()
return ret;
}
-// f.frame_count = val[1] << 8 | val[0];
-// f.pixel_flow_x_sum= val[3] << 8 | val[2];
-// f.pixel_flow_y_sum= val[5] << 8 | val[4];
-// f.flow_comp_m_x= val[7] << 8 | val[6];
-// f.flow_comp_m_y= val[9] << 8 | val[8];
-// f.qual= val[11] << 8 | val[10];
-// f.gyro_x_rate= val[13] << 8 | val[12];
-// f.gyro_y_rate= val[15] << 8 | val[14];
-// f.gyro_z_rate= val[17] << 8 | val[16];
-// f.gyro_range= val[18];
-// f.sonar_timestamp= val[19];
-// f.ground_distance= val[21] << 8 | val[20];
+ if (PX4FLOW_REG == 0) {
+ f.frame_count = val[1] << 8 | val[0];
+ f.pixel_flow_x_sum = val[3] << 8 | val[2];
+ f.pixel_flow_y_sum = val[5] << 8 | val[4];
+ f.flow_comp_m_x = val[7] << 8 | val[6];
+ f.flow_comp_m_y = val[9] << 8 | val[8];
+ f.qual = val[11] << 8 | val[10];
+ f.gyro_x_rate = val[13] << 8 | val[12];
+ f.gyro_y_rate = val[15] << 8 | val[14];
+ f.gyro_z_rate = val[17] << 8 | val[16];
+ f.gyro_range = val[18];
+ 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.gyro_temperature = val[45] << 8 | val[44];
+ f_integral.qual = val[46];
+ }
+
+ 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.gyro_temperature = val[23] << 8 | val[22];
+ f_integral.qual = val[24];
+ }
- 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;
- 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.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.time_since_last_sonar_update;//microseconds
+ report.gyro_temperature = f_integral.gyro_temperature;//Temperature * 100 in centi-degrees Celsius
+
+ report.sensor_id = 0;
+ if (_px4flow_topic < 0) {
+ _px4flow_topic = orb_advertise(ORB_ID(optical_flow), &report);
- /* publish it */
- 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 */
if (_reports->force(&report)) {
@@ -558,50 +602,21 @@ PX4FLOW::cycle_trampoline(void *arg)
void
PX4FLOW::cycle()
{
- /* collection phase? */
- if (_collect_phase) {
-
- /* perform collection */
- if (OK != collect()) {
- debug("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()) {
debug("measure error");
}
- /* next phase is collection */
- _collect_phase = true;
+ /* perform collection */
+ if (OK != collect()) {
+ debug("collection error");
+ /* restart the measurement state machine */
+ start();
+ return;
+ }
+
+ work_queue(HPWORK, &_work, (worker_t)&PX4FLOW::cycle_trampoline, this,
+ _measure_ticks);
- /* 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
@@ -647,14 +662,41 @@ start()
}
/* create the driver */
- g_dev = new PX4FLOW(PX4FLOW_BUS);
+ g_dev = new PX4FLOW(PX4_I2C_BUS_EXPANSION);
if (g_dev == nullptr) {
goto fail;
}
if (OK != g_dev->init()) {
- goto fail;
+
+ #ifdef PX4_I2C_BUS_ESC
+ delete g_dev;
+ /* try 2nd bus */
+ g_dev = new PX4FLOW(PX4_I2C_BUS_ESC);
+
+ if (g_dev == nullptr) {
+ goto fail;
+ }
+
+ if (OK != g_dev->init()) {
+ #endif
+
+ delete g_dev;
+ /* try 3rd bus */
+ g_dev = new PX4FLOW(PX4_I2C_BUS_ONBOARD);
+
+ if (g_dev == nullptr) {
+ goto fail;
+ }
+
+ if (OK != g_dev->init()) {
+ goto fail;
+ }
+
+ #ifdef PX4_I2C_BUS_ESC
+ }
+ #endif
}
/* set the poll rate to default, starts automatic data collection */
@@ -683,7 +725,8 @@ fail:
/**
* Stop the driver
*/
-void stop()
+void
+stop()
{
if (g_dev != nullptr) {
delete g_dev;
@@ -714,6 +757,7 @@ test()
err(1, "%s open failed (try 'px4flow start' if the driver is not running", PX4FLOW_DEVICE_PATH);
}
+
/* do a simple demand read */
sz = read(fd, &report, sizeof(report));
@@ -723,18 +767,18 @@ test()
}
warnx("single read");
- 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("time: %lld", report.timestamp);
-
+ 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("framecount_integral: %u",
+ f_integral.frame_count_since_last_readout);
- /* start the sensor polling at 2Hz */
- if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) {
- errx(1, "failed to set 2Hz poll rate");
+ /* start the sensor polling at 10Hz */
+ if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 10)) {
+ errx(1, "failed to set 10Hz poll rate");
}
/* 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;
/* wait for data to be ready */
@@ -754,9 +798,22 @@ test()
}
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("time: %lld", report.timestamp);
+
+ warnx("framecount_total: %u", f.frame_count);
+ 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);
}
diff --git a/src/examples/flow_position_estimator/flow_position_estimator_main.c b/src/examples/flow_position_estimator/flow_position_estimator_main.c
index c775428ef..0b8c01f79 100644
--- a/src/examples/flow_position_estimator/flow_position_estimator_main.c
+++ b/src/examples/flow_position_estimator/flow_position_estimator_main.c
@@ -308,8 +308,8 @@ int flow_position_estimator_thread_main(int argc, char *argv[])
if (vehicle_liftoff || params.debug)
{
/* copy flow */
- flow_speed[0] = flow.flow_comp_x_m;
- flow_speed[1] = flow.flow_comp_y_m;
+ flow_speed[0] = flow.pixel_flow_x_integral / (flow.integration_timespan / 1e6f) * flow.ground_distance_m;
+ flow_speed[1] = flow.pixel_flow_y_integral / (flow.integration_timespan / 1e6f) * flow.ground_distance_m;
flow_speed[2] = 0.0f;
/* convert to bodyframe velocity */
diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index fb9f65cf5..29b7ec7b7 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/src/modules/mavlink/mavlink_main.cpp
@@ -1405,7 +1405,7 @@ Mavlink::task_main(int argc, char *argv[])
configure_stream("POSITION_TARGET_GLOBAL_INT", 3.0f);
configure_stream("ATTITUDE_TARGET", 3.0f);
configure_stream("DISTANCE_SENSOR", 0.5f);
- configure_stream("OPTICAL_FLOW", 5.0f);
+ configure_stream("OPTICAL_FLOW_RAD", 5.0f);
break;
case MAVLINK_MODE_ONBOARD:
diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp
index 978aee118..a8f956ad0 100644
--- a/src/modules/mavlink/mavlink_messages.cpp
+++ b/src/modules/mavlink/mavlink_messages.cpp
@@ -1834,33 +1834,32 @@ protected:
}
};
-
-class MavlinkStreamOpticalFlow : public MavlinkStream
+class MavlinkStreamOpticalFlowRad : public MavlinkStream
{
public:
const char *get_name() const
{
- return MavlinkStreamOpticalFlow::get_name_static();
+ return MavlinkStreamOpticalFlowRad::get_name_static();
}
static const char *get_name_static()
{
- return "OPTICAL_FLOW";
+ return "OPTICAL_FLOW_RAD";
}
uint8_t get_id()
{
- return MAVLINK_MSG_ID_OPTICAL_FLOW;
+ return MAVLINK_MSG_ID_OPTICAL_FLOW_RAD;
}
static MavlinkStream *new_instance(Mavlink *mavlink)
{
- return new MavlinkStreamOpticalFlow(mavlink);
+ return new MavlinkStreamOpticalFlowRad(mavlink);
}
unsigned get_size()
{
- return _flow_sub->is_published() ? (MAVLINK_MSG_ID_OPTICAL_FLOW_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0;
+ return _flow_sub->is_published() ? (MAVLINK_MSG_ID_OPTICAL_FLOW_RAD_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0;
}
private:
@@ -1868,11 +1867,11 @@ private:
uint64_t _flow_time;
/* do not allow top copying this class */
- MavlinkStreamOpticalFlow(MavlinkStreamOpticalFlow &);
- MavlinkStreamOpticalFlow& operator = (const MavlinkStreamOpticalFlow &);
+ MavlinkStreamOpticalFlowRad(MavlinkStreamOpticalFlowRad &);
+ MavlinkStreamOpticalFlowRad& operator = (const MavlinkStreamOpticalFlowRad &);
protected:
- explicit MavlinkStreamOpticalFlow(Mavlink *mavlink) : MavlinkStream(mavlink),
+ explicit MavlinkStreamOpticalFlowRad(Mavlink *mavlink) : MavlinkStream(mavlink),
_flow_sub(_mavlink->add_orb_subscription(ORB_ID(optical_flow))),
_flow_time(0)
{}
@@ -1882,18 +1881,23 @@ protected:
struct optical_flow_s flow;
if (_flow_sub->update(&_flow_time, &flow)) {
- mavlink_optical_flow_t msg;
+ mavlink_optical_flow_rad_t msg;
msg.time_usec = flow.timestamp;
msg.sensor_id = flow.sensor_id;
- msg.flow_x = flow.flow_raw_x;
- msg.flow_y = flow.flow_raw_y;
- msg.flow_comp_m_x = flow.flow_comp_x_m;
- msg.flow_comp_m_y = flow.flow_comp_y_m;
+ msg.integrated_x = flow.pixel_flow_x_integral;
+ msg.integrated_y = flow.pixel_flow_y_integral;
+ msg.integrated_xgyro = flow.gyro_x_rate_integral;
+ msg.integrated_ygyro = flow.gyro_y_rate_integral;
+ msg.integrated_zgyro = flow.gyro_z_rate_integral;
+ msg.distance = flow.ground_distance_m;
msg.quality = flow.quality;
- msg.ground_distance = flow.ground_distance_m;
+ msg.integration_time_us = flow.integration_timespan;
+ msg.sensor_id = flow.sensor_id;
+ msg.time_delta_distance_us = flow.time_since_last_sonar_update;
+ msg.temperature = flow.gyro_temperature;
- _mavlink->send_message(MAVLINK_MSG_ID_OPTICAL_FLOW, &msg);
+ _mavlink->send_message(MAVLINK_MSG_ID_OPTICAL_FLOW_RAD, &msg);
}
}
};
@@ -2199,7 +2203,7 @@ StreamListItem *streams_list[] = {
new StreamListItem(&MavlinkStreamAttitudeTarget::new_instance, &MavlinkStreamAttitudeTarget::get_name_static),
new StreamListItem(&MavlinkStreamRCChannelsRaw::new_instance, &MavlinkStreamRCChannelsRaw::get_name_static),
new StreamListItem(&MavlinkStreamManualControl::new_instance, &MavlinkStreamManualControl::get_name_static),
- new StreamListItem(&MavlinkStreamOpticalFlow::new_instance, &MavlinkStreamOpticalFlow::get_name_static),
+ new StreamListItem(&MavlinkStreamOpticalFlowRad::new_instance, &MavlinkStreamOpticalFlowRad::get_name_static),
new StreamListItem(&MavlinkStreamAttitudeControls::new_instance, &MavlinkStreamAttitudeControls::get_name_static),
new StreamListItem(&MavlinkStreamNamedValueFloat::new_instance, &MavlinkStreamNamedValueFloat::get_name_static),
new StreamListItem(&MavlinkStreamCameraCapture::new_instance, &MavlinkStreamCameraCapture::get_name_static),
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index ca00d1a67..e98d72afe 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -144,8 +144,8 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
handle_message_command_int(msg);
break;
- case MAVLINK_MSG_ID_OPTICAL_FLOW:
- handle_message_optical_flow(msg);
+ case MAVLINK_MSG_ID_OPTICAL_FLOW_RAD:
+ handle_message_optical_flow_rad(msg);
break;
case MAVLINK_MSG_ID_SET_MODE:
@@ -352,24 +352,27 @@ MavlinkReceiver::handle_message_command_int(mavlink_message_t *msg)
}
void
-MavlinkReceiver::handle_message_optical_flow(mavlink_message_t *msg)
+MavlinkReceiver::handle_message_optical_flow_rad(mavlink_message_t *msg)
{
/* optical flow */
- mavlink_optical_flow_t flow;
- mavlink_msg_optical_flow_decode(msg, &flow);
+ mavlink_optical_flow_rad_t flow;
+ mavlink_msg_optical_flow_rad_decode(msg, &flow);
struct optical_flow_s f;
memset(&f, 0, sizeof(f));
- f.timestamp = hrt_absolute_time();
- f.flow_timestamp = flow.time_usec;
- f.flow_raw_x = flow.flow_x;
- f.flow_raw_y = flow.flow_y;
- f.flow_comp_x_m = flow.flow_comp_m_x;
- f.flow_comp_y_m = flow.flow_comp_m_y;
- f.ground_distance_m = flow.ground_distance;
+ f.timestamp = flow.time_usec;
+ f.integration_timespan = flow.integration_time_us;
+ f.pixel_flow_x_integral = flow.integrated_x;
+ f.pixel_flow_y_integral = flow.integrated_y;
+ f.gyro_x_rate_integral = flow.integrated_xgyro;
+ f.gyro_y_rate_integral = flow.integrated_ygyro;
+ f.gyro_z_rate_integral = flow.integrated_zgyro;
+ f.time_since_last_sonar_update = flow.time_delta_distance_us;
+ f.ground_distance_m = flow.distance;
f.quality = flow.quality;
f.sensor_id = flow.sensor_id;
+ f.gyro_temperature = flow.temperature;
if (_flow_pub < 0) {
_flow_pub = orb_advertise(ORB_ID(optical_flow), &f);
@@ -389,13 +392,18 @@ MavlinkReceiver::handle_message_hil_optical_flow(mavlink_message_t *msg)
struct optical_flow_s f;
memset(&f, 0, sizeof(f));
- f.timestamp = hrt_absolute_time();
- f.flow_timestamp = flow.time_usec;
- f.flow_raw_x = flow.integrated_x;
- f.flow_raw_y = flow.integrated_y;
+ f.timestamp = hrt_absolute_time(); // XXX we rely on the system time for now and not flow.time_usec;
+ f.integration_timespan = flow.integration_time_us;
+ f.pixel_flow_x_integral = flow.integrated_x;
+ f.pixel_flow_y_integral = flow.integrated_y;
+ f.gyro_x_rate_integral = flow.integrated_xgyro;
+ f.gyro_y_rate_integral = flow.integrated_ygyro;
+ f.gyro_z_rate_integral = flow.integrated_zgyro;
+ f.time_since_last_sonar_update = flow.time_delta_distance_us;
f.ground_distance_m = flow.distance;
f.quality = flow.quality;
f.sensor_id = flow.sensor_id;
+ f.gyro_temperature = flow.temperature;
if (_flow_pub < 0) {
_flow_pub = orb_advertise(ORB_ID(optical_flow), &f);
diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h
index e5f2c6a73..a057074a7 100644
--- a/src/modules/mavlink/mavlink_receiver.h
+++ b/src/modules/mavlink/mavlink_receiver.h
@@ -112,7 +112,7 @@ private:
void handle_message(mavlink_message_t *msg);
void handle_message_command_long(mavlink_message_t *msg);
void handle_message_command_int(mavlink_message_t *msg);
- void handle_message_optical_flow(mavlink_message_t *msg);
+ void handle_message_optical_flow_rad(mavlink_message_t *msg);
void handle_message_hil_optical_flow(mavlink_message_t *msg);
void handle_message_set_mode(mavlink_message_t *msg);
void handle_message_vicon_position_estimate(mavlink_message_t *msg);
diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c
index e736a86d7..296919c04 100644
--- a/src/modules/position_estimator_inav/position_estimator_inav_main.c
+++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c
@@ -296,7 +296,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
float w_flow = 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_valid_time = 0; // time of last sonar measurement used for correction (filtered)
@@ -489,8 +489,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
orb_copy(ORB_ID(optical_flow), optical_flow_sub, &flow);
/* calculate time from previous update */
- float flow_dt = flow_prev > 0 ? (flow.flow_timestamp - flow_prev) * 1e-6f : 0.1f;
- flow_prev = flow.flow_timestamp;
+// float flow_dt = flow_prev > 0 ? (flow.flow_timestamp - flow_prev) * 1e-6f : 0.1f;
+// flow_prev = flow.flow_timestamp;
if ((flow.ground_distance_m > 0.31f) &&
(flow.ground_distance_m < 4.0f) &&
@@ -548,8 +548,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
/* convert raw flow to angular flow (rad/s) */
float flow_ang[2];
- flow_ang[0] = flow.flow_raw_x * params.flow_k / 1000.0f / flow_dt;
- flow_ang[1] = flow.flow_raw_y * params.flow_k / 1000.0f / flow_dt;
+ //todo check direction of x und y axis
+ 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 */
float flow_m[3];
flow_m[0] = -flow_ang[0] * flow_dist;
diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c
index 8638a4904..0b6861d2a 100644
--- a/src/modules/sdlog2/sdlog2.c
+++ b/src/modules/sdlog2/sdlog2.c
@@ -1518,11 +1518,14 @@ int sdlog2_thread_main(int argc, char *argv[])
/* --- FLOW --- */
if (copy_if_updated(ORB_ID(optical_flow), subs.flow_sub, &buf.flow)) {
log_msg.msg_type = LOG_FLOW_MSG;
- log_msg.body.log_FLOW.flow_raw_x = buf.flow.flow_raw_x;
- log_msg.body.log_FLOW.flow_raw_y = buf.flow.flow_raw_y;
- log_msg.body.log_FLOW.flow_comp_x = buf.flow.flow_comp_x_m;
- log_msg.body.log_FLOW.flow_comp_y = buf.flow.flow_comp_y_m;
- log_msg.body.log_FLOW.distance = buf.flow.ground_distance_m;
+ log_msg.body.log_FLOW.ground_distance_m = buf.flow.ground_distance_m;
+ log_msg.body.log_FLOW.gyro_temperature = buf.flow.gyro_temperature;
+ log_msg.body.log_FLOW.gyro_x_rate_integral = buf.flow.gyro_x_rate_integral;
+ log_msg.body.log_FLOW.gyro_y_rate_integral = buf.flow.gyro_y_rate_integral;
+ log_msg.body.log_FLOW.gyro_z_rate_integral = buf.flow.gyro_z_rate_integral;
+ log_msg.body.log_FLOW.integration_timespan = buf.flow.integration_timespan;
+ log_msg.body.log_FLOW.pixel_flow_x_integral = buf.flow.pixel_flow_x_integral;
+ log_msg.body.log_FLOW.pixel_flow_y_integral = buf.flow.pixel_flow_y_integral;
log_msg.body.log_FLOW.quality = buf.flow.quality;
log_msg.body.log_FLOW.sensor_id = buf.flow.sensor_id;
LOGBUFFER_WRITE_AND_COUNT(FLOW);
diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h
index fa9bdacb8..30491036a 100644
--- a/src/modules/sdlog2/sdlog2_messages.h
+++ b/src/modules/sdlog2/sdlog2_messages.h
@@ -200,13 +200,19 @@ struct log_ARSP_s {
/* --- FLOW - OPTICAL FLOW --- */
#define LOG_FLOW_MSG 15
struct log_FLOW_s {
- int16_t flow_raw_x;
- int16_t flow_raw_y;
- float flow_comp_x;
- float flow_comp_y;
- float distance;
- uint8_t quality;
+ uint64_t timestamp;
uint8_t sensor_id;
+ float pixel_flow_x_integral;
+ float pixel_flow_y_integral;
+ float gyro_x_rate_integral;
+ float gyro_y_rate_integral;
+ float gyro_z_rate_integral;
+ float ground_distance_m;
+ uint32_t integration_timespan;
+ uint32_t time_since_last_sonar_update;
+ uint16_t frame_count_since_last_readout;
+ int16_t gyro_temperature;
+ uint8_t quality;
};
/* --- GPOS - GLOBAL POSITION ESTIMATE --- */
diff --git a/src/modules/uORB/topics/optical_flow.h b/src/modules/uORB/topics/optical_flow.h
index 0196ae86b..d3dc46ee0 100644
--- a/src/modules/uORB/topics/optical_flow.h
+++ b/src/modules/uORB/topics/optical_flow.h
@@ -55,16 +55,22 @@
*/
struct optical_flow_s {
- 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 */
+ 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 */
+
+
+
};
generated by cgit v1.2.3 (git 2.39.1) at 2024-09-09 15:36:23 +0000