Integrated optical flow

3 files changed, 131 insertions, 6 deletions

diff --git a/apps/mavlink/mavlink.c b/apps/mavlink/mavlink.c
index 1e6156557..66b3865cc 100644
--- a/apps/mavlink/mavlink.c
+++ b/apps/mavlink/mavlink.c
@@ -72,6 +72,7 @@
 #include <uORB/topics/vehicle_local_position_setpoint.h>
 #include <uORB/topics/vehicle_global_position_setpoint.h>
 #include <uORB/topics/vehicle_attitude_setpoint.h>
+#include <uORB/topics/optical_flow.h>
 #include "waypoints.h"
 #include "mavlink_log.h"
 
@@ -126,12 +127,19 @@ static int ardrone_motors_pub = -1;
 static int cmd_pub = -1;
 static int global_pos_sub = -1;
 static int local_pos_sub = -1;
+static int flow_pub = -1;
 static int global_position_setpoint_pub = -1;
 static int local_position_setpoint_pub = -1;
 static bool mavlink_hil_enabled = false;
 
 static char mavlink_message_string[51] = {0};
 
+/* interface mode */
+static enum {
+	MAVLINK_INTERFACE_MODE_OFFBOARD,
+	MAVLINK_INTERFACE_MODE_ONBOARD
+} mavlink_link_mode = MAVLINK_INTERFACE_MODE_OFFBOARD;
+
 
 /* 3: Define waypoint helper functions */
 void mavlink_wpm_send_message(mavlink_message_t *msg);
@@ -828,6 +836,7 @@ mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg)
 void handleMessage(mavlink_message_t *msg)
 {
 	if (msg->msgid == MAVLINK_MSG_ID_COMMAND_LONG) {
+
 		mavlink_command_long_t cmd_mavlink;
 		mavlink_msg_command_long_decode(msg, &cmd_mavlink);
 
@@ -863,12 +872,39 @@ void handleMessage(mavlink_message_t *msg)
 				vcmd.source_component = msg->compid;
 				vcmd.confirmation =  cmd_mavlink.confirmation;
 
-				/* Publish */
+				/* check if topic is advertised */
+				if (cmd_pub <= 0) {
+					cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd);
+				}
+				/* publish */
 				orb_publish(ORB_ID(vehicle_command), cmd_pub, &vcmd);
 			}
 		}
 	}
 
+	if (msg->msgid == MAVLINK_MSG_ID_OPTICAL_FLOW) {
+		mavlink_optical_flow_t flow;
+		mavlink_msg_optical_flow_decode(msg, &flow);
+
+		struct optical_flow_s f;
+
+		f.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.quality = flow.quality;
+		f.sensor_id = flow.sensor_id;
+
+		/* check if topic is advertised */
+		if (flow_pub <= 0) {
+			flow_pub = orb_advertise(ORB_ID(optical_flow), &flow);
+		}
+		/* publish */
+		orb_publish(ORB_ID(optical_flow), flow_pub, &flow);
+	}
+
 	if (msg->msgid == MAVLINK_MSG_ID_SET_MODE) {
 		/* Set mode on request */
 		mavlink_set_mode_t new_mode;
@@ -909,6 +945,10 @@ void handleMessage(mavlink_message_t *msg)
 			ardrone_motors.counter++;
 			ardrone_motors.timestamp = hrt_absolute_time();
 
+			/* check if topic has to be advertised */
+			if (ardrone_motors_pub <= 0) {
+				ardrone_motors_pub = orb_advertise(ORB_ID(ardrone_motors_setpoint), &ardrone_motors);
+			}
 			/* Publish */
 			orb_publish(ORB_ID(ardrone_motors_setpoint), ardrone_motors_pub, &ardrone_motors);
 		}
@@ -1155,7 +1195,7 @@ int mavlink_main(int argc, char *argv[])
 	/* Send raw sensor values at 10 Hz / every 100 ms */
 	mavlink_message_intervals[MAVLINK_MSG_ID_RAW_IMU] = 100;
 
-	//default values for arguments
+	/* default values for arguments */
 	char *uart_name = "/dev/ttyS0";
 	int baudrate = 57600;
 	const char *commandline_usage = "\tusage: %s -d <devicename> -b <baudrate> [-e/--exit-allowed]\n\t\tdefault: -d %s -b %i\n";
@@ -1195,6 +1235,10 @@ int mavlink_main(int argc, char *argv[])
 		if (strcmp(argv[i], "-e") == 0 || strcmp(argv[i], "--exit-allowed") == 0) {
 			mavlink_link_termination_allowed = true;
 		}
+
+		if (strcmp(argv[i], "-o") == 0 || strcmp(argv[i], "--onboard") == 0) {
+			mavlink_link_mode = MAVLINK_INTERFACE_MODE_ONBOARD;
+		}
 	}
 
 	struct termios uart_config_original;
@@ -1211,10 +1255,6 @@ int mavlink_main(int argc, char *argv[])
 	/* Flush UART */
 	fflush(stdout);
 
-	/* topics to advertise */
-	ardrone_motors_pub = orb_advertise(ORB_ID(ardrone_motors_setpoint), &ardrone_motors);
-	cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd);
-
 	/* topics to subscribe globally */
 	/* subscribe to ORB for global position */
 	global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
diff --git a/apps/uORB/objects_common.cpp b/apps/uORB/objects_common.cpp
index 2b12939a3..d6b566b6a 100644
--- a/apps/uORB/objects_common.cpp
+++ b/apps/uORB/objects_common.cpp
@@ -105,6 +105,9 @@ ORB_DEFINE(vehicle_attitude_setpoint, struct vehicle_attitude_setpoint_s);
 #include "topics/manual_control_setpoint.h"
 ORB_DEFINE(manual_control_setpoint, struct manual_control_setpoint_s);
 
+#include "topics/optical_flow.h"
+ORB_DEFINE(optical_flow, struct optical_flow_s);
+
 #include "topics/actuator_controls.h"
 ORB_DEFINE(actuator_controls_0, struct actuator_controls_s);
 ORB_DEFINE(actuator_controls_1, struct actuator_controls_s);
diff --git a/apps/uORB/topics/optical_flow.h b/apps/uORB/topics/optical_flow.h
new file mode 100644
index 000000000..24f842825
--- /dev/null
+++ b/apps/uORB/topics/optical_flow.h
@@ -0,0 +1,82 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Author: @author Lorenz Meier <lm@inf.ethz.ch>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file optical_flow.h
+ * Definition of the optical flow uORB topic.
+ */
+
+#ifndef TOPIC_OPTICAL_FLOW_H_
+#define TOPIC_OPTICAL_FLOW_H_
+
+#include <stdint.h>
+#include <stdbool.h>
+#include "../uORB.h"
+
+/**
+ * @addtogroup topics
+ */
+
+/**
+ * Optical flow in NED body frame in SI units.
+ *
+ * @see http://en.wikipedia.org/wiki/International_System_of_Units
+ */
+struct optical_flow_s {
+
+	/*
+	 * Actual data, this is specific to the type of data which is stored in this struct
+	 * A line containing L0GME will be added by the Python logging code generator to the
+	 * logged dataset.
+	 */
+	uint64_t timestamp;		/**< in microseconds since system start          */
+
+	uint16_t flow_raw_x;		/**< flow in pixels in X direction, not rotation-compensated */
+	uint16_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 */
+
+};
+
+/**
+ * @}
+ */
+
+/* register this as object request broker structure */
+ORB_DECLARE(optical_flow);
+
+#endif