diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2012-08-14 11:40:08 +0200 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2012-08-14 11:40:08 +0200 |
commit | 92a1fab0fd9e95737258c30fc423e839652edb72 (patch) | |
tree | a19288747ea7a7aaf060caca1b38947d39ce9acf | |
parent | ebbdbac97b500075407bc051e38a8c602c202ac0 (diff) | |
download | px4-firmware-92a1fab0fd9e95737258c30fc423e839652edb72.tar.gz px4-firmware-92a1fab0fd9e95737258c30fc423e839652edb72.tar.bz2 px4-firmware-92a1fab0fd9e95737258c30fc423e839652edb72.zip |
Integrated optical flow
-rw-r--r-- | apps/mavlink/mavlink.c | 52 | ||||
-rw-r--r-- | apps/uORB/objects_common.cpp | 3 | ||||
-rw-r--r-- | apps/uORB/topics/optical_flow.h | 82 |
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 |